效果

代码

附上初始写的一个类

class LineDetect_firstway {public:cv::Mat img;geometry_msgs::Twist move;  //include Linear velocity and angular velocity  void imageCallback(const sensor_msgs::ImageConstPtr& msg){cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);img = cv_ptr -> image;GaussianBlur(img, img_gauss, Size(3, 3), 0, 0, BORDER_DEFAULT);cvtColor(img_gauss, img_gray, COLOR_BGR2GRAY);Canny(img_gray, dst2, 50, 200, 3);cv::namedWindow("Canny", cv::WINDOW_AUTOSIZE);cv::imshow("Canny", dst2);waitKey(10);cv::cvtColor(dst2, cdst2, CV_GRAY2BGR);vector< Vec2f > lines;cv::HoughLines(dst2, lines, 1, CV_PI/180, 100, 0, 0 );x_interset_center=0.0f;count_valid_point = 0;vector valid_line_2points;for( size_t i = 0; i < lines.size(); i++ ){float rho = lines[i][0], theta = lines[i][1];cout<< "rho="<0 && rho/cos(theta)<800){x_interset_center = cvRound(x_interset_center + rho/cos(theta));count_valid_point = count_valid_point+1;cv::Point p1(cvRound(rho/cos(theta)), 0);cv::Point p2(cvRound(rho*cos(theta)), rho*sin(theta));valid_line_2points.push_back(p1);valid_line_2points.push_back(p2);} }cout<<"the number of valid points："<< count_valid_point << endl;for(int i=0;i++;i