割草机建图及全覆盖

割草机建图及全覆盖

背景

通过鼠标的移动模拟机器人的移动 捕获机器人的坐标作为割草机地图的边界坐标。得到的地图作为机器人的工作区。

效果图

建图(实现将给定的路径转化成图像IMG)

实现全覆盖路径算法的流程

在地图内进行相关的全覆盖

关于覆盖时关于障碍物的处理

割草机处理全覆盖时遇到的障碍物处理方式

 可以考虑先加入障碍物一边全部的端点再另一边全部的端点，以提高全覆盖时运动的效率

关于主方向的确定

流程:

	1.将二维坐标组成一个矩阵
	2.计算其协方差矩阵(格拉姆矩阵)
	3.计算协方差矩阵得到的特征值及对应的特征向量
	4.最大的特征值对应的特征向量即为轮廓的主方向

代码实现

        /**
         * @brief 通过计算特征值 特征向量得到角度
         *
         * @param contour
         * @return float
         */
        float getMainDirEigenvalues(const std::vector<cv::Point> &contour)
        {
            if (contour.size() < 5)
            {
                LOG(ERROR) << "contour size to short";
                return 0.0;
            }
            cv::Mat image = getCoverImg().clone(); //
            // 将轮廓点转换为矩阵，并按行排列
            cv::Mat contourMat(contour.size(), 2, CV_64FC1);
            for (int i = 0; i < contour.size(); ++i)
            {
                contourMat.at<double>(i, 0) = contour[i].x;
                contourMat.at<double>(i, 1) = contour[i].y;
            }
            std::cout << "contourMat.rows() = " << contourMat.rows << " contourMat.clos " << contourMat.cols << std::endl;

            // 计算轮廓的协方差矩阵
            cv::Mat mean, covar;
            cv::calcCovarMatrix(contourMat, covar, mean, cv::COVAR_NORMAL | cv::COVAR_ROWS | cv::COVAR_SCALE);

            // 计算协方差矩阵的特征值和特征向量
            cv::Mat eigenvalues, eigenvectors;
            cv::eigen(covar, eigenvalues, eigenvectors);
            std::cout << " eigenvalues = " << eigenvalues << std::endl;
            // 寻找最大特征值对应的最大特征向量

            cv::Point2f mainDirection(eigenvectors.at<double>(0, 0), eigenvectors.at<double>(0, 1));

            cv::Mat minEigenvector = eigenvectors.row(0); // eigenvectors.cols - 1

            std::cout << "eigenvectors.rows() = " << eigenvectors.rows << " eigenvectors.clos " << eigenvectors.cols << std::endl;
            for (int i = 0; i < eigenvectors.rows; ++i)
            {
                cv::Mat eigenvector = eigenvectors.row(i);
                // std::cout << "cv::Mat eigenvector =  " << eigenvector << std::endl;
                double angle = calculateAVecngle(eigenvector);
                std::cout << "主成分" << i + 1 << " 的角度：" << angle << " 度" << std::endl;
            }

            double angle = calculateAngle(minEigenvector);
            // if (angle > 0)
            //     angle = angle - 180;
            std::cout << "原始角度angle = " << angle << std::endl;
            angle = convertAngle(angle); //
            // angle = minEigenvector.at<double>(1) / minEigenvector.at<double>(0);

            main_angle_ = angle;
            // main_angle_ = minEigenvector.at<double>(1) / minEigenvector.at<double>(0);
            std::cout << "角度：" << angle << " main_angle_ = " << main_angle_ << std::endl;
            cv::Point center = cv::Point(mean.at<double>(0, 0), mean.at<double>(0, 1));
            cntr_center_ = center;
            cv::Point endpoint = center + cv::Point(static_cast<int>(1000 * mainDirection.x), static_cast<int>(1000 * mainDirection.y));
            cv::line(image, center, endpoint, cv::Scalar(10, 88, 30), 1);
            
            float angle_test = cv::fastAtan2(endpoint.y - center.y, endpoint.x - center.x);
            
            angle_test = common::NormalizeAngleDifference(common::DegToRad(angle_test));
            // 在图像上绘制角度文本
            // std::string angleText = std::to_string(int(angle));
            // cv::putText(image, angleText, cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(55, 66, 123), 2);

            // 显示图像
            cv::imshow("轮廓主方向", image);
            //LOG(INFO) << "***************angle = " << angle << " angle_test = " << common::RadToDeg(angle_test);
            // cv::waitKey(0);
            return angle;
        }