KISS-ICP核心代码解析

---

---

1.核心函数

该RegisterFrame函数的主要功能是对输入的点云帧进行配准。它将输入的点云帧与体素哈希图进行匹配，以初始位姿估计为起点，通过迭代最近点（ICP）算法来计算从初始位姿到最终配准位姿的变换矩阵。若体素哈希图为空，则直接返回初始位姿估计；否则，利用初始位姿变换源点云，在多次迭代中获取对应点对、构建并求解线性系统得到位姿增量，更新源点云和总变换矩阵，当位姿增量小于阈值时提前终止迭代，最后将初始位姿估计与 ICP 迭代得到的变换矩阵相乘得到最终配准变换矩阵并返回。

// 该函数用于对帧进行配准，将输入的点云帧与体素哈希图进行匹配，计算出从初始位姿到最终配准位姿的变换矩阵
// 参数:
// frame: 输入的点云帧，由一系列三维点构成
// voxel_map: 体素哈希图，用于存储场景中的点云信息
// initial_guess: 初始的位姿估计，是一个SE(3)变换矩阵
// max_correspondence_distance: 最大对应距离，用于筛选匹配点对
// kernel: 核函数的参数，用于鲁棒估计
// 返回值:
// 最终的配准变换矩阵，将输入的点云帧从初始位姿变换到最终配准位姿
Sophus::SE3d RegisterFrame(const std::vector<Eigen::Vector3d> &frame,
                           const VoxelHashMap &voxel_map,
                           const Sophus::SE3d &initial_guess,
                           double max_correspondence_distance,
                           double kernel) {
    // 若体素哈希图为空，直接返回初始的位姿估计
    if (voxel_map.Empty()) return initial_guess;

    // Equation (9)
    // 创建一个源点云的副本，初始时源点云与输入的点云帧相同
    std::vector<Eigen::Vector3d> source = frame;
    // 利用初始的位姿估计对源点云进行变换
    TransformPoints(initial_guess, source);

    // ICP-loop
    // 初始化ICP迭代的变换矩阵为单位矩阵
    Sophus::SE3d T_icp = Sophus::SE3d();
    // 进行ICP迭代，最大迭代次数为MAX_NUM_ITERATIONS_
    for (int j = 0; j < MAX_NUM_ITERATIONS_; ++j) 
    {
        // Equation (10)
        // 从体素哈希图中获取源点云与目标点云的对应点对
        // src是源点云的对应点，tgt是目标点云的对应点
        const auto &[src, tgt] = voxel_map.GetCorrespondences(source, max_correspondence_distance);
        // Equation (11)
        // 构建线性系统，计算雅克比矩阵的转置乘以雅克比矩阵（JTJ）和雅克比矩阵的转置乘以残差（JTr）
        const auto &[JTJ, JTr] = BuildLinearSystem(src, tgt, kernel);
        // 求解线性系统，得到位姿的增量dx
        const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr);
        // 利用指数映射将位姿增量dx转换为SE(3)变换矩阵
        const Sophus::SE3d estimation = Sophus::SE3d::exp(dx);
        // Equation (12)
        // 利用新的估计变换矩阵对源点云进行更新
        TransformPoints(estimation, source);
        // 更新迭代过程中的总变换矩阵
        T_icp = estimation * T_icp;
        // 终止条件判断
        // 若位姿增量的范数小于阈值ESTIMATION_THRESHOLD_，则停止迭代
        if (dx.norm() < ESTIMATION_THRESHOLD_) break;
    }
    // 输出最终的配准变换矩阵，将初始位姿估计与ICP迭代得到的变换矩阵相乘
    return T_icp * initial_guess;
}

1.GetCorrespondences函数

        const auto &[src, tgt] = voxel_map.GetCorrespondences(source, max_correspondence_distance);

// VoxelHashMap 类的成员函数，用于获取给定点集的对应点对
// 参数:
// points: 输入的点集，每个点是一个三维向量
// max_correspondance_distance: 最大对应距离，用于筛选有效对应点对
// 返回值:
// 一个元组，包含两个向量，分别是源点集和对应的目标点集
VoxelHashMap::Vector3dVectorTuple VoxelHashMap::GetCorrespondences(
    const Vector3dVector &points, double max_correspondance_distance) const {
    // Lambda 函数，用于获取一个点的最近邻点
    // 该函数可能需要重构以提高代码复用性
    auto GetClosestNeighboor = [&](const Eigen::Vector3d &point) {
        // 计算点所在体素的索引
        auto kx = static_cast<int>(point[0] / voxel_size_);
        auto ky = static_cast<int>(point[1] / voxel_size_);
        auto kz = static_cast<int>(point[2] / voxel_size_);

        // 存储周围 27 个体素的容器
        std::vector<Voxel> voxels;
        // 预先分配空间，避免多次内存重新分配
        voxels.reserve(27);
        // 遍历周围 27 个体素
        for (int i = kx - 1; i < kx + 1 + 1; ++i) {
            for (int j = ky - 1; j < ky + 1 + 1; ++j) {
                for (int k = kz - 1; k < kz + 1 + 1; ++k) {
                    // 将体素添加到容器中
                    voxels.emplace_back(i, j, k);
                }
            }
        }

        // 定义存储邻域点的向量类型
        using Vector3dVector = std::vector<Eigen::Vector3d>;
        // 存储邻域点的向量
        Vector3dVector neighboors;
        // 预先分配空间，避免多次内存重新分配
        neighboors.reserve(27 * max_points_per_voxel_);

        // 遍历所有体素，查找其中的点
        std::for_each(voxels.cbegin(), voxels.cend(), [&](const auto &voxel) 
        {
            // 在体素哈希表中查找当前体素
            auto search = map_.find(voxel);
            // 如果找到该体素
            if (search != map_.end()) {
                // 获取该体素中的点集
                const auto &points = search->second.points;
                // 如果点集不为空
                if (!points.empty()) {
                    // 将体素中的点添加到邻域点向量中
                    for (const auto &point : points) {
                        neighboors.emplace_back(point);
                    }
                }
            }
        });

        // 存储最近邻点
        Eigen::Vector3d closest_neighbor;
        // 初始化最近邻点的最小距离为最大值
        double closest_distance2 = std::numeric_limits<double>::max();

        // 遍历所有邻域点，找到最近邻点
        std::for_each(neighboors.cbegin(), neighboors.cend(), [&](const auto &neighbor)
        {
            // 计算当前邻域点与目标点的距离的平方
            double distance = (neighbor - point).squaredNorm();
            // 如果当前距离小于最小距离
            if (distance < closest_distance2) {
                // 更新最近邻点
                closest_neighbor = neighbor;
                // 更新最小距离
                closest_distance2 = distance;
            }
        });

        // 返回最近邻点
        return closest_neighbor;
    };

    // 定义点集迭代器类型
    using points_iterator = std::vector<Eigen::Vector3d>::const_iterator;

    // 使用 TBB（Threading Building Blocks）进行并行计算和归约
    const auto [source, target] = tbb::parallel_reduce(
        // 迭代范围，从输入点集的开始到结束
        tbb::blocked_range<points_iterator>{points.cbegin(), points.cend()},
        // 初始结果，预先分配与输入点集相同大小的空间
        ResultTuple(points.size()),
        // 第一个 lambda 函数：并行计算
        [max_correspondance_distance, &GetClosestNeighboor](
            const tbb::blocked_range<points_iterator> &r, ResultTuple res) -> ResultTuple 
        {
            // 解包结果元组，获取源点集和目标点集
            auto &[src, tgt] = res;
            // 预先分配空间，避免多次内存重新分配
            src.reserve(r.size());
            tgt.reserve(r.size());
            // 遍历当前迭代范围的所有点
            for (const auto &point : r) {
                // 获取当前点的最近邻点
                Eigen::Vector3d closest_neighboors = GetClosestNeighboor(point);
                // 如果最近邻点与当前点的距离小于最大对应距离
                if ((closest_neighboors - point).norm() < max_correspondance_distance) {
                    // 将当前点添加到源点集中
                    src.emplace_back(point);
                    // 将最近邻点添加到目标点集中
                    tgt.emplace_back(closest_neighboors);
                }
            }
            // 返回更新后的结果
            return res;
        },
        // 第二个 lambda 函数：并行归约
        [](ResultTuple a, const ResultTuple &b) -> ResultTuple 
        {
            // 解包结果元组，获取源点集和目标点集
            auto &[src, tgt] = a;
            const auto &[srcp, tgtp] = b;
            // 将另一个结果的源点集添加到当前结果的源点集中
            src.insert(src.end(),  //
                       std::make_move_iterator(srcp.begin()), std::make_move_iterator(srcp.end()));
            // 将另一个结果的目标点集添加到当前结果的目标点集中
            tgt.insert(tgt.end(),  //
                       std::make_move_iterator(tgtp.begin()), std::make_move_iterator(tgtp.end()));
            // 返回合并后的结果
            return a;
        });

    // 返回最终的源点集和目标点集的元组
    return std::make_tuple(source, target);
}

1. 确定点的体素邻域
   对于任意点 $p\in P$，其所在体素为 $v=f_{\text{voxel}}(p)=(i,j,k)$，周围 27 个体素集合为：
   V 27 ( v ) = { ( i ′ , j ′ , k ′ )   ∣   i − 1 ≤ i ′ ≤ i + 1 ,   j − 1 ≤ j ′ ≤ j + 1 ,   k − 1 ≤ k ′ ≤ k + 1 } V_{27}(v) = \left\{ (i', j', k') \,\big|\, i - 1 \leq i' \leq i + 1,\, j - 1 \leq j' \leq j + 1,\, k - 1 \leq k' \leq k + 1 \right\} V27​(v)={(i′,j′,k′) ​i−1≤i′≤i+1,j−1≤j′≤j+1,k−1≤k′≤k+1}

        auto kx = static_cast<int>(point[0] / voxel_size_);
        auto ky = static_cast<int>(point[1] / voxel_size_);
        auto kz = static_cast<int>(point[2] / voxel_size_);
        std::vector<Voxel> voxels;
        voxels.reserve(27);
        for (int i = kx - 1; i < kx + 1 + 1; ++i) {
            for (int j = ky - 1; j < ky + 1 + 1; ++j) {
                for (int k = kz - 1; k < kz + 1 + 1; ++k) {
                    voxels.emplace_back(i, j, k);
                }
            }
        }

2. 获取邻域点集
   邻域点集$N(p)={\bigcup }_{v^{\prime }\in V_{27}(v)}M(v^{\prime })$，即通过查找周围体素内的所有点形成，用公式表示为：
   $$N(p)=\bigcup _{v^{\prime }\in V_{27}(v)}M(v^{\prime })$$
3. 计算最近邻点
   最近邻点 $c(p)=\arg {\min }_{q\in N(p)}\Vert p-q{\Vert }^2$，通过比较点$p$ 与邻域点集$N(p)$中各点的欧氏距离平方确定，公式为：
   $$c(p)=\arg \underset{q\in N(p)}{\min }\Vert p-q{\Vert }^2$$

        Eigen::Vector3d closest_neighbor;
        double closest_distance2 = std::numeric_limits<double>::max();
        std::for_each(neighboors.cbegin(), neighboors.cend(), [&](const auto &neighbor)
        {
            double distance = (neighbor - point).squaredNorm();
            if (distance < closest_distance2) {
                closest_neighbor = neighbor;
                closest_distance2 = distance;
            }
        });

4. 筛选对应点对
   满足距离条件的对应点对集合为 S = { ( p , c ( p ) )   ∣   p ∈ P ,   ∥ p − c ( p ) ∥ < d max } S = \left\{ (p, c(p)) \,\big|\, p \in P,\, \| p - c(p) \| < d_{\text{max}} \right\} S={(p,c(p)) ​p∈P,∥p−c(p)∥<dmax​}，进一步拆分为源点集$S_{\text{src}}=\{p|(p,c(p))\in S\}$和目标点集$S_{\text{tgt}}=\{c(p)|(p,c(p))\in S\}$，对应公式为：
   S = { ( p , c ( p ) )   ∣   p ∈ P ,   ∥ p − c ( p ) ∥ < d max } S = \left\{ (p, c(p)) \,\big|\, p \in P,\, \| p - c(p) \| < d_{\text{max}} \right\} S={(p,c(p)) ​p∈P,∥p−c(p)∥<dmax​}
   $$S_{\text{src}}=\{p|(p,c(p))\in S\},S_{\text{tgt}}=\{c(p)|(p,c(p))\in S\}$$

        tbb::blocked_range<points_iterator>{points.cbegin(), points.cend()},
        // Identity
        ResultTuple(points.size()),
        // 1st lambda: Parallel computation
        [max_correspondance_distance, &GetClosestNeighboor](
            const tbb::blocked_range<points_iterator> &r, ResultTuple res) -> ResultTuple 
        {
            auto &[src, tgt] = res;
            src.reserve(r.size());
            tgt.reserve(r.size());
            for (const auto &point : r) {
                Eigen::Vector3d closest_neighboors = GetClosestNeighboor(point);
                if ((closest_neighboors - point).norm() < max_correspondance_distance) {
                    src.emplace_back(point);
                    tgt.emplace_back(closest_neighboors);
                }
            }
            return res;
        },
        // 2nd lambda: Parallel reduction
        [](ResultTuple a, const ResultTuple &b) -> ResultTuple 
        {
            auto &[src, tgt] = a;
            const auto &[srcp, tgtp] = b;
            src.insert(src.end(),  //
                       std::make_move_iterator(srcp.begin()), std::make_move_iterator(srcp.end()));
            tgt.insert(tgt.end(),  //
                       std::make_move_iterator(tgtp.begin()), std::make_move_iterator(tgtp.end()));
            return a;
        });

2.BuildLinearSystem函数

// 定义一个函数 BuildLinearSystem，用于构建线性系统
// 输入参数：
// source: 源点云的点集，每个点是三维向量
// target: 目标点云的点集，每个点是三维向量
// kernel: 核函数的参数
// 返回值：一个元组，包含 6x6 的矩阵 JTJ 和 6 维的向量 JTr
std::tuple<Eigen::Matrix6d, Eigen::Vector6d> BuildLinearSystem(
    const std::vector<Eigen::Vector3d> &source,
    const std::vector<Eigen::Vector3d> &target,
    double kernel) {
    // 定义一个 lambda 函数 compute_jacobian_and_residual，用于计算雅可比矩阵和残差
    auto compute_jacobian_and_residual = [&](auto i) {
        // 计算第 i 个点的残差，即源点减去目标点
        const Eigen::Vector3d residual = source[i] - target[i];
        // 定义一个 3x6 的雅可比矩阵 J_r
        Eigen::Matrix3_6d J_r;
        // 将 J_r 的左上角 3x3 子矩阵设置为单位矩阵
        J_r.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
        // 将 J_r 的右上角 3x3 子矩阵设置为 -1 乘以源点的反对称矩阵
        J_r.block<3, 3>(0, 3) = -1.0 * Sophus::SO3d::hat(source[i]);
        // 返回雅可比矩阵和残差组成的元组
        return std::make_tuple(J_r, residual);
    };

    // 使用 tbb::parallel_reduce 进行并行计算
    // 并行计算 JTJ 和 JTr
    const auto &[JTJ, JTr] = tbb::parallel_reduce(
        // 定义并行计算的范围，从 0 到 source 的大小
        tbb::blocked_range<size_t>{0, source.size()},
        // 初始化结果元组
        ResultTuple(),
        // 第一个 lambda 函数，用于并行计算每个块的 JTJ 和 JTr
        [&](const tbb::blocked_range<size_t> &r, ResultTuple J) -> ResultTuple {
            // 定义一个 lambda 函数 Weight，用于计算权重
            auto Weight = [&](double residual2) {
                // 计算权重，使用核函数
                return square(kernel) / square(kernel + residual2);
            };
            // 解包结果元组，得到 JTJ_private 和 JTr_private
            auto &[JTJ_private, JTr_private] = J;
            // 遍历当前块的每个点
            for (auto i = r.begin(); i < r.end(); ++i) {
                // 调用 compute_jacobian_and_residual 函数，计算雅可比矩阵和残差
                const auto &[J_r, residual] = compute_jacobian_and_residual(i);
                // 计算当前点的权重
                const double w = Weight(residual.squaredNorm());
                // 更新 JTJ_private，使用 noalias 避免不必要的临时变量
                JTJ_private.noalias() += J_r.transpose() * w * J_r;
                // 更新 JTr_private，使用 noalias 避免不必要的临时变量
                JTr_private.noalias() += J_r.transpose() * w * residual;
            }
            // 返回更新后的结果元组
            return J;
        },
        // 第二个 lambda 函数，用于合并每个块的 JTJ 和 JTr
        [&](ResultTuple a, const ResultTuple &b) -> ResultTuple { return a + b; });

    // 返回最终的 JTJ 和 JTr 组成的元组
    return std::make_tuple(JTJ, JTr);
}

ICP的高斯牛顿解法公式推导

设源点云为 $\mathbf{P}=\{{\mathbf{p}}_i{\}}_{i=1}^{N_p}$，目标点云为 $\mathbf{Q}=\{{\mathbf{q}}_i{\}}_{i=1}^{N_q}$。
目标：寻找刚体变换 $\mathbf{T}=[\mathbf{R},\mathbf{t}]$（$\mathbf{R}\in \text{SO(3)}$ 为旋转矩阵，$\mathbf{t}\in {\mathbb{R}}^3$ 为平移向量），使变换后的源点云与目标点云对齐，最小化误差函数：
$$E(\mathbf{T})=\sum _{i=1}^{N_p}\Vert \mathbf{R}{\mathbf{p}}_i+\mathbf{t}-{\mathbf{q}}_{c_i}{\Vert }^2$$
其中 ${\mathbf{q}}_{c_i}$ 是 ${\mathbf{p}}_i$ 变换后在 $\mathbf{Q}$ 中的最近邻点。

• 线性化及雅可比
  假设当前估计变换为 ${\mathbf{T}}_k=({\mathbf{R}}_k,{\mathbf{t}}_k)$，引入增量变换 $\Delta \mathbf{x}=[\Delta \mathbf{\theta },\Delta \mathbf{t}{]}^T$（$\Delta \mathbf{\theta }$ 为旋转向量，$\Delta \mathbf{t}$ 为平移向量）。
  利用泰勒展开线性化变换：
  $$\mathbf{R}{\mathbf{p}}_i+\mathbf{t}\approx {\mathbf{R}}_k{\mathbf{p}}_i+{\mathbf{t}}_k+{\mathbf{J}}_i\Delta \mathbf{x}$$
  雅可比矩阵 ${\mathbf{J}}_i\in {\mathbb{R}}^{3\times 6}$ 定义为：
  $${\mathbf{J}}_i=\left[\begin{array}{cc}-[{\mathbf{R}}_k{\mathbf{p}}_i{]}_{\times }& {\mathbf{I}}_{3\times 3}\end{array}\right]$$

其中 $[\mathbf{v}{]}_{\times }$ 是向量 $\mathbf{v}$ 的反对称矩阵：
[ v ] × = [ 0 − v 3 v 2 v 3 0 − v 1 − v 2 v 1 0 ] [\mathbf{v}]_{\times} = \begin{bmatrix} 0 & -v_3 & v_2 \\ v_3 & 0 & -v_1 \\ -v_2 & v_1 & 0 \end{bmatrix} [v]×​= ​0v3​−v2​​−v3​0v1​​v2​−v1​0​ ​
这里已经在RegisterFrame里面乘上了转换矩阵，所以不需要$[{\mathbf{R}}_k{\mathbf{p}}_i]$

    auto compute_jacobian_and_residual = [&](auto i) {
        const Eigen::Vector3d residual = source[i] - target[i];
        Eigen::Matrix3_6d J_r;
        J_r.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
        J_r.block<3, 3>(0, 3) = -1.0 * Sophus::SO3d::hat(source[i]);
        return std::make_tuple(J_r, residual);
    };

3. 高斯牛顿法求解

将线性化后的误差函数表示为：
$$E(\Delta \mathbf{x})\approx \sum _{i=1}^{N_p}\Vert {\mathbf{e}}_i+{\mathbf{J}}_i\Delta \mathbf{x}{\Vert }^2,{\mathbf{e}}_i={\mathbf{R}}_k{\mathbf{p}}_i+{\mathbf{t}}_k-{\mathbf{q}}_{c_i}$$
对 $\Delta \mathbf{x}$ 求导并令导数为零，得到正规方程：
$$\mathbf{H}\Delta \mathbf{x}=\mathbf{b}$$
其中：
$$\mathbf{H}=\sum _{i=1}^{N_p}{\mathbf{J}}_i^T{\mathbf{J}}_i,\mathbf{b}=-\sum _{i=1}^{N_p}{\mathbf{J}}_i^T{\mathbf{e}}_i$$

        const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr);
        const Sophus::SE3d estimation = Sophus::SE3d::exp(dx);