NDT算法

       上一次我们学习了高翔《自动驾驶与机器人中的SLAM技术》中的三维ICP算法，其中包括点对点、点对线、点对面的ICP算法，本次博客学习NDT算法的源码。

       NDT算法与ICP算法的最大不同之处，在我看来是NDT考虑了均值和方差这两个局部统计量。

       从最后的求解方法来看，NDT采用了加权最小二乘问题的高斯-牛顿法，和ICP算法的最明显区别是多了权重分布。从高翔书中的测试结果来看，NDT的收敛速度稍弱于点对面ICP算法，但是精度高于点对面ICP算法，表现最好。

下面我们来看NDT的源码：

ndt_3d.h 以下为头文件 ------------------------------------------------------------------------------------------------

//
// Created by xiang on 2022/7/14.
//

#ifndef SLAM_IN_AUTO_DRIVING_NDT_3D_H
#define SLAM_IN_AUTO_DRIVING_NDT_3D_H

#include "common/eigen_types.h"
#include "common/point_types.h"

namespace sad {

/**
 * 3D 形式的NDT
 */

//采用栅格法最近邻划分体素
class Ndt3d {
   public:
    enum class NearbyType {
        CENTER,   // 只考虑中心
        NEARBY6,  // 上下左右前后
    };

    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;  // 体素的索引
    struct VoxelData {
        VoxelData() {}
        VoxelData(size_t id) { idx_.emplace_back(id); }

        std::vector idx_;      // 点云中点的索引
        Vec3d mu_ = Vec3d::Zero();     // 均值
        Mat3d sigma_ = Mat3d::Zero();  // 协方差
        Mat3d info_ = Mat3d::Zero();   // 协方差之逆
    };

    Ndt3d() {
        options_.inv_voxel_size_ = 1.0 / options_.voxel_size_;
        GenerateNearbyGrids();
    }

    Ndt3d(Options options) : options_(options) {
        options_.inv_voxel_size_ = 1.0 / options_.voxel_size_;
        GenerateNearbyGrids();
    }

//

Ndt3d 类有两个构造函数：

• 默认构造函数 Ndt3d()：它使用默认的 Options 对象来初始化 options_ 成员变量，并执行一些初始化步骤，如计算体素大小的倒数和生成邻近网格。

• 参数化构造函数 Ndt3d(Options options)：它允许用户在创建 Ndt3d 对象时提供一个自定义的 Options 对象，这使得对象的创建更加灵活。这个构造函数也执行与默认构造函数相同的初始化步骤。

    /// 设置目标的Scan
    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();

//lambda表达式接受两个参数：当前的累加值 c 和点云中的下一个点 pt，并返回新的累加值。
    }

    /// 设置被配准的Scan
    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();
    }

    void SetGtPose(const SE3& gt_pose) {
        gt_pose_ = gt_pose;
        gt_set_ = true;
    }

    /// 使用gauss-newton方法进行ndt配准
    bool AlignNdt(SE3& init_pose);

   private:
    void BuildVoxels();

    /// 根据最近邻的类型，生成附近网格
    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_;

    std::unordered_map> grids_;  // 栅格数据
    std::vector nearby_grids_;                          // 附近的栅格
};

}  // namespace sad

#endif  // SLAM_IN_AUTO_DRIVING_NDT_3D_H

在头文件中有几个比较重要的函数定义，GenerateNearbyGrids()，BuildVoxels()， bool AlignNdt(SE3& init_pose)，可以在ndt_3d.cc中查看他们的具体实现。

//
// Created by xiang on 2022/7/14.
//

#include "ndt_3d.h"
#include "common/lidar_utils.h"
#include "common/math_utils.h"

#include
#include
#include

namespace sad {

void Ndt3d::BuildVoxels() {

//在程序创建了 Ndt3d 类的对象之后，立即调用 SetTarget 函数来设置目标点云?
    assert(target_ != nullptr);

//assert 语句用于确保 target_ 指针不是空指针（nullptr）。
    assert(target_->empty() == false);

//assert 语句用于检查 target_ 指向的点云对象是否不为空。
    grids_.clear();//清空栅格数据

    /// 分配体素
    std::vector 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) {

//[this] 的作用是捕获当前对象的指针，使得在 lambda 表达式内部可以访问 target_ 和 options_ 这两个成员变量。
        Vec3d pt = ToVec3d(target_->points[idx]) * options_.inv_voxel_size_;//点云坐标除以分辨率
        auto key = CastToInt(pt);//将浮点数向量转换为整数向量
        if (grids_.find(key) == grids_.end()) {//grids_.end()是空的，判断如果key不在grid_的索引中
            grids_.insert({key, {idx}});//向grid_添加一个键值对 （对应栅格索引：点云索引）
        } else {
            grids_[key].idx_.emplace_back(idx);
        }
    });

    /// 计算每个体素中的均值和协方差
    std::for_each(std::execution::par_unseq, grids_.begin(), grids_.end(), [this](auto& v) {
        if (v.second.idx_.size() > options_.min_pts_in_voxel_) {
            // 要求至少有３个点
            math::ComputeMeanAndCov(v.second.idx_, v.second.mu_, v.second.sigma_,
                                    [this](const size_t& idx) { return ToVec3d(target_->points[idx]); });

void ComputeMeanAndCov(const C& data, Eigen::Matrix& mean, Eigen::Matrix& cov,
                       Getter&& getter) {
//&& 表示这是一个右值引用，它允许 getter 参数以右值的方式传递给函数。
Getter获取数据函数, 接收一个容器内数据类型，返回一个Eigen::Matrix 矢量类型
    using D = Eigen::Matrix;
    using E = Eigen::Matrix;
    size_t len = data.size();
    assert(len > 1);

    // clang-format off
    mean = std::accumulate(data.begin(), data.end(), Eigen::Matrix::Zero().eval(),[&getter](const D& sum, const auto& data) -> D { return sum + getter(data); }) / len;
    cov = std::accumulate(data.begin(), data.end(), E::Zero().eval(),[&mean, &getter](const E& sum, const auto& data) -> E {D v = getter(data) - mean;return sum + v * v.transpose();}) / (len - 1);
    // clang-format on
}

            // SVD 检查最大与最小奇异值，限制最小奇异值

//采用分解协方差矩阵的方法求协方差矩阵的逆。

            Eigen::JacobiSVD svd(v.second.sigma_, Eigen::ComputeFullU | Eigen::ComputeFullV);
            Vec3d lambda = svd.singularValues();//用来记录奇异值
            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();//计算信息矩阵，信息矩阵是一个对角阵。

            // v.second.info_ = (v.second.sigma_ + Mat3d::Identity() * 1e-3).inverse();  // 避免出nan

//

• Mat3d::Identity() 创建一个 3x3 的单位矩阵，它是对角线上全是 1，其他位置全是 0 的方阵。
• * 1e-3 将单位矩阵的每个元素乘以 0.001，得到一个小的对角矩阵。这个操作通常用于添加一个微小的正值到协方差矩阵的对角线上，以确保协方差矩阵是正定的。

            v.second.info_ = svd.matrixV() * inv_lambda * svd.matrixU().transpose();

//v.second.info_协方差之逆
        }
    });

    /// 删除点数不够的
    for (auto iter = grids_.begin(); iter != grids_.end();) {

//这是一个 for 循环，使用迭代器 iter 遍历 grids_ 容器。循环的条件是迭代器 iter 不等于 grids_.end()，即迭代器指向容器的末尾。
        if (iter->second.idx_.size() > options_.min_pts_in_voxel_) {
            iter++;
        } else {
            iter = grids_.erase(iter);

//grids_.erase(iter) 调用将删除当前迭代器指向的元素，并返回一个指向下一个元素的迭代器。这个迭代器赋值给 iter，这样循环可以继续检查下一个元素。
        }
    }
}

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();
    }

//为什么remove_centroid_的初值是false？

//将 remove_centroid_ 的初始值设置为 false 是为了提供默认的灵活性和安全性，同时允许用户根据具体需求调整算法的行为?(ai的回复）

    // 对点的索引，预先生成
    int num_residual_per_point = 1;
    if (options_.nearby_type_ == NearbyType::NEARBY6) {
        num_residual_per_point = 7;
    }

    std::vector index(source_->points.size());
    for (int i = 0; i < index.size(); ++i) {
        index[i] = i;
    }

    // 我们来写一些并发代码
    int total_size = index.size() * num_residual_per_point;

    for (int iter = 0; iter < options_.max_iteration_; ++iter) {
        std::vector effect_pts(total_size, false);
        std::vector> jacobians(total_size);
        std::vector errors(total_size);
        std::vector infos(total_size);

        // gauss-newton 迭代
        // 最近邻，可以并发
        std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {

//[&] 是一个 lambda 表达式的捕获列表，它告诉编译器在 lambda 表达式内部使用的所有外部变量都应该是引用捕获的。
            auto q = ToVec3d(source_->points[idx]);
            Vec3d qs = pose * q;  // 转换之后的q

            // 计算qs所在的栅格以及它的最近邻栅格
            Vec3i key = CastToInt(Vec3d(qs * options_.inv_voxel_size_));

            for (int i = 0; i < nearby_grids_.size(); ++i) {

//nearby_grids_ = {KeyType(0, 0, 0), KeyType(-1, 0, 0), KeyType(1, 0, 0), KeyType(0, 1, 0),

KeyType(0, -1, 0), KeyType(0, 0, -1), KeyType(0, 0, 1)};

                auto key_off = key + nearby_grids_[i];

//key 是当前点的体素索引。key_off 是计算得到的新体素索引，它是当前点的索引加上周围的偏移量

                auto it = grids_.find(key_off);
                int real_idx = idx * num_residual_per_point + i;

//

• idx 是当前点在点云中的索引。
• num_residual_per_point 是每个点对应的残差数量。
• real_idx 是计算得到的残差在残差数组中的索引。

                if (it != grids_.end()) {
                    auto& v = it->second;  // voxel
                    Vec3d e = qs - v.mu_;

                    // check chi2 th
                    double res = e.transpose() * v.info_ * e;
                    if (std::isnan(res) || res > options_.res_outlier_th_) {
                        effect_pts[real_idx] = false;
                        continue;
                    }

                    // build residual
                    Eigen::Matrix J;
                    J.block<3, 3>(0, 0) = -pose.so3().matrix() * SO3::hat(q);
                    J.block<3, 3>(0, 3) = Mat3d::Identity();

                    jacobians[real_idx] = J;
                    errors[real_idx] = e;
                    infos[real_idx] = v.info_;

//std::vector effect_pts(total_size, false);
//        std::vector> jacobians(total_size);
 //       std::vector errors(total_size);
   //     std::vector infos(total_size);这是前面的定义，我重新放一下
                    effect_pts[real_idx] = true;
                } else {
                    effect_pts[real_idx] = false;
                }
            }
        });

        // 累加Hessian和error,计算dx
        // 原则上可以用reduce并发，写起来比较麻烦，这里写成accumulate

//后面就和ICP算法一致了，只是求解的时候，中间多乘了协方差矩阵的逆。不过多说明了。
        double total_res = 0;
        int effective_num = 0;

        Mat6d H = Mat6d::Zero();//6*6
        Vec6d err = Vec6d::Zero();//6*1

        for (int idx = 0; idx < effect_pts.size(); ++idx) {
            if (!effect_pts[idx]) {
                continue;
            }

            total_res += errors[idx].transpose() * infos[idx] * errors[idx];
            // chi2.emplace_back(errors[idx].transpose() * infos[idx] * errors[idx]);
            effective_num++;

            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;
        }

        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()
                  << ", dx: " << dx.transpose();

        // std::sort(chi2.begin(), chi2.end());
        // LOG(INFO) << "chi2 med: " << chi2[chi2.size() / 2] << ", .7: " << chi2[chi2.size() * 0.7]
        //           << ", .9: " << chi2[chi2.size() * 0.9] << ", max: " << chi2.back();

        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;
}

void Ndt3d::GenerateNearbyGrids() {
    if (options_.nearby_type_ == NearbyType::CENTER) {
        nearby_grids_.emplace_back(KeyType::Zero());
    } else if (options_.nearby_type_ == NearbyType::NEARBY6) {
        nearby_grids_ = {KeyType(0, 0, 0),  KeyType(-1, 0, 0), KeyType(1, 0, 0), KeyType(0, 1, 0),
                         KeyType(0, -1, 0), KeyType(0, 0, -1), KeyType(0, 0, 1)};
    }
}

}  // namespace sad

补充右值引用：

在 C++ 中，左值引用（lvalue reference）和右值引用（rvalue reference）是两种不同类型的引用，它们在语义和使用场景上有所不同。以下是它们的主要区别：

1. 定义和用途：

   • 左值引用：它们绑定到左值（lvalues）上，即那些具有持久存储的对象。左值引用通常用于访问和修改已经存在的对象。
   • 右值引用：它们绑定到右值（rvalues）上，即那些临时的、不具有持久存储的对象，通常是表达式的结果。右值引用主要用于移动语义和完美转发。

2. 语法：

   • 左值引用：使用 type& 声明，例如 int& a;。
   • 右值引用：使用 type&& 声明，例如 int&& b;。

3. 绑定对象：

   • 左值引用：必须绑定到一个持久的对象上，不能绑定到临时对象或表达式的结果上。
   • 右值引用：可以绑定到临时对象或表达式的结果上，这是它们的主要用例。

4. 移动语义：

   • 左值引用：不支持移动语义，因为它们与持久对象绑定。
   • 右值引用：支持移动语义，允许将资源（如内存、文件句柄等）从一个对象转移到另一个对象，而不需要复制。

5. 完美转发：

   • 左值引用：不能用于完美转发，因为它们总是引用左值。
   • 右值引用：可以用于完美转发，这意味着它们可以保留参数的左值或右值性质，并将这些性质传递给其他函数。

6. 生命周期：

   • 左值引用：它们的生命周期与它们引用的对象相同，因此它们不能比它们引用的对象活得更久。
   • 右值引用：通常用于临时对象，但也可以绑定到持久对象上。在移动语义中，右值引用可以延长临时对象的生命周期。

7. 可变性：

   • 左值引用：可以修改它们引用的对象。
   • 右值引用：通常用于移动操作，不用于修改对象。

8. 常量性：

   • 左值引用：可以是常量引用（const type&），用于防止修改引用的对象。
   • 右值引用：也可以是常量引用（const type&&），但在移动操作中通常不是必需的。

9. 函数参数：

   • 左值引用：通常用于函数参数，以允许修改传递给函数的对象。
   • 右值引用：用于函数参数，以允许移动传递给函数的临时对象。

10. 返回值：

    • 左值引用：函数可以返回左值引用，但通常不推荐返回局部对象的引用，因为这可能导致悬挂引用。
    • 右值引用：函数可以返回右值引用，这在移动返回值优化（move return optimization）中非常有用。

补充：奇异值分解求协方差矩阵的逆的原理是什么？

奇异值分解（SVD）是一种将矩阵分解为一系列正交矩阵和对角矩阵的数学方法。对于协方差矩阵 ΣΣ，SVD 可以表示为：

Σ=UΣVTΣ=UΣVT

其中：

• UU 和 VV 是正交矩阵。
• ΣΣ 是一个对角矩阵，其对角线上的元素是 ΣΣ 的奇异值。

协方差矩阵的逆可以通过以下步骤使用 SVD 来求解：

1. 奇异值分解： 首先对协方差矩阵 ΣΣ 进行奇异值分解，得到 UU, ΣΣ, 和 VTVT。

2. 计算逆矩阵： 协方差矩阵的逆可以表示为：

   Σ−1=VΣ−1UTΣ−1=VΣ−1UT

   其中 Σ−1Σ−1 是一个对角矩阵，其对角线上的元素是 ΣΣ 对应奇异值的倒数。

3. 处理零或接近零的奇异值： 如果协方差矩阵不是满秩的，或者某些奇异值非常小，直接求倒数可能会导致数值不稳定。在这种情况下，可以设置一个阈值，将小于该阈值的奇异值视为零，或者给它们一个非零的最小值，以避免除以零或非常小的数。

4. 计算信息矩阵： 使用修改后的奇异值，计算信息矩阵 Σ−1Σ−1，然后将其乘以 VV 和 UTUT 来得到协方差矩阵的逆。

使用 SVD 求协方差矩阵的逆的原理是基于 SVD 能够将矩阵分解为一系列正交变换和一个对角线上包含所有奇异值的对角矩阵。这种方法在数值上通常比直接求逆更稳定，尤其是当矩阵接近奇异或病态时。