数学部分

补充：拟合的数学模型是直线一般式

当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：激光点云车道线检测及最小二乘法拟合相关推荐

车道线检测综述及新工作汇总
编辑丨3D视觉工坊车道线检测综述及近期新工作--为ADAS.自动驾驶服务. 部分视频效果展示: https://www.bilibili.com/video/BV1E54y1V7G4 https:/ ...
一文详解车道线检测技术分析
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达针对车道线检测的任务,我们需要弄清楚几个问题: 1.车道线的表示形式? 输出类型:掩码/点集/矢量线条 ...
车道线检测综述及近期新工作
车道线检测综述及近期新工作--为ADAS.自动驾驶服务. 部分视频效果展示: https://www.bilibili.com/video/BV1E54y1V7G4 https://www.bilib ...
面向量产的3D目标与车道线检测方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 Part 1 背景介绍 1. 1 团队介绍商汤科技自动驾驶团队依托公司为背景,以 SenseParr ...
学习笔记16--环境感知实例之车道线检测
本系列博客包括6个专栏,分别为:<自动驾驶技术概览>.<自动驾驶汽车平台技术基础>.<自动驾驶汽车定位技术>.<自动驾驶汽车环境感知>.<自动驾驶 ...
无人驾驶汽车系统入门（六）——基于传统计算机视觉的车道线检测(1)
无人驾驶汽车系统入门(六)--基于传统计算机视觉的车道线检测(1) 感知,作为无人驾驶汽车系统中的"眼睛",是目前无人驾驶汽车量产和商用化的最大障碍之一(技术角度), 目前,高等级 ...
2D车道线检测、车位检测、BEV感知工作总结
作者 | MrTian 编辑 | 汽车人原文链接:https://zhuanlan.zhihu.com/p/601731464 点击下方卡片,关注"自动驾驶之心"公众号 ADA ...
VIL-100: 一个新的车道线检测数据集和基线模型（ICCV2021）
作者丨StrongerTang@知乎来源丨https://zhuanlan.zhihu.com/p/411156533 编辑丨3D视觉工坊之前写过一篇车道线检测的综述,得到了一些朋友的认可,也因此 ...
实战教程 | 车道线检测项目实战，霍夫变换新方法 Spatial CNN
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达此文按照这样的逻辑进行撰写.分享机器学习.计算机视觉的基础知识,接着我们以一个实际的项目,带领大家自己 ...

PCL：激光点云车道线检测及最小二乘法拟合

PCL：激光点云车道线检测及最小二乘法拟合

数学部分

代码实现

PCL：激光点云车道线检测及最小二乘法拟合相关推荐

最新文章

热门文章