直线最小二乘法线性拟合-- points点集合

// 直线最小二乘法线性拟合 points为点集合 n为点的个数,错误时，斜率给返回一个错误值10000
pair<Point2D, double> CRackRecognition::lineFit(vector<Point2D> points, const double &tilt_angle_max, const double &increment, const double &size_dev_factor, const double &feature_size_hor)
{pair<Point2D, double> result;result.first.x = 0;result.first.y = 0;result.second = 10000;int n = points.size();// 不足两个点，直接返回if (n < 2){ROS_WARN("lineFit : points size < 2 ");return result;}float minX = 0, maxX = 0;float avgX = 0, avgY = 0;float Lxx = 0, Lyy = 0, Lxy = 0;// 计算x,y平均值for (int i = 0; i < n; i++){if (i == 0){minX = points[i].x;maxX = points[i].x;}if (minX > points[i].x){minX = points[i].x;}if (maxX < points[i].x){maxX = points[i].x;}avgX += points[i].x / n;avgY += points[i].y / n;}// 计算Lxx,Lyy,Lxyfor (int i = 0; i < n; i++){Lxy += (points[i].x - avgX) * (points[i].y - avgY);Lxx += (points[i].x - avgX) * (points[i].x - avgX);Lyy += (points[i].y - avgY) * (points[i].y - avgY);}float a = Lxy / Lxx;float b = avgY - a * avgX;float r = Lxy / sqrt(Lxx * Lyy); // 相关系数// 相关系数很差if (std::fabs(r) < LINE_FIT_R){ROS_WARN("lineFit: r=%f", r);return result;}// 斜率超出范围double limit_a = std::tan(PI * tilt_angle_max / 180);if (std::fabs(a) > limit_a){ROS_WARN("lineFit: a=%f,limit_a=%f", a, limit_a);return result;}// 尺寸判断double y_min = a * minX + b;double y_max = a * maxX + b;double disOfAB = getDistance(maxX, y_max, minX, y_min); // 线段长度// 计算此处的宽度误差result.first.x = (minX + maxX) / 2;result.first.y = a * result.first.x + b;double dis = result.first.distanceTo(Point2D(0, 0));double resolution = 2 * dis * sin((increment / 2));if (resolution < MIN_RESOLUTION){resolution = MIN_RESOLUTION;}double devi = resolution * size_dev_factor; // 距离分辨率*误差系数// 误差超出if (std::fabs(feature_size_hor - disOfAB) > devi){ROS_WARN("lineFit: feature_size_hor=%f,disOfAB=%f,devi=%f", feature_size_hor, disOfAB, devi);return result;}result.second = a;return result;
}

首先，这个函数的目标是使用最小二乘法对一组点进行线性拟合，然后返回拟合直线的中点坐标和斜率。如果出现错误，比如点数不够、相关系数太低、斜率超出范围或者尺寸不符合预期&#x