《自动驾驶与机器人中的slam技术:从理论到实践》笔记——ch7（1）

7. 3D SLAM

        三维激光雷达的信息比二维激光雷达的信息更丰富，不容易被遮挡，能够更好地重建场地的三维结构。我们既可以对三维点云直接进行配准，也可以提取一些几何特征后再进行配准。

7.1多线激光雷达的工作原理

7.1.1机械旋转式激光雷达

        多线激光雷达与单线激光雷达的测距原理相同，不同点在于单线激光雷达是单条扫描线输出2D点集，多线激光雷达是多层扫描线输出的是3D点云，由此可见单线激光雷法仅能检测同一高度的障碍物而多线激光雷达可以测得不同高度的障碍物信息。它们的测距原理是向被测物体发送一个激光脉冲，然后测量返回脉冲与发送脉冲之间的时间间隔，再乘以光速来计算被测物体的距离。如果测量过程中发生透射，接收器可能得到多次回波，这时以最强的回波作为被测距离值。这种测距原理称为飞行时间原理，细分的话还可以分为DToF和IToF。由于DToF的功率较低，实现简单，目前多数激光雷达和RGBD相机都是使用DToF方案来测距。

        机械旋转式激光雷达，通常具有多个激光雷达发射器。它们由电机控制，按固定频率(例如每秒10圈)绕某个转轴进行旋转。这些激光雷达探头彼此之间有一个很小的夹角，旋转起来后，就可以扫描到一定视野范围内的物体。机械旋转式激光雷达的水平探测角度通常是360°，垂直方向是通常在30°-45°，探头的数量也称为线束，线束通常是2的整数倍。

        如图所示在三维点云中可以检测到车辆、路牌等形状明显的物体，可以检测静止和运动的障碍物，甚至可以通过反射强度获取车道线信息。。从定位和建图的角度来说，可以将元素特征进行配准也可以将多个扫描数据进行配准，得到全局的点云地图，然后让标注人员在点云中标注高精地图之后实现高精定位。

7.1.2固态激光雷达

        相对于需要在内置一套精密的运动机构使激光探头转动起来的这种价格高昂的装备，市面上也存在整体上让激光发射和接收装置不运动就能测量距离的雷达，他们统称为固态激光雷达。

        固态激光雷达有以下几种测量原理。

        1.保持激光雷达发射器和接收器不动，在前方加一个棱镜结构，改变激光的走向，实现不同位置的扫描。这种称为半固态雷达。

        2.整个激光雷达发射接收机构都不运动，但存在一个面阵式的发射和接收机构。它们或者按找一定顺序来扫描，或者同时扫描接收，形成纯固态激光雷达。

        无论是半固态雷达还是纯固态雷达，这些雷达都需要一个对外扫描窗口，用户可以得到该窗口扫描到的点云数据，我们将这个窗口的大小称为激光雷达的视野。固态激光雷达的水平视野远小于旋转式激光雷达，通常在120°以内，垂直视野则机械旋转式激光雷达相当。

7.2多线激光雷达的扫描匹配

        多线激光雷达的扫描匹配效果通常好于单线激光雷达，后端的处理更简单，大多数多线激光雷达的SLAM系统都不使用子地图这样的概念，而是直接管理点云本身。点云的配准主要以Scan to Scan或Scan to Map为主。

7.2.1 点到点ICP

        在单线激光雷达中，我们使用的是点到点的匹配方式。在多线激光雷达中，我们仍可使用点到点的配准方式，来配准两个点云

和

，那么对一组匹配点

来说，应该满足：

在此假设中并不假设两个点云的点数是相等的，也不假设按照初始的排序，点云里的每个点都是匹配的，是为了是的问题的研究更有普遍性，为了解决两个点云的配准问题，ICP的整体思路如下：

        此处

是当前点云中的值，之后迭代优化，找出当前点云中其他点的位姿

，在当前位姿下，目标匹配点云的原点

用

变换后，在目标点云中找最近连的点，形成匹配点对，在所有的值中挑出距离最小的位姿（收敛），作为匹配点同时也得到了最终位姿。

        以上为基础ICP的实现思路，在书中的点到点的匹配中，作者对算法进行了优化，并且增加了一些最小二乘法的额外功能，用于点到点匹配的误差表示：

        那么，就将旋转和平移的导数定义为（可通过代数运算来进行）

        代码实现部分：

/** * @brief 点到点（Point-to-Point）ICP配准核心实现 * @param init_pose 输入：位姿初始值；输出：配准后的最优位姿（SE3：旋转+平移） * @return 配准是否成功（有效内点数≥最小阈值则返回true） * @note 核心思想：最小化源点云变换后与目标点云对应点的欧式距离，通过高斯牛顿迭代求解最优位姿 */ bool Icp3d::AlignP2P(SE3& init_pose) { LOG(INFO) << "aligning with point to point"; // 断言保护：目标/源点云必须提前初始化，否则直接终止程序 assert(target_ != nullptr && source_ != nullptr); // 初始化迭代位姿：从输入的初始位姿开始 SE3 pose = init_pose; // 若不使用用户指定的初始平移，则用「目标点云中心 - 源点云中心」作为初始平移（加速收敛） if (!options_.use_initial_translation_) { pose.translation() = target_center_ - source_center_; // 设置平移初始值 } // 预先生成源点云的索引数组（用于后续并发遍历所有点） std::vector<int> index(source_->points.size()); for (int i = 0; i < index.size(); ++i) { index[i] = i; } // 初始化并发计算所需的容器： std::vector<bool> effect_pts(index.size(), false); // 标记每个源点是否为有效内点（过滤外点） std::vector<Eigen::Matrix<double, 3, 6>> jacobians(index.size()); // 每个点的雅可比矩阵（3x6：残差对旋转/平移的偏导） std::vector<Vec3d> errors(index.size()); // 每个点的残差（目标点 - 变换后源点） // 高斯牛顿迭代主循环（最多迭代max_iteration_次） for (int iter = 0; iter < options_.max_iteration_; ++iter) { // 高斯牛顿核心步骤1：并发遍历所有源点，计算残差和雅可比（C++17 par_unseq：多线程并行执行） // 最近邻搜索是计算密集型操作，并发可大幅提升效率 std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) { // 1. 提取第idx个源点的坐标（转换为Eigen向量，方便矩阵运算） auto q = ToVec3d(source_->points[idx]); // 2. 用当前位姿变换源点到目标点云坐标系：qs = R*q + t（SE3重载乘法） Vec3d qs = pose * q; // 3. KD树搜索目标点云中距离qs最近的1个点（nn存储最近邻点的索引） std::vector<int> nn; kdtree_->GetClosestPoint(ToPointType(qs), nn, 1); // 若找到有效最近邻 if (!nn.empty()) { // 提取目标点云的最近邻点坐标 Vec3d p = ToVec3d(target_->points[nn[0]]); // 计算变换后源点与目标点的平方距离（避免开根号，提升计算效率） double dis2 = (p - qs).squaredNorm(); // 距离超限：标记为外点，跳过后续计算 if (dis2 > options_.max_nn_distance_) { effect_pts[idx] = false; return; } // 标记为有效内点，开始构建残差和雅可比 effect_pts[idx] = true; // 残差构建：e = 目标点 - 变换后源点（最小化残差二范数） Vec3d e = p - qs; // 雅可比矩阵J（3x6）：J = [∂e/∂θ, ∂e/∂t]，θ为旋转李代数，t为平移向量 Eigen::Matrix<double, 3, 6> J; // ∂e/∂θ = R * hat(q) （hat：向量转反对称矩阵，对应旋转的李代数导数） J.block<3, 3>(0, 0) = pose.so3().matrix() * SO3::hat(q); // ∂e/∂t = -I（单位矩阵，平移的导数为负单位阵） J.block<3, 3>(0, 3) = -Mat3d::Identity(); // 保存当前点的雅可比和残差（后续累加Hessian用） jacobians[idx] = J; errors[idx] = e; } else { // 未找到最近邻：标记为外点 effect_pts[idx] = false; } }); // 高斯牛顿核心步骤2：累加Hessian矩阵（H=J^T*J）和误差向量（b=-J^T*e） double total_res = 0; // 所有内点的残差平方和（评估迭代效果） int effective_num = 0; // 有效内点数量（过滤外点后的点数） // accumulate：遍历索引，累加H和b（原则上可并发reduce，此处简化为顺序累加） auto H_and_err = std::accumulate( index.begin(), index.end(), std::pair<Mat6d, Vec6d>(Mat6d::Zero(), Vec6d::Zero()), [&jacobians, &errors, &effect_pts, &total_res, &effective_num](const std::pair<Mat6d, Vec6d>& pre, int idx) -> std::pair<Mat6d, Vec6d> { // 外点：跳过累加 if (!effect_pts[idx]) { return pre; } else { // 累加残差平方和 total_res += errors[idx].dot(errors[idx]); effective_num++; // 累加Hessian：pre.first + J^T*J // 累加误差向量：pre.second - J^T*e return std::pair<Mat6d, Vec6d>(pre.first + jacobians[idx].transpose() * jacobians[idx], pre.second - jacobians[idx].transpose() * errors[idx]); } }); // 有效内点数量不足：配准失败，返回false if (effective_num < options_.min_effective_pts_) { LOG(WARNING) << "effective num too small: " << effective_num; return false; } // 提取累加后的Hessian矩阵和误差向量 Mat6d H = H_and_err.first; Vec6d err = H_and_err.second; // 高斯牛顿核心步骤3：求解增量dx（H*dx = err），直接求逆（工程中建议用Cholesky分解更稳定） Vec6d dx = H.inverse() * err; // 更新位姿：旋转部分用李代数指数映射（SO3::exp），平移部分直接累加 pose.so3() = pose.so3() * SO3::exp(dx.head<3>()); // 旋转更新：R = R * exp(dθ) pose.translation() += dx.tail<3>(); // 平移更新：t = t + dt // 打印迭代信息：残差、有效点数、平均残差、增量范数（评估收敛趋势） LOG(INFO) << "iter " << iter << " total res: " << total_res << ", eff: " << effective_num << ", mean res: " << total_res / effective_num << ", dxn: " << dx.norm(); // 若设置了真实位姿（GT），打印当前位姿与GT的误差（仅用于评估，不影响配准） if (gt_set_) { double pose_error = (gt_pose_.inverse() * pose).log().norm(); LOG(INFO) << "iter " << iter << " pose error: " << pose_error; } // 收敛判断：增量范数小于阈值，停止迭代（避免无效迭代） if (dx.norm() < options_.eps_) { LOG(INFO) << "converged, dx = " << dx.transpose(); break; } } // 将配准后的最优位姿赋值给输出参数 init_pose = pose; return true; } /** * @brief 点到平面（Point-to-Plane）ICP配准核心实现 * @param init_pose 输入：位姿初始值；输出：配准后的最优位姿 * @return 配准是否成功（有效内点数≥最小阈值则返回true） * @note 相比P2P，P2Plane收敛更快、精度更高，核心是最小化源点到目标点云局部平面的距离 */ bool Icp3d::AlignP2Plane(SE3& init_pose) { LOG(INFO) << "aligning with point to plane"; assert(target_ != nullptr && source_ != nullptr); // 整体流程与P2P一致，核心差异：残差为点到平面的距离，雅可比维度变为1x6（标量残差） SE3 pose = init_pose; // 初始平移设置（逻辑同P2P） if (!options_.use_initial_translation_) { pose.translation() = target_center_ - source_center_; // 设置平移初始值 } // 预生成源点索引（并发遍历用） std::vector<int> index(source_->points.size()); for (int i = 0; i < index.size(); ++i) { index[i] = i; } // 初始化容器（与P2P的差异：残差为标量，雅可比为1x6） std::vector<bool> effect_pts(index.size(), false); // 有效内点标记 std::vector<Eigen::Matrix<double, 1, 6>> jacobians(index.size()); // 点到平面残差的雅可比（1x6） std::vector<double> errors(index.size()); // 点到平面的距离（标量残差） // 高斯牛顿迭代主循环 for (int iter = 0; iter < options_.max_iteration_; ++iter) { // 并发遍历所有源点：最近邻搜索 + 平面拟合 + 残差/雅可比计算 std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) { auto q = ToVec3d(source_->points[idx]); Vec3d qs = pose * q; // 变换后源点坐标 // 搜索5个最近邻（至少3个点才能拟合平面，取5个提升鲁棒性） std::vector<int> nn; kdtree_->GetClosestPoint(ToPointType(qs), nn, 5); // 最近邻数量足够（>3），才能拟合平面 if (nn.size() > 3) { // 转换最近邻点为Eigen向量（方便拟合平面） std::vector<Vec3d> nn_eigen; for (int i = 0; i < nn.size(); ++i) { nn_eigen.emplace_back(ToVec3d(target_->points[nn[i]])); } // 拟合平面：n = [a,b,c,d]，满足 ax+by+cz+d=0（n.head<3>()为平面法向量，已单位化） Vec4d n; if (!math::FitPlane(nn_eigen, n)) { // 平面拟合失败：标记为外点 effect_pts[idx] = false; return; } // 计算点到平面的距离：dis = a*qs.x + b*qs.y + c*qs.z + d（法向量单位化后无需除以模长） double dis = n.head<3>().dot(qs) + n[3]; // 距离超限：标记为外点 if (fabs(dis) > options_.max_plane_distance_) { effect_pts[idx] = false; return; } // 标记为有效内点，构建残差和雅可比 effect_pts[idx] = true; // 点到平面残差的雅可比矩阵（1x6）：J = [∂dis/∂θ, ∂dis/∂t] Eigen::Matrix<double, 1, 6> J; // ∂dis/∂θ = -n^T * R * hat(q)（旋转部分的导数） J.block<1, 3>(0, 0) = -n.head<3>().transpose() * pose.so3().matrix() * SO3::hat(q); // ∂dis/∂t = n^T（平移部分的导数） J.block<1, 3>(0, 3) = n.head<3>().transpose(); jacobians[idx] = J; errors[idx] = dis; } else { // 最近邻数量不足：标记为外点 effect_pts[idx] = false; } }); // 累加Hessian和误差向量（逻辑同P2P，仅雅可比/残差维度不同） double total_res = 0; int effective_num = 0; auto H_and_err = std::accumulate( index.begin(), index.end(), std::pair<Mat6d, Vec6d>(Mat6d::Zero(), Vec6d::Zero()), [&jacobians, &errors, &effect_pts, &total_res, &effective_num](const std::pair<Mat6d, Vec6d>& pre, int idx) -> std::pair<Mat6d, Vec6d> { if (!effect_pts[idx]) { return pre; } else { total_res += errors[idx] * errors[idx]; effective_num++; return std::pair<Mat6d, Vec6d>(pre.first + jacobians[idx].transpose() * jacobians[idx], pre.second - jacobians[idx].transpose() * errors[idx]); } }); // 有效点数不足：配准失败 if (effective_num < options_.min_effective_pts_) { LOG(WARNING) << "effective num too small: " << effective_num; return false; } // 求解增量并更新位姿（逻辑同P2P） Mat6d H = H_and_err.first; Vec6d err = H_and_err.second; Vec6d dx = H.inverse() * err; pose.so3() = pose.so3() * SO3::exp(dx.head<3>()); pose.translation() += dx.tail<3>(); // 打印迭代信息 LOG(INFO) << "iter " << iter << " total res: " << total_res << ", eff: " << effective_num << ", mean res: " << total_res / effective_num << ", dxn: " << dx.norm(); // 打印与GT的误差（若有） if (gt_set_) { double pose_error = (gt_pose_.inverse() * pose).log().norm(); LOG(INFO) << "iter " << iter << " pose error: " << pose_error; } // 收敛判断 if (dx.norm() < options_.eps_) { LOG(INFO) << "converged, dx = " << dx.transpose(); break; } } init_pose = pose; return true; } /** * @brief 构建目标点云的KD树（加速最近邻搜索） * @note 必须在配准前调用！启用ANN（近似最近邻）平衡搜索速度与精度 */ void Icp3d::BuildTargetKdTree() { // 创建KD树实例 kdtree_ = std::make_shared<KdTree>(); // 基于目标点云构建KD树 kdtree_->BuildTree(target_); // 启用近似最近邻搜索（ANN）：比精确搜索快，适合大规模点云 kdtree_->SetEnableANN(); } /** * @brief 点到线（Point-to-Line）ICP配准核心实现 * @param init_pose 输入：位姿初始值；输出：配准后的最优位姿 * @return 配准是否成功（有效内点数≥最小阈值则返回true） * @warning 代码存在逻辑BUG：use_initial_translation_判断反了（应为!use_initial_translation_） * @note 核心思想：最小化源点到目标点云局部直线的距离，流程与P2Plane几乎一致 */ bool Icp3d::AlignP2Line(SE3& init_pose) { LOG(INFO) << "aligning with point to line"; assert(target_ != nullptr && source_ != nullptr); // 点到线与点到平面流程一致，核心差异：残差为点到直线的距离，雅可比为3x6 SE3 pose = init_pose; // ========== 潜在BUG ========== // 正确逻辑应为 !options_.use_initial_translation_（与P2P/P2Plane保持一致） if (options_.use_initial_translation_) { pose.translation() = target_center_ - source_center_; // 设置平移初始值 LOG(INFO) << "init trans set to " << pose.translation().transpose(); } // 预生成源点索引（并发遍历用） std::vector<int> index(source_->points.size()); for (int i = 0; i < index.size(); ++i) { index[i] = i; } // 初始化容器（与P2P一致：残差为3维向量，雅可比为3x6） std::vector<bool> effect_pts(index.size(), false); // 有效内点标记 std::vector<Eigen::Matrix<double, 3, 6>> jacobians(index.size()); // 点到线残差的雅可比（3x6） std::vector<Vec3d> errors(index.size()); // 点到线的误差向量 // 高斯牛顿迭代主循环 for (int iter = 0; iter < options_.max_iteration_; ++iter) { // 并发遍历所有源点：最近邻搜索 + 直线拟合 + 残差/雅可比计算 std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) { auto q = ToVec3d(source_->points[idx]); Vec3d qs = pose * q; // 变换后源点坐标 // 搜索5个最近邻（拟合直线需至少2个点，取5个提升鲁棒性） std::vector<int> nn; kdtree_->GetClosestPoint(ToPointType(qs), nn, 5); // 严格要求5个最近邻（可调整为≥2，增强鲁棒性） if (nn.size() == 5) { // 转换最近邻点为Eigen向量（方便拟合直线） std::vector<Vec3d> nn_eigen; for (int i = 0; i < 5; ++i) { nn_eigen.emplace_back(ToVec3d(target_->points[nn[i]])); } // 拟合直线：p0为直线上一点，d为直线方向向量（已单位化） Vec3d d, p0; if (!math::FitLine(nn_eigen, p0, d, options_.max_line_distance_)) { // 直线拟合失败：标记为外点 effect_pts[idx] = false; return; } // 计算点到直线的误差向量：err = hat(d) * (qs - p0)（叉乘表示垂直距离） Vec3d err = SO3::hat(d) * (qs - p0); // 距离超限：标记为外点 if (err.norm() > options_.max_line_distance_) { effect_pts[idx] = false; return; } // 标记为有效内点，构建残差和雅可比 effect_pts[idx] = true; // 点到线残差的雅可比矩阵（3x6）：J = [∂err/∂θ, ∂err/∂t] Eigen::Matrix<double, 3, 6> J; // ∂err/∂θ = -hat(d) * R * hat(q)（旋转部分的导数） J.block<3, 3>(0, 0) = -SO3::hat(d) * pose.so3().matrix() * SO3::hat(q); // ∂err/∂t = hat(d)（平移部分的导数） J.block<3, 3>(0, 3) = SO3::hat(d); jacobians[idx] = J; errors[idx] = err; } else { // 最近邻数量不足：标记为外点 effect_pts[idx] = false; } }); // 累加Hessian和误差向量（逻辑同P2P） double total_res = 0; int effective_num = 0; auto H_and_err = std::accumulate( index.begin(), index.end(), std::pair<Mat6d, Vec6d>(Mat6d::Zero(), Vec6d::Zero()), [&jacobians, &errors, &effect_pts, &total_res, &effective_num](const std::pair<Mat6d, Vec6d>& pre, int idx) -> std::pair<Mat6d, Vec6d> { if (!effect_pts[idx]) { return pre; } else { total_res += errors[idx].dot(errors[idx]); effective_num++; return std::pair<Mat6d, Vec6d>(pre.first + jacobians[idx].transpose() * jacobians[idx], pre.second - jacobians[idx].transpose() * errors[idx]); } }); // 有效点数不足：配准失败 if (effective_num < options_.min_effective_pts_) { LOG(WARNING) << "effective num too small: " << effective_num; return false; } // 求解增量并更新位姿（逻辑同P2P） Mat6d H = H_and_err.first; Vec6d err = H_and_err.second; Vec6d dx = H.inverse() * err; pose.so3() = pose.so3() * SO3::exp(dx.head<3>()); pose.translation() += dx.tail<3>(); // 打印与GT的误差（若有） if (gt_set_) { double pose_error = (gt_pose_.inverse() * pose).log().norm(); LOG(INFO) << "iter " << iter << " pose error: " << pose_error; } // 打印迭代信息 LOG(INFO) << "iter " << iter << " total res: " << total_res << ", eff: " << effective_num << ", mean res: " << total_res / effective_num << ", dxn: " << dx.norm(); // 收敛判断 if (dx.norm() < options_.eps_) { LOG(INFO) << "converged, dx = " << dx.transpose(); break; } } init_pose = pose; return true; }

测试程序：

// 定义源点云/目标点云指针（sad::CloudPtr是PCL点云指针的封装，sad::PointCloudType对应pcl::PointCloud<pcl::PointXYZ>） sad::CloudPtr source(new sad::PointCloudType), target(new sad::PointCloudType); // 从命令行参数指定的路径加载源点云PCD文件 pcl::io::loadPCDFile(fLS::FLAGS_source, *source); // 从命令行参数指定的路径加载目标点云PCD文件 pcl::io::loadPCDFile(fLS::FLAGS_target, *target); // 标记配准是否成功的布尔变量（跨三个ICP变体共享） bool success; /** * 测试「点到点（P2P）ICP」配准 * sad::evaluate_and_call：工具函数，作用是执行传入的匿名函数，并统计函数调用耗时（输出平均时间/调用次数） * 参数1：待执行的匿名函数（核心ICP配准逻辑） * 参数2：耗时统计的标识名称（日志输出用） * 参数3：函数调用次数（此处为1次） */ sad::evaluate_and_call( [&]() { // 初始化ICP 3D配准器实例 sad::Icp3d icp; // 设置源点云（需要配准的点云） icp.SetSource(source); // 设置目标点云（参考点云，固定不动） icp.SetTarget(target); // 设置位姿真值（Ground Truth），用于评估配准精度（非必需，仅日志输出误差） icp.SetGroundTruth(gt_pose); // 初始化配准结果位姿（SE3：旋转+平移，初始为单位矩阵） SE3 pose; // 执行P2P ICP配准，返回是否成功（有效内点数达标则为true） success = icp.AlignP2P(pose); // 若配准成功 if (success) { // 日志输出配准结果：四元数（旋转）+ 平移向量 LOG(INFO) << "icp p2p align success, pose: " << pose.so3().unit_quaternion().coeffs().transpose() << ", " << pose.translation().transpose(); // 定义变换后的源点云指针（用于可视化配准效果） sad::CloudPtr source_trans(new sad::PointCloudType); // 将源点云通过配准后的位姿变换到目标坐标系（SE3矩阵转float类型适配PCL接口） pcl::transformPointCloud(*source, *source_trans, pose.matrix().cast<float>()); // 保存变换后的源点云到PCD文件（后续可通过pcl_viewer可视化验证配准效果） sad::SaveCloudToFile("./data/ch7/icp_trans.pcd", *source_trans); } else { // 配准失败（有效点数不足），输出错误日志 LOG(ERROR) << "align failed."; } }, "ICP P2P", 1); // 耗时统计标识："ICP P2P"，调用次数：1 /// 测试「点到平面（P2Plane）ICP」配准（逻辑与P2P一致，仅配准算法不同） sad::evaluate_and_call( [&]() { sad::Icp3d icp; icp.SetSource(source); icp.SetTarget(target); icp.SetGroundTruth(gt_pose); SE3 pose; // 执行点到平面ICP配准 success = icp.AlignP2Plane(pose); if (success) { LOG(INFO) << "icp p2plane align success, pose: " << pose.so3().unit_quaternion().coeffs().transpose() << ", " << pose.translation().transpose(); sad::CloudPtr source_trans(new sad::PointCloudType); pcl::transformPointCloud(*source, *source_trans, pose.matrix().cast<float>()); // 保存点到平面ICP变换后的点云（区分P2P的保存路径） sad::SaveCloudToFile("./data/ch7/icp_plane_trans.pcd", *source_trans); } else { LOG(ERROR) << "align failed."; } }, "ICP P2Plane", 1); // 耗时统计标识："ICP P2Plane" /// 测试「点到线（P2Line）ICP」配准（逻辑与前两种一致，仅配准算法不同） sad::evaluate_and_call( [&]() { sad::Icp3d icp; icp.SetSource(source); icp.SetTarget(target); icp.SetGroundTruth(gt_pose); SE3 pose; // 执行点到线ICP配准 success = icp.AlignP2Line(pose); if (success) { LOG(INFO) << "icp p2line align success, pose: " << pose.so3().unit_quaternion().coeffs().transpose() << ", " << pose.translation().transpose(); sad::CloudPtr source_trans(new sad::PointCloudType); pcl::transformPointCloud(*source, *source_trans, pose.matrix().cast<float>()); // 保存点到线ICP变换后的点云（区分前两种的保存路径） sad::SaveCloudToFile("./data/ch7/icp_line_trans.pcd", *source_trans); } else { LOG(ERROR) << "align failed."; } }, "ICP P2Line", 1); // 耗时统计标识："ICP P2Line"

测试结果：

        由测试数据可知并发ICP速度明显快于PCL版本，其精准度也优于PCL版本的ICP。在点云密度较小时，并发ICP的计算效率更高。每次迭代的总体误差、平均误差和真值误差也明显随迭代次数下降。

        点云匹配图示结果：

        由于多线激光点云比较稀疏,即使激光打到同一个物体上,每次也不一定返回同一个点。而点到点 ICP 的基本目标是将一个点云中的每个点和另一个点云进行配准，这必然会带来一些问题。我们可以对 ICP进行各种各样的改进，例如允许一个点与多个点进行匹配，或者将一个点和另一些点的统计量进行配准。

7.2.2点到线、点到面ICP

        点到线ICP和点到面ICP是两种对ICP的直接改进方法。它们的原理与二维点到线ICP类似，只是优化变量部分需要改成三维的，自变量和导数也要按照流形来处理。设依然寻找两个点云

和

之间的变换

。对某个

进行变换后，并不是寻找一个点而是寻找一些最近邻，将他们拟合成一个平面或直线。

        如果是对平面的情况，假设你拟合成的平面参数

,其中

为单位长度法线，

为截距。对于平面上的点

，它与平面参数之间的关系为

对于平面外的一个点，它离平面的距离可以写成

，建立

与它最近邻点构成的那个平面之间的误差函数：

则它对

和

的导数：

        在使用代码实现时：点到面的ICP与点到点ICP的思路基本一致，但是有两个小的差异点。

1.需要给最近邻点拟合平面，因此需要查找多个最近邻，然后进行局部的平面拟合。

2.需要添加一些阈值检查来判定平面是否合理，然后用平面参数计算高斯-牛顿函数的雅可比矩阵。

        部分代码实现：

/** * @brief ICP3d类的点到线（Point-to-Line）配准核心方法 * @details 基于高斯牛顿（Gauss-Newton）迭代实现3D点云的ICP配准，目标函数为源点云转换后到目标点云局部拟合直线的距离 * @param init_pose 输入初始位姿（SE3：旋转+平移），输出配准后的最优位姿 * @return bool 配准是否成功（有效点数足够、迭代收敛等） */ bool Icp3d::AlignP2Line(SE3& init_pose) { LOG(INFO) << "aligning with point to line"; // 断言检查：目标点云（target_）和源点云（source_）必须已初始化（非空） assert(target_ != nullptr && source_ != nullptr); // 点到线（P2L）与点到面（P2P）ICP的核心迭代框架完全一致，仅残差和雅可比计算不同 // 初始化迭代位姿：从输入的初始位姿开始 SE3 pose = init_pose; // 如果配置了使用初始平移估计：用目标点云中心 - 源点云中心作为初始平移 if (options_.use_initial_translation_) { pose.translation() = target_center_ - source_center_; // 设置平移初始值 LOG(INFO) << "init trans set to " << pose.translation().transpose(); } // 构建源点云索引数组（用于后续并行遍历所有源点） std::vector<int> index(source_->points.size()); for (int i = 0; i < index.size(); ++i) { index[i] = i; } // 初始化辅助数组： // effect_pts：标记每个源点是否为有效点（拟合直线成功、误差在阈值内） // jacobians：每个点的雅可比矩阵（3x6，残差对位姿增量的偏导） // errors：每个点的残差（点到线的误差向量） std::vector<bool> effect_pts(index.size(), false); std::vector<Eigen::Matrix<double, 3, 6>> jacobians(index.size()); std::vector<Vec3d> errors(index.size()); // 高斯牛顿迭代主循环（最多迭代max_iteration_次） for (int iter = 0; iter < options_.max_iteration_; ++iter) { // -------------------------- 步骤1：并行计算每个源点的残差和雅可比 -------------------------- // 并行遍历所有源点（par_unseq：并行+无序执行，最大化多核利用率） std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) { // 取出第idx个源点，转换为Eigen向量（Vec3d = Eigen::Vector3d） auto q = ToVec3d(source_->points[idx]); // 用当前位姿将源点转换到目标点云坐标系下：qs = pose * q (齐次变换) Vec3d qs = pose * q; // 存储目标点云中的最近邻索引（取5个最近邻用于拟合直线） std::vector<int> nn; // 通过KD树查找转换后点qs在目标点云中的5个最近邻 kdtree_->GetClosestPoint(ToPointType(qs), nn, 5); // 仅当找到5个最近邻时，才进行直线拟合和残差计算 if (nn.size() == 5) { // 将5个最近邻点转换为Eigen向量格式，方便后续拟合 std::vector<Vec3d> nn_eigen; for (int i = 0; i < 5; ++i) { nn_eigen.emplace_back(ToVec3d(target_->points[nn[i]])); } // p0：拟合直线上的一个点；d：直线的单位方向向量 Vec3d d, p0; // 对5个最近邻点拟合直线，失败则标记为无效点 // max_line_distance_：点到拟合直线的最大距离阈值（用于过滤离群点） if (!math::FitLine(nn_eigen, p0, d, options_.max_line_distance_)) { effect_pts[idx] = false; return; } // 计算点到线的残差向量：err = d × (qs - p0)（叉乘，模长为点到线的距离） // 数学原理：直线方向为d，点qs到直线的向量为(qs-p0)，叉乘结果的模长是点到线的垂直距离 Vec3d err = SO3::hat(d) * (qs - p0); // 残差模长超过阈值，标记为无效点 if (err.norm() > options_.max_line_distance_) { effect_pts[idx] = false; return; } // 标记该点为有效点 effect_pts[idx] = true; // -------------------------- 构建雅可比矩阵 J (3x6) -------------------------- // 雅可比矩阵含义：残差err对6维位姿增量dx（前3：旋转增量，后3：平移增量）的偏导数 // 位姿增量模型：pose_new = pose_old * exp(dx)，其中exp(dx)是SE3的指数映射 Eigen::Matrix<double, 3, 6> J; // 旋转部分雅可比 (3x3)：-d^× * R * q^× （R是当前位姿的旋转矩阵，q^×是q的反对称矩阵） // 数学推导：d/dθ [d × (R(θ)q + t - p0)] = -d^× * R * q^× J.block<3, 3>(0, 0) = -SO3::hat(d) * pose.so3().matrix() * SO3::hat(q); // 平移部分雅可比 (3x3)：d^× （残差对平移增量的偏导） // 数学推导：d/dt [d × (Rq + t - p0)] = d^× J.block<3, 3>(0, 3) = SO3::hat(d); // 保存当前点的雅可比和残差 jacobians[idx] = J; errors[idx] = err; } else { // 最近邻数量不足5个，标记为无效点 effect_pts[idx] = false; } }); // -------------------------- 步骤2：累加Hessian矩阵和误差向量 -------------------------- // 初始化总残差、有效点数 double total_res = 0; int effective_num = 0; // 累加所有有效点的Hessian（H=J^T*J）和误差向量（b=-J^T*err） // accumulate：从初始值（零矩阵+零向量）开始，遍历所有点累加 auto H_and_err = std::accumulate( index.begin(), index.end(), std::pair<Mat6d, Vec6d>(Mat6d::Zero(), Vec6d::Zero()), [&jacobians, &errors, &effect_pts, &total_res, &effective_num](const std::pair<Mat6d, Vec6d>& pre, int idx) -> std::pair<Mat6d, Vec6d> { // 无效点跳过 if (!effect_pts[idx]) { return pre; } else { // 累加总残差（残差向量的点积，即残差平方和） total_res += errors[idx].dot(errors[idx]); // 有效点数+1 effective_num++; // 累加Hessian矩阵：pre.first + J^T*J // 累加误差向量：pre.second - J^T*err return std::pair<Mat6d, Vec6d>(pre.first + jacobians[idx].transpose() * jacobians[idx], pre.second - jacobians[idx].transpose() * errors[idx]); } }); // 检查有效点数是否满足最小值要求，不足则配准失败 if (effective_num < options_.min_effective_pts_) { LOG(WARNING) << "effective num too small: " << effective_num; return false; } // 提取累加后的Hessian矩阵H和误差向量err Mat6d H = H_and_err.first; Vec6d err = H_and_err.second; // -------------------------- 步骤3：求解位姿增量并更新位姿 -------------------------- // 高斯牛顿求解：H*dx = err → dx = H⁻¹ * err Vec6d dx = H.inverse() * err; // 更新旋转：用SO3的指数映射将旋转增量（dx前3维）转换为旋转矩阵，左乘当前旋转 pose.so3() = pose.so3() * SO3::exp(dx.head<3>()); // 更新平移：直接加上平移增量（dx后3维） pose.translation() += dx.tail<3>(); // 如果设置了真实位姿（gt_pose_），计算当前位姿与真实位姿的误差并打印 if (gt_set_) { double pose_error = (gt_pose_.inverse() * pose).log().norm(); LOG(INFO) << "iter " << iter << " pose error: " << pose_error; } // 打印当前迭代信息：总残差、有效点数、平均残差、增量范数 LOG(INFO) << "iter " << iter << " total res: " << total_res << ", eff: " << effective_num << ", mean res: " << total_res / effective_num << ", dxn: " << dx.norm(); // -------------------------- 步骤4：判断收敛 -------------------------- // 增量范数小于收敛阈值（eps_），说明位姿变化足够小，收敛退出 if (dx.norm() < options_.eps_) { LOG(INFO) << "converged, dx = " << dx.transpose(); break; } } // 将配准后的最终位姿赋值给输入参数（输出结果） init_pose = pose; // 配准成功 return true; }

        点到线的实现思路，只需将点面的残差改为点线残差即可。

        单独使用点到点或者点到面的模型是为了如果目标点云在被查询的那个点附近呈现为平面形状，就用点面的残差模型；如果呈现为直线的形状，就用点线的残差模型。这是自动驾驶中非常常见的一种做法，先对点云提取特征，再根据特征形式调用不同的ICP算法。（可以不可把所有物体的特征提取出来，然后调库直接对点云进行目标匹配呢）。

7.2.3NDT方法

        与原始ICP（点到点ICP）相比，点到线ICP和点到面ICP不再单独把点配准到某个单独的点上，而是与某个统计量进行配准。那么如果得到这堆点云的局部统计信息（通常为均值和方），然后利用这些统计信息进行匹配就可以了。

        NDT的大致思路是：

        1.把目标点云按一定分辨率分成若干体素，实现点云的轻量化与分块管理。

        2.计算每个体素中的点云高斯分布。设第

个体素的均值为

，方差为

，描述体素的统计特征。

        3.配准时，先计算每个点落在哪个体素中，然后建立该点与该体素的

，

构成的残差。

这一步是取源点云的任意一个点

用当前估计的位姿参数（R,t），将其变换到目标点云的坐标系下，得到

，确定其所在体素以及它的均值和方差。

        4.利用高斯-牛顿法或L-M方法对估计位姿进行迭代，然后更新位姿估计。

在关键的第三步中，设被配准的点云中的某个点为

，它经过R,t变换后，落在某个统计指标为

的体素中，记这个栅格的残差为

上述问题同样也可以看做是最大化每个点落在所在栅格的概率分布，因此是一个最大似然估计：

        代码实现部分：.h文件

/** * @brief 3D正态分布变换（Normal Distributions Transform, NDT）配准类 * @details 基于体素化目标点云、高斯牛顿迭代实现3D点云配准，核心思想是将目标点云体素化并计算每个体素的正态分布（均值+协方差）， * 最小化源点云转换后在目标体素分布下的负对数似然函数，实现高精度点云配准 */ class Ndt3d { public: /** * @brief 邻域体素搜索类型枚举 * @details 配准时为源点查找目标点云体素的策略：仅搜索中心体素 / 搜索6邻域体素（上下左右前后） */ enum class NearbyType { CENTER, // 仅搜索源点转换后位置对应的中心体素 NEARBY6, // 搜索中心体素+6个邻域体素（上下/左右/前后），提升匹配鲁棒性 }; /** * @brief NDT配准参数配置结构体 * @details 包含迭代次数、体素参数、收敛条件、异常值过滤等核心配置 */ struct Options { int max_iteration_ = 20; // 高斯牛顿最大迭代次数 double voxel_size_ = 1.0; // 体素网格大小（单位：米），决定体素化粒度 double inv_voxel_size_ = 1.0; // 体素大小的倒数（预计算，用于快速计算体素索引） int min_effective_pts_ = 10; // 配准有效点数阈值（低于此值则配准失败） int min_pts_in_voxel_ = 3; // 单个体素内有效点数阈值（低于此值不计算协方差） double eps_ = 1e-2; // 收敛判定阈值（位姿增量的范数小于此值则停止迭代） double res_outlier_th_ = 20.0; // 残差异常值阈值（过滤离群点，提升配准稳定性） bool remove_centroid_ = false; // 是否对源/目标点云去中心（移除整体平移，仅配准旋转） NearbyType nearby_type_ = NearbyType::NEARBY6; // 邻域体素搜索类型 }; using KeyType = Eigen::Matrix<int, 3, 1>; // 体素的三维整数索引类型（x/y/z方向的体素编号） /** * @brief 单个体素的数据结构 * @details 存储体素内的点索引、统计特征（均值/协方差）、预计算的信息矩阵（协方差逆） */ struct VoxelData { /// 默认构造函数（初始化空体素） VoxelData() {} /// 带初始点索引的构造函数 /// @param id 目标点云中的点索引，加入当前体素 VoxelData(size_t id) { idx_.emplace_back(id); } std::vector<size_t> idx_; // 当前体素包含的目标点云点索引列表 Vec3d mu_ = Vec3d::Zero(); // 体素内点的均值（正态分布均值） Mat3d sigma_ = Mat3d::Zero(); // 体素内点的协方差矩阵（正态分布协方差） Mat3d info_ = Mat3d::Zero(); // 协方差矩阵的逆（信息矩阵，预计算避免重复求逆） }; /** * @brief 默认构造函数 * @details 初始化默认配置，计算体素大小倒数，预生成邻域体素索引偏移量 */ Ndt3d() { options_.inv_voxel_size_ = 1.0 / options_.voxel_size_; // 预计算体素大小倒数 GenerateNearbyGrids(); // 预生成邻域体素索引偏移量（如6邻域） } /** * @brief 带参数的构造函数 * @param options 自定义的NDT配准参数 * @details 初始化指定配置，计算体素大小倒数，预生成邻域体素索引偏移量 */ Ndt3d(Options options) : options_(options) { options_.inv_voxel_size_ = 1.0 / options_.voxel_size_; // 预计算体素大小倒数 GenerateNearbyGrids(); // 预生成邻域体素索引偏移量 } /** * @brief 设置目标点云（参考点云） * @param target 目标点云指针（CloudPtr为点云智能指针，如pcl::PointCloud<PointType>::Ptr） * @details 存储目标点云，构建体素网格并计算每个体素的统计特征（均值/协方差/信息矩阵）， * 同时计算目标点云的整体中心 */ void SetTarget(CloudPtr target) { target_ = target; BuildVoxels(); // 构建目标点云的体素网格，计算体素统计特征 // 计算目标点云的中心（所有点坐标的均值） target_center_ = std::accumulate(target->points.begin(), target_->points.end(), Vec3d::Zero().eval(), [](const Vec3d& c, const PointType& pt) -> Vec3d { return c + ToVec3d(pt); }) / target_->size(); } /** * @brief 设置源点云（待配准点云） * @param source 源点云指针 * @details 存储源点云，计算源点云的整体中心（用于可选的去中心操作） */ void SetSource(CloudPtr source) { source_ = source; // 计算源点云的中心（所有点坐标的均值） source_center_ = std::accumulate(source_->points.begin(), source_->points.end(), Vec3d::Zero().eval(), [](const Vec3d& c, const PointType& pt) -> Vec3d { return c + ToVec3d(pt); }) / source_->size(); } /** * @brief 设置真实位姿（Ground Truth） * @param gt_pose 真实的配准位姿（SE3：旋转+平移） * @details 用于评估配准精度，仅调试/验证时使用 */ void SetGtPose(const SE3& gt_pose) { gt_pose_ = gt_pose; gt_set_ = true; // 标记已设置真实位姿 } /** * @brief 核心配准函数：使用高斯牛顿法实现NDT配准 * @param init_pose 输入初始位姿（SE3），输出配准后的最优位姿 * @return bool 配准是否成功（有效点数足够、迭代收敛等） */ bool AlignNdt(SE3& init_pose); private: /** * @brief 构建目标点云的体素网格 * @details 将目标点云划分到体素中，统计每个体素的点索引，计算均值、协方差、信息矩阵 */ void BuildVoxels(); /** * @brief 预生成邻域体素索引偏移量 * @details 根据NearbyType（CENTER/NEARBY6）生成需要搜索的邻域体素索引偏移（如(0,0,0)、(±1,0,0)等）， * 存入nearby_grids_，避免每次搜索重复计算 */ void GenerateNearbyGrids(); // 核心成员变量 CloudPtr target_ = nullptr; // 目标点云（参考点云）指针 CloudPtr source_ = nullptr; // 源点云（待配准点云）指针 Vec3d target_center_ = Vec3d::Zero(); // 目标点云的整体中心坐标 Vec3d source_center_ = Vec3d::Zero(); // 源点云的整体中心坐标 SE3 gt_pose_; // 真实位姿（仅用于评估配准精度） bool gt_set_ = false; // 是否设置了真实位姿 Options options_; // NDT配准参数配置 /** * @brief 体素网格哈希表 * @details 键：体素三维整数索引（KeyType）；值：体素数据（VoxelData） * @note hash_vec<3>是自定义的Eigen::Matrix<int,3,1>哈希函数，用于unordered_map */ std::unordered_map<KeyType, VoxelData, hash_vec<3>> grids_; std::vector<KeyType> nearby_grids_; // 预生成的邻域体素索引偏移量列表（如6邻域的偏移） };

.c文件

/** * @brief 构建目标点云的体素网格（核心预处理步骤） * @details 将目标点云划分到三维体素网格中，统计每个体素的点索引，并计算体素内点集的均值、协方差及信息矩阵（协方差逆）， * 为后续NDT配准提供正态分布模型；通过SVD处理协方差矩阵避免奇异值导致的不可逆问题 */ void Ndt3d::BuildVoxels() { // 断言检查：目标点云非空且已初始化 assert(target_ != nullptr); assert(target_->empty() == false); grids_.clear(); // 清空旧的体素网格数据 /// 第一步：将目标点云分配到对应体素中（构建体素-点索引映射） // 生成目标点云的点索引列表（0 ~ 点云大小-1） std::vector<size_t> index(target_->size()); std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; }); // 并行遍历所有点，将点映射到体素并记录索引 std::for_each(index.begin(), index.end(), [this](const size_t& idx) { // 1. 将点坐标转换为体素索引空间（乘以体素大小倒数，等价于除以体素大小） Vec3d pt = ToVec3d(target_->points[idx]) * options_.inv_voxel_size_; // 2. 将浮点型体素坐标转换为整数索引（KeyType：三维整数索引） auto key = CastToInt(pt); // 3. 哈希表中不存在该体素则创建，存在则追加点索引 if (grids_.find(key) == grids_.end()) { grids_.insert({key, {idx}}); } else { grids_[key].idx_.emplace_back(idx); } }); /// 第二步：并行计算每个体素的均值、协方差及信息矩阵（协方差逆） std::for_each(std::execution::par_unseq, grids_.begin(), grids_.end(), [this](auto& v) { // 仅处理点数超过阈值的体素（至少3个点才能计算协方差） if (v.second.idx_.size() > options_.min_pts_in_voxel_) { // 计算体素内点集的均值(mu_)和协方差(sigma_) math::ComputeMeanAndCov( v.second.idx_, // 体素内的点索引列表 v.second.mu_, // 输出：均值（正态分布均值） v.second.sigma_, // 输出：协方差（正态分布协方差） [this](const size_t& idx) { return ToVec3d(target_->points[idx]); } // 点索引到坐标的映射函数 ); // SVD分解协方差矩阵，限制最小奇异值（避免协方差矩阵奇异导致不可逆） // 协方差矩阵是半正定矩阵，SVD分解为 U*Σ*V^T，Σ为奇异值对角矩阵 Eigen::JacobiSVD svd(v.second.sigma_, Eigen::ComputeFullU | Eigen::ComputeFullV); Vec3d lambda = svd.singularValues(); // 提取奇异值（降序排列） // 限制次小/最小奇异值不小于最大奇异值的1e-3，避免数值病态 if (lambda[1] < lambda[0] * 1e-3) { lambda[1] = lambda[0] * 1e-3; } if (lambda[2] < lambda[0] * 1e-3) { lambda[2] = lambda[0] * 1e-3; } // 构建奇异值的逆对角矩阵 Mat3d inv_lambda = Vec3d(1.0 / lambda[0], 1.0 / lambda[1], 1.0 / lambda[2]).asDiagonal(); // 重构协方差的逆（信息矩阵）：info_ = V * Σ⁻¹ * U^T（基于SVD的稳健求逆） // 替代直接求逆：避免协方差矩阵接近奇异时出现NaN/Inf // v.second.info_ = (v.second.sigma_ + Mat3d::Identity() * 1e-3).inverse(); // 备用方案：加正则项 v.second.info_ = svd.matrixV() * inv_lambda * svd.matrixU().transpose(); } }); /// 第三步：删除点数不足的体素（减少无效计算） for (auto iter = grids_.begin(); iter != grids_.end();) { if (iter->second.idx_.size() > options_.min_pts_in_voxel_) { iter++; // 点数足够，保留体素 } else { iter = grids_.erase(iter); // 点数不足，删除体素 } } } /** * @brief NDT配准核心迭代函数（高斯牛顿法） * @details 最小化源点云转换后在目标体素正态分布下的负对数似然函数，通过迭代求解最优位姿； * 支持单中心体素/6邻域体素搜索，并行计算残差和雅可比，提升配准鲁棒性与效率 * @param init_pose 输入：初始位姿（SE3）；输出：配准后的最优位姿 * @return bool 配准是否成功（有效残差数≥阈值、迭代收敛） */ bool Ndt3d::AlignNdt(SE3& init_pose) { LOG(INFO) << "aligning with ndt"; assert(grids_.empty() == false); // 断言：体素网格已构建完成 // 初始化迭代位姿：从输入初始位姿开始 SE3 pose = init_pose; // 若开启去中心：初始平移设为目标中心 - 源中心（消除整体平移偏差） if (options_.remove_centroid_) { pose.translation() = target_center_ - source_center_; LOG(INFO) << "init trans set to " << pose.translation().transpose(); } // 预计算每个源点的最大残差数（单中心：1个；6邻域：7个（中心+6邻域）） int num_residual_per_point = 1; if (options_.nearby_type_ == NearbyType::NEARBY6) { num_residual_per_point = 7; } // 生成源点云的索引列表（用于并行遍历） std::vector<int> index(source_->points.size()); for (int i = 0; i < index.size(); ++i) { index[i] = i; } // 总残差数 = 源点数 × 每个点的残差数 int total_size = index.size() * num_residual_per_point; // 高斯牛顿迭代主循环（最多迭代max_iteration_次） for (int iter = 0; iter < options_.max_iteration_; ++iter) { // 初始化辅助数组（按总残差数分配）： // effect_pts：标记残差是否有效；jacobians：残差对位姿的雅可比矩阵；errors：残差向量；infos：体素信息矩阵 std::vector<bool> effect_pts(total_size, false); std::vector<Eigen::Matrix<double, 3, 6>> jacobians(total_size); std::vector<Vec3d> errors(total_size); std::vector<Mat3d> infos(total_size); /// 第一步：并行计算每个源点在邻域体素中的残差、雅可比和信息矩阵 std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) { // 取出第idx个源点的坐标 auto q = ToVec3d(source_->points[idx]); // 用当前位姿将源点转换到目标点云坐标系 Vec3d qs = pose * q; // 计算转换后点qs对应的体素中心索引 Vec3i key = CastToInt(Vec3d(qs * options_.inv_voxel_size_)); // 遍历预生成的邻域体素偏移（中心/6邻域） for (int i = 0; i < nearby_grids_.size(); ++i) { // 当前邻域体素的索引（中心索引 + 偏移） auto key_off = key + nearby_grids_[i]; // 计算当前残差的全局索引（源点索引×单点点残差数 + 邻域索引） int real_idx = idx * num_residual_per_point + i; // 查找该邻域体素是否存在 auto it = grids_.find(key_off); if (it != grids_.end()) { auto& v = it->second; // 体素数据（均值、信息矩阵等） Vec3d e = qs - v.mu_; // 残差向量：转换后点 - 体素均值 // 计算卡方残差（chi2 = e^T * info_ * e），过滤异常值 double res = e.transpose() * v.info_ * e; if (std::isnan(res) || res > options_.res_outlier_th_) { effect_pts[real_idx] = false; // 残差异常，标记无效 continue; } /// 构建雅可比矩阵 J (3x6)：残差对位姿增量dx的偏导数 /// dx = [ωx, ωy, ωz, tx, ty, tz]^T（前3：旋转增量，后3：平移增量） Eigen::Matrix<double, 3, 6> J; // 旋转部分雅可比 (3x3)：-R * q^×（R为当前旋转矩阵，q^×为源点的反对称矩阵） // 推导：d/dω [R(ω)q + t - μ] = -R * q^× J.block<3, 3>(0, 0) = -pose.so3().matrix() * SO3::hat(q); // 平移部分雅可比 (3x3)：单位矩阵（残差对平移增量的偏导为1） J.block<3, 3>(0, 3) = Mat3d::Identity(); // 保存当前残差、雅可比、信息矩阵，标记为有效 jacobians[real_idx] = J; errors[real_idx] = e; infos[real_idx] = v.info_; effect_pts[real_idx] = true; } else { effect_pts[real_idx] = false; // 邻域体素不存在，标记无效 } } }); /// 第二步：累加Hessian矩阵（H=J^T*info*J）和误差向量（b=-J^T*info*e） double total_res = 0; // 总残差（卡方和） int effective_num = 0; // 有效残差数 Mat6d H = Mat6d::Zero(); // Hessian矩阵（6x6，位姿增量的二阶导数） Vec6d err = Vec6d::Zero();// 误差向量（6x1） for (int idx = 0; idx < effect_pts.size(); ++idx) { if (!effect_pts[idx]) { continue; // 跳过无效残差 } // 累加总残差（卡方值） total_res += errors[idx].transpose() * infos[idx] * errors[idx]; effective_num++; // 累加Hessian矩阵和误差向量 H += jacobians[idx].transpose() * infos[idx] * jacobians[idx]; err += -jacobians[idx].transpose() * infos[idx] * errors[idx]; } // 检查有效残差数是否足够，不足则配准失败 if (effective_num < options_.min_effective_pts_) { LOG(WARNING) << "effective num too small: " << effective_num; return false; } /// 第三步：求解位姿增量并更新位姿 // 高斯牛顿求解：H*dx = err → dx = H⁻¹ * err Vec6d dx = H.inverse() * err; // 更新旋转：用SO3指数映射将旋转增量转换为旋转矩阵，左乘当前旋转 pose.so3() = pose.so3() * SO3::exp(dx.head<3>()); // 更新平移：直接累加平移增量 pose.translation() += dx.tail<3>(); /// 第四步：打印迭代信息，判断收敛 LOG(INFO) << "iter " << iter << " total res: " << total_res << ", eff: " << effective_num << ", mean res: " << total_res / effective_num << ", dxn: " << dx.norm() << ", dx: " << dx.transpose(); // 若设置了真实位姿，计算当前位姿与真实位姿的误差（用于评估） if (gt_set_) { double pose_error = (gt_pose_.inverse() * pose).log().norm(); LOG(INFO) << "iter " << iter << " pose error: " << pose_error; } // 位姿增量范数小于收敛阈值，停止迭代 if (dx.norm() < options_.eps_) { LOG(INFO) << "converged, dx = " << dx.transpose(); break; } } // 将配准后的最终位姿赋值给输入参数（输出结果） init_pose = pose; return true; } /** * @brief 预生成邻域体素的索引偏移量 * @details 根据NearbyType配置，生成需要搜索的邻域体素相对于中心体素的索引偏移， * 存入nearby_grids_，避免每次配准迭代重复计算偏移量，提升效率 */ void Ndt3d::GenerateNearbyGrids() { if (options_.nearby_type_ == NearbyType::CENTER) { // 仅搜索中心体素：偏移量为(0,0,0) nearby_grids_.emplace_back(KeyType::Zero()); } else if (options_.nearby_type_ == NearbyType::NEARBY6) { // 搜索中心体素+6邻域体素（上下/左右/前后） nearby_grids_ = {KeyType(0, 0, 0), // 中心 KeyType(-1, 0, 0), // x-方向 KeyType(1, 0, 0), // x+方向 KeyType(0, 1, 0), // y+方向 KeyType(0, -1, 0), // y-方向 KeyType(0, 0, -1), // z-方向 KeyType(0, 0, 1)}; // z+方向 } }

        由于协方差矩阵的存在，基于高斯-牛顿法的迭代过程中必须施加一些协方差矩阵的约束，这也会使问题能够更快地收敛。

7.2.4方法对比

        点到点ICP、点到面ICP、点到线ICP、NDT与PCL版本的ICP、PCL版本的NDT的对比。

ps：学到这里可以做毕设吗