PCL：激光點云車道線檢測及最小二乘法擬合

• 數學部分
• 代碼實現

目前已經將車道線的最小二乘擬合及配合rviz可視化實現了。

數學部分

補充：擬合的數學模型是直線一般式

當x1≠x2，y1≠y2時，直線的斜率k=(y2-y1)/(x2-x1) 故直線方程為y-y1=(y2-y1)/(x2-x1)×(x-x1) 即x2y-x1y-x2y1+x1y1=(y2-y1)x-x1(y2-y1) 即(y2-y1)x-(x2-x1)y-x1(y2-y1)+(x2-x1)y1=0 即(y2-y1)x+(x1-x2)y+x2y1-x1y2=0 ① 可以發現，當x1=x2或y1=y2時，①式仍然成立。所以直線AX+BY+C=0的一般式方程就是： A = Y2 - Y1 B = X1 - X2 C = X2*Y1 - X1*Y2

對于一元二次多項式，可以轉換為線性方程組求解，我們一般寫成矩陣形式 Ax = y。

Ax = y非一致方程和一致方程的求解
一致與非一致方程

一致方程是指Ax = y有至少一個解非一致方程指Ax = y沒有解

Ax = y求解

如果A是滿秩的方陣，則x = inv(A）*y如果A不是方陣，但是是行滿秩或者列滿秩，那么解為A的偽逆乘以y如果A是秩虧的，那么A的解為A的廣義逆乘以y

實際上廣義逆包括逆、偽逆，廣義逆又稱為：Moore-Penrose逆矩陣，所以Ax = y的解可以統一為A 的Moore-Penrose逆矩陣乘以y，特別的是，對于一致性方程，該解為最小范數解，對于非一致方程，該解為最小范數最小二乘解

Moore-Penrose逆矩陣

代碼實現

// 擬合求系數 void line_fitting(pcl::PointCloud<pcl::PointXYZ>::Ptr &in_cloud, double &a, double&b, double&c) {vector<double>x, y;int num = in_cloud->size();for (int i = 0; i < num; i++){pcl::PointXYZ p = in_cloud->at(i);x.push_back(p.x);y.push_back(p.y);}MatrixXd A(3, 3), B(3, 1), Y(3, 1);double A01(0), A02(0), A12(0), A22(0), B00(0), B10(0), B12(0);for (int i=0; i<num; i++){A01 += x[i];A02 += x[i] * x[i];A12 += x[i] * x[i] * x[i];A22 += x[i] * x[i] * x[i] * x[i];B00 += y[i];B10 += x[i] * y[i];B12 += x[i] * x[i] * y[i];}A << num, A01, A02;A01, A02, A12;A02, A12, A22;B << B00,B10,B12;Y = A.inverse() * B;a = Y(2, 0);b = Y(1, 0);c = Y(0, 0); } // rviz void line_fitting_show(pcl::PointCloud<pcl::PointXYZ>::Ptr &in_cloud, visualization_msgs::Marker &line, double a0, double a1, double a2; {pcl::PointXYZ min, max;line.lifetime = ros::Buration(0.3);line.type = visualization_msgs::Marker::LINE_STRIP;line.action = visualization_msgs::Marker::ADD;line.ns = "lane";line.id = 0;line.scale.x = 0.2;line.color.g = 1;line.color.a = 1;line.pose.orientation.w = 1;pcl::getMinMax3D(*in_cloud, min, max);float x_min = min.x;float x_max = max.x;vector<double> xx, yy;double step = 0.5;int step_num = std::ceil((x_max - x_min) /step);for （int i = 0; i < step_num + 2; i++){double tmp_value = x_min + i * step;if (tmp_value > x_max){tmp_value = x_max;}xx.push_back(tmp_value);geometry_msgs::Point p;p.x = tmp_value;p.y = a0 + a1*xx[i] + a2*xx[i]*xx[i];p.z = min.z;line.points.push_back(p);} }

總結

以上是生活随笔為你收集整理的PCL：激光点云车道线检测及最小二乘法拟合的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。