激光雷达定位与建图-拟合问题

本篇文章介绍如何在点云中提取线段和平面。

一、平面拟合

1. 问题提出

给定一组由N个点组成的点云 $=\left \{x_{1}, \cdots , x_{n} \right \}$ ,其中每个点取三维欧式坐标 $x_{k}$ ，寻找一组平面参数n，d，使得：
$n^{T}x_{k} + d = 0$
其中：n为法向量，d为截距。
在给出的一组点云数据，满足上述平面方程，且误差最小，于是可直接用最小二乘求解。
误差最小化：

在这里插入图片描述
使用齐次坐标，将上试简化：

其中： $\tilde{x} = \begin{bmatrix} x^T & 1 \end{bmatrix}$ , $\tilde{n} = \begin{bmatrix} n^T & d \end{bmatrix}^{T}$ 。
将 $\tilde{x}_{k}$ 看做矩阵A，进行SVD分解找到最小奇异值对应的右奇异向量即是方程的解。

2. 实现

/*** @brief 测试平面拟合功能* * 该函数生成带有噪声的平面点，然后使用FitPlane函数进行平面拟合，* 最后比较估计的平面参数与真实参数。*/
void PlaneFittingTest() {// 定义真实平面参数Vec4d true_plane_coeffs(0.1, 0.2, 0.3, 0.4);true_plane_coeffs.normalize();std::vector<Vec3d> points;// 随机生成仿真平面点cv::RNG rng;for (int i = 0; i < FLAGS_num_tested_points_plane; ++i) {// 生成随机点并投影到平面上Vec3d p(rng.uniform(0.0, 1.0), rng.uniform(0.0, 1.0), rng.uniform(0.0, 1.0));double n4 = -p.dot(true_plane_coeffs.head<3>()) / true_plane_coeffs[3];p = p / (n4 + std::numeric_limits<double>::min());  // 防止除零// 添加高斯噪声p += Vec3d(rng.gaussian(FLAGS_noise_sigma), rng.gaussian(FLAGS_noise_sigma), rng.gaussian(FLAGS_noise_sigma));points.emplace_back(p);// 验证点是否在平面上（考虑到噪声，结果应接近但不完全等于0）LOG(INFO) << "res of p: " << p.dot(true_plane_coeffs.head<3>()) + true_plane_coeffs[3];}// 使用FitPlane函数进行平面拟合Vec4d estimated_plane_coeffs;if (sad::math::FitPlane(points, estimated_plane_coeffs)) {// 拟合成功，输出估计的平面参数和真实参数LOG(INFO) << "estimated coeffs: " << estimated_plane_coeffs.transpose()<< ", true: " << true_plane_coeffs.transpose();} else {// 拟合失败LOG(INFO) << "plane fitting failed";}
}/*** @brief 拟合平面* * @tparam S 数据类型（通常为float或double）* @param data 输入的3D点集* @param plane_coeffs 输出的平面方程系数 (ax + by + cz + d = 0)* @param eps 拟合误差阈值* @return bool 拟合是否成功*/
template <typename S>
bool FitPlane(std::vector<Eigen::Matrix<S, 3, 1>>& data, Eigen::Matrix<S, 4, 1>& plane_coeffs, double eps = 1e-2) {// 确保至少有3个点才能拟合平面if (data.size() < 3) {return false;}// 构建系数矩阵AEigen::MatrixXd A(data.size(), 4);for (int i = 0; i < data.size(); ++i) {A.row(i).head<3>() = data[i].transpose();A.row(i)[3] = 1.0;}// 使用SVD求解最小二乘问题Eigen::JacobiSVD svd(A, Eigen::ComputeThinV);plane_coeffs = svd.matrixV().col(3);// 检查拟合误差是否在阈值范围内for (int i = 0; i < data.size(); ++i) {double err = plane_coeffs.template head<3>().dot(data[i]) + plane_coeffs[3];if (err * err > eps) {return false;}}return true;
}

二、线性拟合

1. 问题提出

给定一组由N个点组成的点云 $=\left \{x_{1}, \cdots , x_{n} \right \}$ ,其中每个点取三维欧式坐标 $x_{k}$ ，寻找一组直线参数d，p使得：
$x = d t + p$
其中：d为直线方向，满足\left | d \right | = 1，p为直线上一点；t为直线参数。
构造最小二乘，对于任意一个不在直线上的点 $x_{k}$ ，利用勾股定理，计算与直线的垂直距离的平方，如下：
在这里插入图片描述
然后构造最小二乘问题，求解d 和 p。

继续：

故p可以取点云的中心。于是可以先确定p，再求d。
令 $y_{k} = x_{k} - p$ ，对上述构造的最小二乘进行化简，如下：

由于第一项不包含d，而求第二项最小值，只需取 $d^{T}y_{k}y^{T}_{k}d$ 的最大值即可。
将 $y^{T}_{k}$ 看做矩阵A，进行SVD分解找到最大奇异值对应的右奇异向量即是方程的解。

2. 实现

/*** @brief 拟合三维空间中的直线* * @tparam S 数据类型（通常为float或double）* @param data 输入的3D点集* @param origin 输出的直线上的一点（通常为点集的质心）* @param dir 输出的直线方向向量* @param eps 拟合误差阈值* @return bool 拟合是否成功*/
template <typename S>
bool FitLine(std::vector<Eigen::Matrix<S, 3, 1>>& data, Eigen::Matrix<S, 3, 1>& origin, Eigen::Matrix<S, 3, 1>& dir,double eps = 0.2) {// 确保至少有2个点才能拟合直线if (data.size() < 2) {return false;}// 计算点集的质心作为直线上的一点origin = std::accumulate(data.begin(), data.end(), Eigen::Matrix<S, 3, 1>::Zero().eval()) / data.size();// 构建矩阵Y，每行是点到质心的向量Eigen::MatrixXd Y(data.size(), 3);for (int i = 0; i < data.size(); ++i) {Y.row(i) = (data[i] - origin).transpose();}// 使用SVD求解主方向Eigen::JacobiSVD svd(Y, Eigen::ComputeFullV);dir = svd.matrixV().col(0);// 检查每个点到拟合直线的距离是否在阈值范围内for (const auto& d : data) {if (dir.template cross(d - origin).template squaredNorm() > eps) {return false;}}return true;
}