算法流程
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> 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 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 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());
}