本篇文章介绍如何在点云中提取线段和平面。
一、平面拟合
1. 问题提出
给定一组由N个点组成的点云
X
=
{
x
1
,
⋯
,
x
n
}
X =\left \{x_{1}, \cdots , x_{n} \right \}
X={x1,⋯,xn} ,其中每个点取三维欧式坐标
x
k
x_{k}
xk，寻找一组平面参数n，d，使得：
n
T
x
k
+
d
=
0
n^{T}x_{k} + d = 0
nTxk+d=0
其中：n为法向量，d为截距。
在给出的一组点云数据，满足上述平面方程，且误差最小，于是可直接用最小二乘求解。
误差最小化：
使用齐次坐标，将上试简化：
其中：
x
~
=
[
x
T
1
]
\tilde{x} = \begin{bmatrix} x^T & 1 \end{bmatrix}
x~=[xT1],
n
~
=
[
n
T
d
]
T
\tilde{n} = \begin{bmatrix} n^T & d \end{bmatrix}^{T}
n~=[nTd]T。
将
x
~
k
\tilde{x}_{k}
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;
}
// 构建系数矩阵A
Eigen::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个点组成的点云
X
=
{
x
1
,
⋯
,
x
n
}
X =\left \{x_{1}, \cdots , x_{n} \right \}
X={x1,⋯,xn} ,其中每个点取三维欧式坐标
x
k
x_{k}
xk，寻找一组直线参数d，p使得：
x
=
d
t
+
p
x = dt +p
x=dt+p
其中：d为直线方向，满足\left | d \right | = 1，p为直线上一点；t为直线参数。
构造最小二乘，对于任意一个不在直线上的点
x
k
x_{k}
xk，利用勾股定理，计算与直线的垂直距离的平方，如下：
然后构造最小二乘问题，求解d 和 p。
继续：
故p可以取点云的中心。于是可以先确定p，再求d。
令
y
k
=
x
k
−
p
y_{k} = x_{k} - p
yk=xk−p，对上述构造的最小二乘进行化简，如下：
由于第一项不包含d，而求第二项最小值，只需取
d
T
y
k
y
k
T
d
d^{T}y_{k}y^{T}_{k}d
dTykykTd的最大值即可。
将
y
k
T
y^{T}_{k}
ykT看做矩阵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;
}
三、最小二乘的解法
1. 线性最小二乘SVD法
SVD相关介绍请看参考文献1
2.非线性最小二乘
（1）迭代最小二乘法
（2）优化方法
a. 高斯牛顿法
b. LM
四、参考
1.https://blog.csdn.net/qq_34213260/article/details/115345986
2.<<自动驾驶与机器人中的SLAM技术从理论到实践>>