// // Created by leoxae on 19-8-1. // #include "PlaneGeometry.h" //----------------------基础相关函数------------------------- /** * [1]将直线的斜率转化为角度 * @param k 斜率 * @return angle 角度 */ float PlaneGeometry::ktoAngle(float k) { float angle_H = atan(k); float angle = angle_H * 180 / CV_PI; return angle; } //----------------------点相关函数------------------------- /** *[2]计算卡片中心点坐标 * @param cacheRect * @return */ Point2f PlaneGeometry::calcPoint(Rect matchrect) { // 分别取左上角tl 及 右下角br 的坐标 int x1 = matchrect.tl().x; int y1 = matchrect.tl().y; int x2 = matchrect.br().x; int y2 = matchrect.br().y; Point2f center; center.x = (x1 + (x2 - x1) / 2); center.y = (y1 + (y2 - y1) / 2); return center; } /** * [3]旋转坐标变换 * 说明:Node1绕Node2旋转angle角度后的新坐标 * @param node1 卡片中心点 * @param node2 卡片中心点 * @param angle * @return new_point 旋转完之后的坐标 */ cv::Point2f PlaneGeometry::NodeRotate(Point2f node1, double angle, int w, int h) { Point2f Node; //平面直角坐标系和笛卡尔坐标系的转换 angle = angle * g_pi / 180; //angle为角度需要转换运算,大于0时为顺时针,小于零时为逆时针 //round()四舍五入并保留两位小数 Node.x = node1.x * cos(angle) - node1.y * sin(angle) - 0.5 * w * cos(angle) + 0.5 * h * sin(angle) + 0.5 * w; Node.y = node1.y * cos(angle) + node1.x * sin(angle) - 0.5 * w * sin(angle) - 0.5 * h * cos(angle) + 0.5 * h; return Node; } //----------------------点线相关函数------------------------- /** * [4]斜率计算 * 说明:计算根据两点坐标确定的直线斜率 * @param node1 卡片中心点 * @param node2 卡片中心点 * @return k 斜率 */ double PlaneGeometry::NodeLineGradient(Point2f node1, Point2f node2) { double x = node2.x - node1.x; double y = node2.y - node1.y; //分母不能为零 if (x == 0) { x = 0.0000000000000001; } double k = y / x; return k; } /** * [5]点到点的直线距离 * 说明:两点间的直线距离 * @param node1 卡片中心点 * @param node2 卡片中心点 * @return distance 两点间的距离 */ double PlaneGeometry::NodeDistance(Point2f node1, Point2f node2) { double x2 = powf((node2.x - node1.x), 2); double y2 = powf((node2.y - node1.y), 2); double distance = sqrtf(x2 + y2); return distance; } /** * [6]点到直线的距离 * 说明:通过数学两点式计算 * @param node * @param line * @return dis 距离 pedal 垂足 */ tuple<double, Point2f> PlaneGeometry::NodeLineDistance(Point2f node, Vec4f line) { Vector2d Vlongi(line[2] - line[0], line[3] - line[1]); Vector2d Vtrans(line[2] - node.x, line[3] - node.y); double temp = Vtrans.dot(Vlongi) / Vlongi.dot(Vlongi); Vector2d Vtemp = Vlongi * temp; Vector2d Vdistance = Vtrans - Vtemp; double distance = Vdistance.norm(); //返回 距离,垂足 Point2f pedal; pedal.x = line[2] - Vtemp[0]; pedal.y = line[3] - Vtemp[1]; return tuple<double, Point2f>(distance, pedal); } //----------------------线相关函数------------------------- /** * [7]计算斜率 * 说明:通过直线line计算斜率 * @param line * @return k 斜率 */ double PlaneGeometry::LineGradient(Vec4f line) { double x = line[2] - line[0]; double y = line[3] - line[1]; if (x == 0) { x = 0.0000000000000001; } double k = y / x; return k; } /** * [11]计算夹角 * 说明:计算两条直线之间的夹角 * @param line1 * @param line2 * @return crossAngle 夹角 */ double PlaneGeometry::LineCrossAngle(Vec4f line1, Vec4f line2) { Vector2d arr_0(line1[2] - line1[0], line1[3] - line1[1]); Vector2d arr_1(line2[2] - line2[0], line2[3] - line2[1]); double cos_value = arr_0.dot(arr_1) / (arr_0.norm() * arr_1.norm()); return acos(cos_value) * (180 / g_pi); } /** * [12]直线内向量叉乘 * 说明:用容器分别存放直线内叉乘计算的集合 * @param line * @return linelist 叉乘计算集合 */ vector<float> PlaneGeometry::LineCrossProduct(Vec4f line) { vector<float> linelist = {line[1] - line[3], line[2] - line[0], line[0] * line[3] - line[2] * line[1]}; return linelist; } /** * [13]两条直线相交输出交点 * @param line1 * @param line2 * @return */ tuple<int, Point2f> PlaneGeometry::LineIntersectionPoint(Vec4f line1, Vec4f line2) { //线性代数 行列式求解法 vector<float> Line1List = LineCrossProduct(line1); vector<float> Line2List = LineCrossProduct(line2); double d = Line1List[0] * Line2List[1] - Line2List[0] * Line1List[1]; //d为0的情况下 说明两条直线平行 if (d == 0) { d = 0.0000000000000001; } Point2f crossPoint; crossPoint.x = round(((Line1List[1] * Line2List[2] - Line2List[1] * Line1List[2]) * 1.0 / d) * 100) / 100.0; crossPoint.y = round(((Line1List[2] * Line2List[0] - Line2List[2] * Line1List[0]) * 1.0 / d) * 100) / 100.0; //tag为交点是否在线段上(不含端点) int tag; if (((((crossPoint.x - line1[0]) * (crossPoint.y - line1[2]) <= 0) && (crossPoint.x - line2[2]) * (crossPoint.x - line2[2]) <= 0) && (crossPoint.y - line1[1]) * (crossPoint.y - line1[3]) <= 0) && (crossPoint.y - line2[1]) * (crossPoint.y - line2[3]) <= 0) tag = 1; else tag = 0; return tuple<int, Point2f>(tag, crossPoint); } /** * 数据精度计算函数 * @param src 待求精度数 * @param bit 精度值(表示保留小数点后几位) * @return f 精度求取结果 */ float PlaneGeometry::roundbit(float src, int bits) { float f; stringstream ss; ss << fixed << setprecision(bits) << src; ss >> f; return f; } /** * 计算向量之间的角度余弦 * 说明:从pt0->pt1和从pt0->pt2 * @param pt1 * @param pt2 * @param pt0 * @return */ double PlaneGeometry::calcVecAngle( Point pt1, Point pt2, Point pt0 ) { double dx1 = pt1.x - pt0.x; double dy1 = pt1.y - pt0.y; double dx2 = pt2.x - pt0.x; double dy2 = pt2.y - pt0.y; return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10); //cos<a,b>=(ab的内积)/(|a||b|) }
// // Created by leoxae on 19-8-1. // #ifndef KEEKOAIROBOT_PLANEGEOMETRY_H #define KEEKOAIROBOT_PLANEGEOMETRY_H #include "../../globals.h" #include <cmath> #include <numeric> #include "../../utils/TempHelper.h" #include "../../utils/RecHelper.h" #include <Eigen/Dense> #include <math.h> #include <iostream> #include <vector> #include <opencv2/core/eigen.hpp> #include <opencv2/opencv.hpp> using namespace std; using namespace cv; using namespace Eigen; class PlaneGeometry { public: //----------------------基础相关函数------------------------- /** * [13]将直线的斜率转化为角度 * @param k 斜率 * @return angle 角度 */ static float ktoAngle(float k); //----------------------点相关函数------------------------- /** * [1]计算卡片中心点坐标 * @param cacheRect * @return */ static Point2f calcPoint(Rect cacheRect); /** * [2]旋转坐标变换 * 说明:Node1绕Node2旋转angle角度后的新坐标 * @param node1 卡片中心点 * @param node2 卡片中心点 * @param angle * @return new_point 旋转完之后的坐标 */ static Point2f NodeRotate(Point2f node1, double angle,int w,int h); //----------------------点线相关函数------------------------- /** * [3]斜率计算 * 说明:计算根据两点坐标确定的直线斜率 * @param node1 卡片中心点 * @param node2 卡片中心点 * @return k 斜率 */ static double NodeLineGradient(Point2f node1, Point2f node2); /** * [5]点到点的直线距离 * 说明:两点间的直线距离 * @param node1 卡片中心点 * @param node2 卡片中心点 * @return distance 两点间的距离 */ static double NodeDistance(Point2f node1, Point2f node2); /** * [6]点到直线的距离 * 说明:通过数学两点式计算 * @param node * @param line * @return dis 距离 */ static tuple<double, Point2f> NodeLineDistance(Point2f node, Vec4f line); //----------------------线相关函数------------------------- /** * [4]计算斜率 * 说明:通过直线line计算斜率 * @param line * @return k 斜率 */ static double LineGradient(Vec4f line); /** * [10]计算夹角 * 说明:计算两条直线之间的夹角 * @param line1 * @param line2 * @return crossAngle 夹角 */ static double LineCrossAngle(Vec4f line1, Vec4f line2); /** * [11]直线内向量叉乘 * 说明:用容器分别存放直线内叉乘计算的集合 * @param line * @return linelist 叉乘计算集合 */ static vector<float> LineCrossProduct(Vec4f line); /** * [12]两条直线相交输出交点 * 说明:通过线性代数的行列式求解法 * @param line1 * @param line2 * @return */ static tuple<int, Point2f> LineIntersectionPoint(Vec4f line1, Vec4f line2); /** * 数据精度计算函数 * @param src 待求精度数 * @param bit 精度值(表示保留小数点后几位) * @return f 精度求取结果 */ float roundbit(float src,int bits) ; /** * 计算向量之间的角度余弦 * 说明:从pt0->pt1和从pt0->pt2 * @param pt1 * @param pt2 * @param pt0 * @return */ static double calcVecAngle(Point pt1, Point pt2, Point pt0); }; #endif //KEEKOAIROBOT_PLANEGEOMETRY_H