移动机器人全覆盖路径规划及仿真（二.选择合适的遍历方向）

移动机器人全覆盖路径规划及仿真（二.选择合适的遍历方向）

算法流程
1.地图预处理；
2.识别地图中的直线；
3.选取最长或最短直线计算斜率；
4.将地图沿该斜率方向旋转。

float cal_rat_angle(cv::Mat map)
{
    // Parameter configuration
    double meters_per_pix = 0.05;
    double robot_size_in_meters = 0.48;
    int robot_radius = ComputeRobotRadius(meters_per_pix, robot_size_in_meters);
    //find contours
    std::vector<std::vector<cv::Point>> obstacle_contours;
    std::vector<std::vector<cv::Point>> wall_contours;
    ExtractContours(map, wall_contours, obstacle_contours, 4);
    Polygon wall = ConstructWall(map, wall_contours.front());
    PolygonList obstacles = ConstructObstacles(map, obstacle_contours);

    cv::Mat3b src = cv::Mat3b(map.size());
    src.setTo(cv::Scalar(0, 0, 0));
    cv::fillPoly(src, wall_contours, cv::Scalar(255, 255, 255));
    cv::fillPoly(src, obstacle_contours, cv::Scalar(0, 0, 0));
    std::cout << "Get map2" << std::endl;
    //cv::imshow("aa",src);
    //cv::waitKey(0);

    cv::Mat edges, dst;
    //cv::imshow("aa",src);
    //cv::waitKey(0);
    //cv::cvtColor(src,dst,CV_BGR2GRAY);
    cv::Canny(src, edges, 50, 150, 3);
    //cv::imshow("Canny",edges);
    //cv::waitKey(0);
    std::vector<Vec4f> lines;
    //std::vector<std::vector<float>> lines;
    cv::HoughLinesP(edges, lines, 1, CV_PI/180, 100, 50, 10);
    //cv::cvtColor(src,dst,CV_GRAY2BGR);

    for (auto line : lines)
    {
        float x8, y8, x9, y9;
        x8 = line[0];
        y8 = line[1];
        x9 = line[2];
        y9 = line[3];
        cv::line(src,cv::Point(x8, y8), cv::Point(x9, y9), (0, 0, 255), 1);
        //line(src,Point(1,1),Point(250,250),Scalar(0,0,255),5,CV_AA);

        cv::imshow("line_detect_possible_demo", src);
        //cv::waitKey(0);
    }

    float mink;
    std::vector<float> linek;
    for(auto line : lines)
    {
        float x0, y0, x1, y1;
        x0 = line[0];
        y0 = line[1];
        x1 = line[2];
        y1 = line[3];
        //x0, y0, x1, y1 = line;
        linek.push_back((y1 - y0)/(x1 - x0));
    }
    std::vector<float> verticalk;
    for(float i : linek)
    {
        //std::vector<float> temp = i * linek;
        std::vector<float> temp;
        for(float n : linek)
        {
            temp.push_back(i * n);
        }
        for(float j : temp)
        {
            if (0.9 < abs(j) && abs(j) < 1.1)
                verticalk.push_back(i);
        }
    }
    /**if(verticalk.size() != 0)
    {
        auto iter = std::min_element(std::begin(verticalk),std::end(verticalk));
        mink = *iter;
    }

    else
    {
        std::vector<float> line_length;
        for(auto line : lines)
        {
            float x2, y2, x3, y3;
            x2 = line[0];
            y2 = line[1];
            x3 = line[2];
            y3 = line[3];
            //x2, y2, x3, y3 = line;
            float length = (y3 - y2)*(y3 - y2) + (x3 - x2)*(x3 - x2);
            line_length.push_back(length);
        }
        auto iter1 = std::max_element(line_length.begin(),line_length.end());
        //float longest = *iter1;
        int index1 = std::distance(std::begin(line_length), iter1);
        float x4, y4, x5, y5;
        x4 = lines[index1][0];
        y4 = lines[index1][1];
        x5 = lines[index1][2];
        y5 = lines[index1][3];
        //x4, y4, x5, y5 = temp1;
        mink = (y5 - y4)/(x5 - x4);
    }**/

    std::vector<float> line_length;
    for(auto line : lines)
    {
        float x2, y2, x3, y3;
        x2 = line[0];
        y2 = line[1];
        x3 = line[2];
        y3 = line[3];
        //x2, y2, x3, y3 = line;
        float length = (y3 - y2)*(y3 - y2) + (x3 - x2)*(x3 - x2);
        line_length.push_back(length);
    }
    //auto iter1 = std::max_element(line_length.begin(),line_length.end());
    auto iter1 = std::max_element(line_length.begin(),line_length.end());
    //float longest = *iter1;
    int index1 = std::distance(std::begin(line_length), iter1);
    float x4, y4, x5, y5;
    x4 = lines[index1][0];
    y4 = lines[index1][1];
    x5 = lines[index1][2];
    y5 = lines[index1][3];
    //x4, y4, x5, y5 = temp1;
    mink = (y5 - y4)/(x5 - x4);
    return mink;
}

void rotate_arbitrarily_angle(cv::Mat &src,cv::Mat &dst,float angle)
{


    int maxBorder =(int) (cv::max(src.cols, src.rows)* 1.414 ); //即为sqrt(2)*max
    int dx = (maxBorder - src.cols)/2;
    int dy = (maxBorder - src.rows)/2;
    copyMakeBorder(src, dst, dy, dy, dx, dx, cv::BORDER_CONSTANT);
    cv::Point2f center( (float)(dst.cols/2) , (float) (dst.rows/2));
    cv::Mat affine_matrix = getRotationMatrix2D( center, angle, 1.0 );//求得旋转矩阵
    warpAffine(dst, dst, affine_matrix, dst.size());
}

平行最长边的遍历方向。

垂直最长边的方向。

地图预处理后识别的所有边。