// 法二: 向量方向性, 满足(BA*Bp)(DC*Dp)>=0,先叉乘,利用叉乘正负判断方向,矩形对面所夹着的点方向相同Vector2d BA(A.x -B.x, A.y - B.y);Vector2d Bp(p.x -B.x, p.y - B.y);Vector2d DC(C.x -D.x, C.y - D.y);Vector2d Dp(p.x -D.x, p.y - D.y);/* 若严格按照ABCD顺序来,两个数都是大于0,乘积的方式是顾忌到矩形4个顶点的ABCD顺时针排序还是逆时针排序 */auto val1 = (BA.x * Bp.y - Bp.x * BA.y) * (DC.x * Dp.y - Dp.x * DC.y);Vector2d DA(D.x -A.x, D.y - A.y);Vector2d BC(B.x -C.x, B.y - C.y);auto val2 = (DA.x * Dp.y - Dp.x * DA.y)* (BC.x * Bp.y - Bp.x * BC.y);if(val1 >=0 && val2 >=0) return true;else return false;
效果如下图,方便调试,查看结果。
// 绘制箭头void DrawArrow(cv::Mat &img, cv::Point pStart, cv::Point pEnd, int len, int alpha, cv::Scalar &color, int thickness, int lineType) {cv::Point arrow;double angle =atan2((double)(pStart.y - pEnd.y), (double)(pStart.x - pEnd.x));cv::line(img, pStart, pEnd, color, thickness, lineType);arrow.x = pEnd.x + len * cos(angle + PI * alpha / 180);arrow.y = pEnd.y + len * sin(angle + PI * alpha / 180);cv::line(img, pEnd, arrow, color, thickness, lineType);arrow.x = pEnd.x + len * cos(angle - PI * alpha / 180);arrow.y = pEnd.y + len * sin(angle - PI * alpha / 180);cv::line(img, pEnd, arrow, color, thickness, lineType);}// Debug: 绘制旋转矩形和待测试点void DrawRotateRectangle(const pcl::PointCloud& cloud, const Point& p){int img_x = 200, img_y = 200;cv::Scalar arrow_color = cv::Scalar(0, 0, 0);cv::Mat img(img_y, img_x, CV_8UC3, cv::Scalar(255, 255, 255));DrawArrow(img, cv::Point(img_x/2.0, img_y -20),cv::Point(img_x/2.0, 20), 6, 45, arrow_color, 2, 8);DrawArrow(img, cv::Point(20, img_y/2.0),cv::Point(img_x-20, img_y/2.0), 6, 45, arrow_color, 2, 8);for(int i=0; i<4; ++i){if(i < 3){cv::line(img, cv::Point(cloud.points[i].x+100, -cloud.points[i].y+100), cv::Point(cloud.points[i+1].x+100, -cloud.points[i+1].y+100), arrow_color, 2, 8);}else{cv::line(img, cv::Point(cloud.points[i].x+100, -cloud.points[i].y+100), cv::Point(cloud.points[0].x+100, -cloud.points[0].y+100), arrow_color, 2, 8);}}cv::circle(img, cv::Point(p.x+100, -p.y+100), 2, arrow_color, 2, 8);cv::imshow("rotate_rectangle", img);cv::waitKey(0);}
// 还原4个角点坐标pcl::PointCloud cloud;cloud.points.emplace_back(pcl::PointXYZ(-w/2.0, h/2.0, 0));cloud.points.emplace_back(pcl::PointXYZ(-w/2.0, -h/2.0, 0));cloud.points.emplace_back(pcl::PointXYZ(w/2.0, -h/2.0, 0));cloud.points.emplace_back(pcl::PointXYZ(w/2.0, h/2.0, 0));// DrawRotateRectangle(cloud, p);Eigen::Matrix4d mat = Eigen::Matrix4d::Identity();// 旋转矩阵形式mat << cos(theta*PI/ 180.0), -sin(theta*PI/ 180.0), 0, center.x, sin(theta*PI/ 180.0), cos(theta*PI/ 180.0),0, center.y, 0,0, 1, 0, 0,0, 0, 1 ;std::cout << "mat: " << mat << std::endl;pcl::transformPointCloud(cloud, cloud, mat);