[OpenCV] aruco Markers识别 小车巡线

小车巡线代码

#include<ros/ros.h>
#include<sensor_msgs/Image.h>
#include<geometry_msgs/Twist.h>
#include<cv_bridge/cv_bridge.h>
#include<opencv2/opencv.hpp>
#include<opencv2/imgproc.hpp>
#include<opencv2/imgproc/types_c.h>
#include<opencv2/core/core.hpp>

double twist_linear_x , twist_angular_z;
sensor_msgs::Image hsv_image ;

void image_Callback(const sensor_msgs::Image& msg);

int main(int argc, char **argv){
    ros::init(argc, argv, "follower_line");
    ros::NodeHandle nh;

    ros::Subscriber img_sub = nh.subscribe("/sensor_msgs/image_gray", 10, image_Callback);
    ros::Publisher cmd_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    ros::Publisher img_pub = nh.advertise<sensor_msgs::Image>("/image_hsv",10);

    while(ros::ok()){
        geometry_msgs::Twist twist;
        twist.linear.x = twist_linear_x;
        twist.angular.z = twist_angular_z;
        cmd_pub.publish(twist);
        img_pub.publish(hsv_image);
        ros::spinOnce();
    }
    return 0;
}

void image_Callback(const sensor_msgs::Image& msg){
    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
    cv::Mat image = cv_ptr -> image;
    cv::Mat hsv = image.clone();
    cv::Mat res = image.clone();
    cv::cvtColor(image, hsv, cv::COLOR_BGR2HSV);
    cv::inRange(hsv, cv::Scalar(0, 0, 0), cv::Scalar(180, 255, 46), res);
    
    int h = image.rows;
    int w = image.cols;
    int search_top = 3 * h / 4, search_bot = search_top + 20;
    for(int i = 0; i < search_top; i ++){
        for(int j = 0; j < w; j ++){
            res.at<uchar>(i,j) = 0;
        }
    }
    for(int i = search_bot; i < h; i++){
        for(int j = 0; j < w; j ++){
            res.at<uchar>(i,j) = 0;
        }
    }
    cv::Moments M = cv::moments(res);

    if(M.m00 > 0){
        int cx = int (cvRound(M.m10 / M.m00));
        int cy = int (cvRound(M.m01 / M.m00));
        ROS_INFO("cx: %d cy: %d", cx, cy);
        cv::circle(image, cv::Point(cx, cy), 10, (0, 0, 255));
        int v = cx - w / 2;
        twist_linear_x = 0.1;
        twist_angular_z = -float(v) / 300 * 0.4;
        //cmd_pub.publish(twist);
    }
    else{
        ROS_INFO("not found line!");
        twist_linear_x = 0;
        twist_angular_z = 0;
        //cmd_pub.publish(twist);
    }
    sensor_msgs::ImagePtr hsv_image_ = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
    hsv_image = *hsv_image_;
}
 

 姿态估计（Pose estimation）在计算机视觉领域扮演着十分重要的角色：机器人导航、增强现实以及其它。这一过程的基础是找到现实世界和图像投影之间的对应点。这通常是很困难的一步，因此我们常常用自己制作的或基本的Marker来让这一切变得更容易。

        最