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

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

一、平面拟合

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. 实现

cpp 复制代码

/**
 * @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. 实现

cpp 复制代码

/**
 * @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技术从理论到实践>>