1 两点之间的运算
已知两点P1(x1,y1), P2(x2,y2)
1.1 两点直线的直线斜率：
double K = (y2-y1)/(x2-x1);
double B = y2-K*x2;
1.2 两点之间的距离：
double D = sqrt((pow((y2 - y1), 2)) + (pow((x2 -x1), 2)));
2 三点之间的运算
已知三点P1(x1,y1), P2(x2,y2), P3(x3,y3),P1和P2确定一条直线，斜率是K，截距是B
2.1 点P3到直线（P2→P1）的距离：
double Dl = (fabs(K*x3 + B - y3))/sqrt(1 + K*K);
2.2 三点组成一个圆弧，计算圆弧半径和原点
cv::Point2d defineCircle(cv::Point2d P1, cv::Point2d P2, cv::Point2d P3, double &radius)
{
double x1 = P1.x, x2 = P2.x, x3 = P3.x;
double y1 = P1.y, y2 = P2.y, y3 = P3.y;
double a = x1 - x2;
double b = y1 - y2;
double c = x1 - x3;
double d = y1 - y3;
double e = ((x1 * x1 - x2 * x2) + (y1 * y1 - y2 * y2)) / 2.0;
double f = ((x1 * x1 - x3 * x3) + (y1 * y1 - y3 * y3)) / 2.0;
double det = b * c - a * d;
if( fabs(det) < 1e-5)
{
radius = -1;
return cv::Point2d(0,0);
}
double x0 = -(d * e - b * f) / det;
double y0 = -(a * f - c * e) / det;
radius = hypot(x1 - x0, y1 - y0);
return cv::Point2d(x0, y0);
}
2.3 求夹角
//=====================================================
// 函数功能： 计算两个向量之间的夹角（ P1→P2， P2→P3）
//=====================================================
double Cal_Angle(cv::Point2d P1, cv::Point2d P2, cv::Point2d P3)
{
double Mod1 = sqrt(pow(P1.x - P2.x, 2) + pow(P1.y - P2.y, 2));
double Mod2 = sqrt(pow(P2.x - P3.x, 2) + pow(P2.y - P3.y, 2));
double dot = (P2.x - P1.x)*(P3.x - P2.x) + (P2.y - P1.y)*(P3.y - P2.y);
return acos(dot / (Mod1*Mod2))*180.0 / 3.14159;
}
3 四点之间的运算
已知三点P1(x1,y1), P2(x2,y2), P3(x3,y3), P4(x4,y4)
3.1 P1和P2确定一条直线，P3和P4确定一条直线，求两条直线之间的交点坐标
cv::Point2d Cal_InterPoint_TwoLine(cv::Point2d P1, cv::Point2d P2, cv::Point2d P3, cv::Point2d P4)
{
double a1, a2, a3, b1, b2, b3, t;
cv::Point2d pp1;
a1 = P4.y - P3.y;
a2 = P3.y - P1.y;
a3 = P2.y - P1.y;
b1 = P4.x - P3.x;
b2 = P3.x - P1.x;
b3 = P2.x - P1.x;
t = (a3 * b2 - a2 * b3) / (a1 * b3 - a3 * b1);
pp1.x = (P4.x - P3.x) * t + P3.x;
pp1.y = (P4.y - P3.y) * t + P3.y;
return pp1;
}
3.2 求两条直线之间的夹角
double cal_LineAngle(double K1, double B1, double K2, double B2)
{
double T = fabs(K2-K1)/(1+fabs(K2*K1)); //弧度
return atan(T) * 180 / 3.14159;
}
4 最小二乘法拟合直线方程
//========================================================= // 函数功能： 最小二乘法拟合直线方程//=========================================================void FitLine(vector<cv::Point2d> InData, double &k, double &b, double &R2)
{
int n = InData.size();
if(n < 2) { R2 = -1; return; }
double sumX = 0, sumY = 0, s_xy = 0, s_xx = 0;
for(int i = 0; i < n; i++)
{
sumX += InData.at(i).x;
sumY += InData.at(i).y;
s_xy += InData.at(i).x * InData.at(i).y;
s_xx += InData.at(i).x * InData.at(i).x;
}
double _x = sumX/n;
double _y = sumY/n;
k = (n*s_xy - sumX*sumY)/(n*s_xx - sumX*sumX);
b = (s_xx*sumY-sumX*s_xy)/(n*s_xx - sumX*sumX);
//计算R平方
double tempSumXX = 0, tempSumYY = 0, E = 0;
for(int i = 0; i < n; i++)
{
tempSumXX += (InData.at(i).x - sumX/n)*(InData.at(i).x - sumX/n);
tempSumYY += (InData.at(i).y - sumY/n)*(InData.at(i).y - sumY/n);
E += (InData.at(i).x - sumX/n)*(InData.at(i).y - sumY/n);
}
double R = E/(sqrt(tempSumXX)*sqrt(tempSumYY));
R2 = R*R;
}
5 一阶低通数字滤波器
cv::Point2d lowPassFilter(cv::Point2d currentVal, cv::Point2d prevFilteredVal)
{
double ALPHA = 0.5;
cv::Point2d filteredVal;
filteredVal.x = ALPHA * currentVal.x + (1 - ALPHA) * prevFilteredVal.x;
filteredVal.y = ALPHA * currentVal.y + (1 - ALPHA) * prevFilteredVal.y;
return filteredVal;
}
