自动驾驶与机器人中的SLAM技术--第七章（7.1-7.3）--3D SLAM

weixin_47046769

已于 2024-08-28 14:33:48 修改

阅读量670

点赞数 7

分类专栏：自动驾驶与机器人中的SLAM技术文章标签：自动驾驶机器人 3d

于 2024-08-18 21:01:20 首次发布

本文链接：https://blog.csdn.net/weixin_47046769/article/details/141264321

版权

自动驾驶与机器人中的SLAM技术专栏收录该内容

6 篇文章 0 订阅

订阅专栏

主函数中的启动函数——test_icp.cc

icp_3d.cc中的点到面icp函数

7.2.3 NDT（Normal Distribution Transform）方法

测试主函数——test_ndt_lo.cc

incremental_ndt_lo.cc——增量式NDT激光雷达里程计

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

7.1.1 机械旋转式激光雷达

        多个激光雷达发射器，按固定频率旋转，雷达探头之间有夹角。
        水平探测角度通常是360°，垂直探测角度通常是30°~45°。
        探头数量也称线束，通常是2的整数倍，有4、16、32、64、128线激光雷达。
        成本过高，震动等恶劣条件下容易损坏。

多线测距的原理和单线一样，称为飞行时间原理 ToF（Time of Flight）。细分为DToF（Direct ToF）和IToF（Indirect ToF）。DToF功率较低，实现简单，多数激光雷达和RGBD相机使用该方案测距。

7.1.2 固态激光雷达

水平视野（Field of View，FoV）通常在120°以内，一般不讨论线束概念，但会有一个等效线束（多数雷达会等效于128线以上的机械激光雷达）。成本较低，寿命更长，用于感知障碍，很少用于L4级的高精地图与定位。

转镜式激光雷达或半固态激光雷达：
激光雷达发射器和接收器不动，前方加一个能微小运动的棱镜。
纯固态激光雷达：
整个发射接收机构不动，存在一个面阵式的发射和接收机构。按照一定顺序扫描（相位控制技术）或同时扫描和接收（Flash技术）。

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

7.2.1 点到点ICP

多数多线激光雷达SLAM不使用子地图的概念，点云配准主要以Scan to Scan或Scan to Map为主。
对于两个点云 $S_{1} = \left \{ p_{1},\dots,p_{m}\right \}$ 和 $S_{2} = \left \{ q_{1},\dots,q_{m}\right \}$ ，若能正确配准，应满足：

$p_{i} = Rq_{j} + t$

ICP整体思路：

1. 设初始位姿估计为R0，t0。
2. 从初始位姿估计开始迭代。设第k次迭代时位姿估计为Rk，tk。
3. 在Rk，tk估计下，按照最近邻方式寻找匹配点。记匹配点对为（pi，qi）。
4. 计算本次迭代结果： $R_{k+1},t_{k+1}=arg\min_{R,t}\left \| p_{i}-(Rq_{i}+t) \right \|_{2}^{2}$
5. 判断解是否收敛，若不收敛则返回3，收敛则退出。

代码实现

测试程序主函数——test_icp.cc

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    // EPFL 雕像数据集：./ch7/EPFL/aquarius_{sourcd.pcd, target.pcd}，真值在对应目录的_pose.txt中
    // EPFL 模型比较精细，配准时应该采用较小的栅格

    std::ifstream fin(FLAGS_ground_truth_file);
    SE3 gt_pose;
    if (fin) {
        double tx, ty, tz, qw, qx, qy, qz;
        fin >> tx >> ty >> tz >> qw >> qx >> qy >> qz;
        fin.close();
        gt_pose = SE3(Quatd(qw, qx, qy, qz), Vec3d(tx, ty, tz));
    }

    sad::CloudPtr source(new sad::PointCloudType), target(new sad::PointCloudType);
    pcl::io::loadPCDFile(fLS::FLAGS_source, *source);
    pcl::io::loadPCDFile(fLS::FLAGS_target, *target);

    bool success;

    sad::evaluate_and_call(
        [&]() {
            sad::Icp3d icp;
            icp.SetSource(source);
            icp.SetTarget(target);
            icp.SetGroundTruth(gt_pose);
            SE3 pose;
            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);
                pcl::transformPointCloud(*source, *source_trans, pose.matrix().cast<float>());
                sad::SaveCloudToFile("./data/ch7/icp_trans.pcd", *source_trans);
            } else {
                LOG(ERROR) << "align failed.";
            }
        },
        "ICP P2P", 1);

icp_3d.h

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

#ifndef SLAM_IN_AUTO_DRIVING_ICP_3D_H
#define SLAM_IN_AUTO_DRIVING_ICP_3D_H

#include "ch5/kdtree.h"

namespace sad {

/**
 * 3D 形式的ICP
 * 先SetTarget, 再SetSource, 然后调用Align方法获取位姿
 *
 * ICP 求解R,t 将source点云配准到target点云上
 * 如果 p 是source点云中的点，那么R*p+t就得到target中的配对点
 *
 * 使用第5章的Kd树来求取最近邻
 */
class Icp3d {
   public:
    struct Options {
        int max_iteration_ = 20;                // 最大迭代次数
        double max_nn_distance_ = 1.0;          // 点到点最近邻查找时阈值
        double max_plane_distance_ = 0.05;      // 平面最近邻查找时阈值
        double max_line_distance_ = 0.5;        // 点线最近邻查找时阈值
        int min_effective_pts_ = 10;            // 最近邻点数阈值
        double eps_ = 1e-2;                     // 收敛判定条件
        bool use_initial_translation_ = false;  // 是否使用初始位姿中的平移估计
    };

    Icp3d() {}
    Icp3d(Options options) : options_(options) {}

    /// 设置目标的Scan
    void SetTarget(CloudPtr target) {
        target_ = target;
        BuildTargetKdTree();

        // 计算点云中心
        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();
        LOG(INFO) << "target center: " << target_center_.transpose();
    }

    /// 设置被配准的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();
        LOG(INFO) << "source center: " << source_center_.transpose();
    }

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

    /// 使用gauss-newton方法进行配准, 点到点
    bool AlignP2P(SE3& init_pose);

    /// 基于gauss-newton的点线ICP
    bool AlignP2Line(SE3& init_pose);

    /// 基于gauss-newton的点面ICP
    bool AlignP2Plane(SE3& init_pose);

   private:
    // 建立目标点云的Kdtree
    void BuildTargetKdTree();

    std::shared_ptr<KdTree> kdtree_ = nullptr;  // 第5章的kd树

    CloudPtr target_ = nullptr;
    CloudPtr source_ = nullptr;

    Vec3d target_center_ = Vec3d::Zero();
    Vec3d source_center_ = Vec3d::Zero();

    bool gt_set_ = false;  // 真值是否设置
    SE3 gt_pose_;

    Options options_;
};

}  // namespace sad

#endif  // SLAM_IN_AUTO_DRIVING_ICP_3D_H

icp_3d.cc

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());
    std::vector<Vec3d> errors(index.size());

使用高斯牛顿法求解位姿，最大20次迭代
对source点云中的每个点q进行位姿转换得到qs（即其在target点云中的位置）。KD树来查找点云的最近邻，将查找到的最近邻p和qs相减，作为误差。然后建立雅可比矩阵。

for (int iter = 0; iter < options_.max_iteration_; ++iter) {
        // gauss-newton 迭代
        // 最近邻，可以并发
        std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {
            auto q = ToVec3d(source_->points[idx]);
            Vec3d qs = pose * q;  // 转换之后的q
            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;

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

                jacobians[idx] = J;
                errors[idx] = e;
            } else {
                effect_pts[idx] = false;
            }
        });

计算有效点的总残差和数量
累积雅可比矩阵和误差向量的相关值
求出位姿并更新位姿

// 累加Hessian和error,计算dx
        // 原则上可以用reduce并发，写起来比较麻烦，这里写成accumulate
        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;
        }

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

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

7.2.2 点到线、点到面ICP

相比与点到点ICP，点到线以及点到面ICP方法不再是去target点云寻找最近邻点，而是需要k个最近邻点，将这些点拟合成一个平面或直线。
假设拟合成的平面参数为（n，d），n为单位长度法线（3维向量），d为截距（1维）。对于平面上的点p，有：

$n^{\top}p+d =0$

于是可以建立source点云点qi与它最近邻点构成的平面之间的误差函数：

$e_{i}=n^{\top}(Rq_{i}+t)+d$

它对R和t的导数为：

$\frac{\partial e}{\partial R} =-n^{\top}Rq_{i}^{\wedge } , \ \frac{\partial e}{\partial t}=n$

如果qi的最近邻构成直线，则设直线方程为：

$p=d\tau +p_{0}$

d是单位长度的方向向量，p0是直线上一点， $\tau$ 为参数。对于不在直线上的点qi，将它到直线的垂直向量长度作为误差函数。优化叉乘向量：

$e_{i}=d\times (Rq_{i}+t-p_{0})$

此误差关于R和t的导数为：

$\frac{\partial e}{\partial R} =-d^{\wedge }Rq_{i}^{\wedge } , \ \frac{\partial e}{\partial t}=d^{\wedge }$

代码实现

点到面ICP与点到点ICP的思路基本一致。有两个小差异：
1. 需要给最近邻点拟合平面，因此需要查找多个最近邻。
2. 需要添加一些阈值检查来判定平面是否合理，然后使用平面参数n来计算雅可比矩阵。

主函数中的启动函数——test_icp.cc

/// 点到面
    sad::evaluate_and_call(
        [&]() {
            sad::Icp3d icp;
            icp.SetSource(source);
            icp.SetTarget(target);
            icp.SetGroundTruth(gt_pose);
            SE3 pose;
            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>());
                sad::SaveCloudToFile("./data/ch7/icp_plane_trans.pcd", *source_trans);
            } else {
                LOG(ERROR) << "align failed.";
            }
        },
        "ICP P2Plane", 1);

icp_3d.cc中的点到面icp函数

流程和点到点基本一致，取5个最近邻拟合成平面，求出单位长度法线n和截距d，用于构建雅可比矩阵和误差。
点到面ICP的迭代次数更少，误差下降更快。整体计算效率略优于点到点ICP。
点到线的实现方法也差不多，其结果与点到点相当，略差于点到面。

bool Icp3d::AlignP2Plane(SE3& init_pose) {
    LOG(INFO) << "aligning with point to plane";
    assert(target_ != nullptr && source_ != nullptr);
    // 整体流程与p2p一致，读者请关注变化部分

    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, 1, 6>> jacobians(index.size());
    std::vector<double> errors(index.size());

    for (int iter = 0; iter < options_.max_iteration_; ++iter) {
        // gauss-newton 迭代
        // 最近邻，可以并发
        std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {
            auto q = ToVec3d(source_->points[idx]);
            Vec3d qs = pose * q;  // 转换之后的q
            std::vector<int> nn;
            kdtree_->GetClosestPoint(ToPointType(qs), nn, 5);  // 这里取5个最近邻
            if (nn.size() > 3) {
                // convert to eigen
                std::vector<Vec3d> nn_eigen;
                for (int i = 0; i < nn.size(); ++i) {
                    nn_eigen.emplace_back(ToVec3d(target_->points[nn[i]]));
                }

                Vec4d n;
                if (!math::FitPlane(nn_eigen, n)) {
                    // 失败的不要
                    effect_pts[idx] = false;
                    return;
                }

                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;

                // build residual
                Eigen::Matrix<double, 1, 6> J;
                J.block<1, 3>(0, 0) = -n.head<3>().transpose() * pose.so3().matrix() * SO3::hat(q);
                J.block<1, 3>(0, 3) = n.head<3>().transpose();

                jacobians[idx] = J;
                errors[idx] = dis;
            } else {
                effect_pts[idx] = false;
            }
        });

        // 累加Hessian和error,计算dx
        // 原则上可以用reduce并发，写起来比较麻烦，这里写成accumulate
        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;
        }

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

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

7.2.3 NDT（Normal Distribution Transform）方法

NDT思路如下：

1. 把目标点云按一定分辨率分成若干体素。
2. 计算每个体素中的点云高斯分布。设第k个体素中的均值为 $\mu_{k}$ ，方差为 $\Sigma _{k}$ 。
3. 配准时，先计算每个点落在哪个体素中，然后建立该点与该体素的 $\mu_{k}$ ， $\Sigma _{k}$ 构成的残差。
4. 利用高斯-牛顿法或L-M（Levenberg-Marquardt）方法对估计位姿进行迭代，然后更新位姿估计。

设source点云中的点qi，在某个栅格i中的残差为：

$e_{i}=Rq_{i}+t-\mu_{i}$

R,t由加权最小二乘问题决定：

$(R,t)^{*}=arg\min_{R,t}\sum_{i}(e_{i}^{\top}\Sigma_{i}^{-1}e_{i})$

由最小二乘法知识可以看出，上述问题相当于最大化每个点落在所在栅格的概率分布，因此是一个最大似然估计（MLE）：

$(R,t)^{*}=arg\max_{R,t}\prod_{i} P(Rq_{i}+t)$

一个加权最小二乘问题的高斯-牛顿法如下：

$\sum_{i}(J_{i}^{\top}\Sigma_{i}^{-1}J_{i})\triangle x=-\sum_{i}J_{i}^{\top}\Sigma_{i}^{-1}e_{i}$

误差对变量导数为：

$\frac{\partial e_{i}}{\partial R} =-Rq_{i}^{\wedge } , \ \frac{\partial e_{i}}{\partial t}=I$

代码实现

ndt_3d.cc

建立体素的函数

void Ndt3d::BuildVoxels() {
    assert(target_ != nullptr);
    assert(target_->empty() == false);
    grids_.clear();

    /// 分配体素
    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) {
        Vec3d pt = ToVec3d(target_->points[idx]) * options_.inv_voxel_size_;
        auto key = CastToInt(pt);
        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) {
        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]); });
            // 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
            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);
        }
    }
}

NDT优化，选择当前体素的周围六个体素作为最近邻。对每个source点云进行并发处理，获得位姿转换后的qs，计算qs所在的栅格以及最近邻。对每个最近邻栅格（包括qs所在栅格），取出其体素数据，用来建立误差error和雅可比矩阵J。最后用累计的J和error，计算H和err（对应上面公式的左边和右边），求解增量dx。

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

    // 对点的索引，预先生成
    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;

    for (int iter = 0; iter < options_.max_iteration_; ++iter) {
        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);

        // gauss-newton 迭代
        // 最近邻，可以并发
        std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {
            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) {
                auto key_off = key + nearby_grids_[i];
                auto it = grids_.find(key_off);
                int real_idx = idx * num_residual_per_point + i;
                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<double, 3, 6> 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_;
                    effect_pts[real_idx] = true;
                } else {
                    effect_pts[real_idx] = false;
                }
            }
        });

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

        Mat6d H = Mat6d::Zero();
        Vec6d err = Vec6d::Zero();

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

如果点云比较稠密，则可以仅使用中心体素。如果点云比较稀疏，则应该考虑周围体素。
NDT和其他配准方法一样，依赖初始估计。体素大小也会对NDT造成影响，体素大小取决于点云的规模，若点云场景大而体素太小，则会因点数太少而失去拟合意义；若点云密集而体素太大，则体素内部结构过于复杂无法拟合。

7.3 直接法激光雷达里程计

在3D SLAM中，点云之间可以进行合并，也可以将过去一段时间内的点云组成一个局部地图，然后将当前帧和这个局部地图进行配准。不需要提取特征，成为直接法激光雷达里程计（Direct Lidar Odometry）。根据配准方法不同，有两种实现激光雷达里程计的思路：基于更新关键帧队列和基于更新NDT体素。

7.3.1 使用NDT构建激光雷达里程计

测试主函数——test_ndt_lo.cc

#include <gflags/gflags.h>
#include <glog/logging.h>

#include "ch7/direct_ndt_lo.h"
#include "ch7/ndt_3d.h"
#include "common/dataset_type.h"
#include "common/io_utils.h"
#include "common/timer/timer.h"

/// 本程序以ULHK数据集为例
/// 测试以NDT为主的Lidar Odometry
/// 若使用PCL NDT的话，会重新建立NDT树
DEFINE_string(bag_path, "./dataset/sad/ulhk/test2.bag", "path to rosbag");
DEFINE_string(dataset_type, "ULHK", "NCLT/ULHK/KITTI/WXB_3D");  // 数据集类型
DEFINE_bool(use_pcl_ndt, false, "use pcl ndt to align?");
DEFINE_bool(use_ndt_nearby_6, false, "use ndt nearby 6?");
DEFINE_bool(display_map, true, "display map?");

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    sad::RosbagIO rosbag_io(fLS::FLAGS_bag_path, sad::Str2DatasetType(FLAGS_dataset_type));

    sad::DirectNDTLO::Options options;
    options.use_pcl_ndt_ = fLB::FLAGS_use_pcl_ndt;
    options.ndt3d_options_.nearby_type_ =
        FLAGS_use_ndt_nearby_6 ? sad::Ndt3d::NearbyType::NEARBY6 : sad::Ndt3d::NearbyType::CENTER;
    options.display_realtime_cloud_ = FLAGS_display_map;
    sad::DirectNDTLO ndt_lo(options);

    rosbag_io
        .AddAutoPointCloudHandle([&ndt_lo](sensor_msgs::PointCloud2::Ptr msg) -> bool {
            sad::common::Timer::Evaluate(
                [&]() {
                    SE3 pose;
                    ndt_lo.AddCloud(sad::VoxelCloud(sad::PointCloud2ToCloudPtr(msg)), pose);
                },
                "NDT registration");
            return true;
        })
        .Go();

    if (FLAGS_display_map) {
        // 把地图存下来
        ndt_lo.SaveMap("./data/ch7/map.pcd");
    }

    sad::common::Timer::PrintAll();
    LOG(INFO) << "done.";

    return 0;
}

direct_ndt_lo.h

/**
 * 使用直接NDT方法进行递推的Lidar Odometry
 * 使用历史几个关键帧作为local map，进行NDT定位
 */
class DirectNDTLO {
   public:
    struct Options {
        Options() {}
        double kf_distance_ = 0.5;            // 关键帧距离
        double kf_angle_deg_ = 30;            // 旋转角度
        int num_kfs_in_local_map_ = 30;       // 局部地图含有多少个关键帧
        bool use_pcl_ndt_ = true;            // 使用本章的NDT还是PCL的NDT
        bool display_realtime_cloud_ = true;  // 是否显示实时点云

        Ndt3d::Options ndt3d_options_;  // NDT3D 的配置
    };

    DirectNDTLO(Options options = Options()) : options_(options) {
        if (options_.display_realtime_cloud_) {
            viewer_ = std::make_shared<PCLMapViewer>(0.5);
        }

        ndt_ = Ndt3d(options_.ndt3d_options_);

        ndt_pcl_.setResolution(1.0);
        ndt_pcl_.setStepSize(0.1);
        ndt_pcl_.setTransformationEpsilon(0.01);
    }

    /**
     * 往LO中增加一个点云
     * @param scan  当前帧点云
     * @param pose 估计pose
     */
    void AddCloud(CloudPtr scan, SE3& pose);

    /// 存储地图(viewer里）
    void SaveMap(const std::string& map_path);

   private:
    /// 与local map进行配准
    SE3 AlignWithLocalMap(CloudPtr scan);

    /// 判定是否为关键帧
    bool IsKeyframe(const SE3& current_pose);

   private:
    Options options_;
    CloudPtr local_map_ = nullptr;
    std::deque<CloudPtr> scans_in_local_map_;
    std::vector<SE3> estimated_poses_;  // 所有估计出来的pose，用于记录轨迹和预测下一个帧
    SE3 last_kf_pose_;                  // 上一关键帧的位姿

    pcl::NormalDistributionsTransform<PointType, PointType> ndt_pcl_;
    Ndt3d ndt_;

    std::shared_ptr<PCLMapViewer> viewer_ = nullptr;
};

}  // namespace sad

#endif  // SLAM_IN_AUTO_DRIVING_DIRECT_NDT_LO_H

direct_ndt_lo.cc

对每帧新进来的点云scan，计算scan相对于local map的位姿（使用NDT配准方法），然后把scan从局部坐标系转换到世界坐标系。在配准过程中，如果存储的估计位姿大于等于2，则使用最近的两个位姿来预测当前帧的初始位姿。令T2=Tm2为最近的位姿（相对于local map），T1=Tm1为第二近的位姿，则初始位姿guess = Tm2（T1m * Tm2）= Tm2 * T12。这一点还是不明白为什么
检查是否是关键帧，如果是关键帧，则送入局部地图中，重新建立局部地图。

namespace sad {

void DirectNDTLO::AddCloud(CloudPtr scan, SE3& pose) {
    if (local_map_ == nullptr) {
        // 第一个帧，直接加入local map
        local_map_.reset(new PointCloudType);
        // operator += 用来拼接点云
        *local_map_ += *scan;
        pose = SE3();
        last_kf_pose_ = pose;

        if (options_.use_pcl_ndt_) {
            ndt_pcl_.setInputTarget(local_map_);
        } else {
            ndt_.SetTarget(local_map_);
        }

        return;
    }

    // 计算scan相对于local map的位姿
    pose = AlignWithLocalMap(scan);
    CloudPtr scan_world(new PointCloudType);
    pcl::transformPointCloud(*scan, *scan_world, pose.matrix().cast<float>());

    if (IsKeyframe(pose)) {
        last_kf_pose_ = pose;

        // 重建local map
        scans_in_local_map_.emplace_back(scan_world);
        if (scans_in_local_map_.size() > options_.num_kfs_in_local_map_) {
            scans_in_local_map_.pop_front();
        }

        local_map_.reset(new PointCloudType);
        for (auto& scan : scans_in_local_map_) {
            *local_map_ += *scan;
        }

        if (options_.use_pcl_ndt_) {
            ndt_pcl_.setInputTarget(local_map_);
        } else {
            ndt_.SetTarget(local_map_);
        }
    }

    if (viewer_ != nullptr) {
        viewer_->SetPoseAndCloud(pose, scan_world);
    }
}

bool DirectNDTLO::IsKeyframe(const SE3& current_pose) {
    // 只要与上一帧相对运动超过一定距离或角度，就记关键帧
    SE3 delta = last_kf_pose_.inverse() * current_pose;
    return delta.translation().norm() > options_.kf_distance_ ||
           delta.so3().log().norm() > options_.kf_angle_deg_ * math::kDEG2RAD;
}

SE3 DirectNDTLO::AlignWithLocalMap(CloudPtr scan) {
    if (options_.use_pcl_ndt_) {
        ndt_pcl_.setInputSource(scan);
    } else {
        ndt_.SetSource(scan);
    }

    CloudPtr output(new PointCloudType());

    SE3 guess;
    bool align_success = true;
    if (estimated_poses_.size() < 2) {
        if (options_.use_pcl_ndt_) {
            ndt_pcl_.align(*output, guess.matrix().cast<float>());
            guess = Mat4ToSE3(ndt_pcl_.getFinalTransformation().cast<double>().eval());
        } else {
            align_success = ndt_.AlignNdt(guess);
        }
    } else {
        // 从最近两个pose来推断
        SE3 T1 = estimated_poses_[estimated_poses_.size() - 1];
        SE3 T2 = estimated_poses_[estimated_poses_.size() - 2];
        guess = T1 * (T2.inverse() * T1);

        if (options_.use_pcl_ndt_) {
            ndt_pcl_.align(*output, guess.matrix().cast<float>());
            guess = Mat4ToSE3(ndt_pcl_.getFinalTransformation().cast<double>().eval());
        } else {
            align_success = ndt_.AlignNdt(guess);
        }
    }

    LOG(INFO) << "pose: " << guess.translation().transpose() << ", "
              << guess.so3().unit_quaternion().coeffs().transpose();

    if (options_.use_pcl_ndt_) {
        LOG(INFO) << "trans prob: " << ndt_pcl_.getTransformationProbability();
    }

    estimated_poses_.emplace_back(guess);
    return guess;
}

void DirectNDTLO::SaveMap(const std::string& map_path) {
    if (viewer_) {
        viewer_->SaveMap(map_path);
    }
}

}  // namespace sad

增加关键帧时，每次都要重新建立局部地图，并且要重置所有的NDT体素。可以把当前扫描数据直接放到NDT体素中，同时对体素增加一个时间队列处理，自动丢弃很久没有用到的体素，这种做法称为增量的NDT。

7.3.2 增量NDT里程计

高斯分布的增量更新

设某个体素之前有m个点云，其高斯分布为 $\mu_{H}$ ， $\Sigma _{H}$ 。当这个体素新增了n个点，这n个点的分布为 $\mu_{A}$ ， $\Sigma _{A}$ 。设合并之后的估计为 $\mu$ ， $\Sigma$ 。记原m个点云为{x1，···，xm}，新增n个点云为{y1，···，yn}。则可得（推导过程省略）：

$\mu = \frac{m\mu_{H}+n\mu_{A}}{m+n}$

$\Sigma=\frac{m(\Sigma_{H}+(\mu_{H}-\mu)(\mu_{H}-\mu)^{\top})+n(\Sigma_{A}+(\mu_{A}-\mu)(\mu_{A}-\mu)^{\top})}{m+n}$

利用以上公式更新NDT中的均值和方差估计。

体素的增量维护

把体素数量限制在一定范围内，需要维护一个近期使用的缓存机制（Least Recently Used Cache，LRU）。

ndt_inc.h

#ifndef SLAM_IN_AUTO_DRIVING_NDT_INC_H
#define SLAM_IN_AUTO_DRIVING_NDT_INC_H

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

#include <list>
namespace sad {

/**
 * 增量版本的NDT
 * 内部会维护增量式的voxels，自动删除较旧的voxel，往voxel里添加点云时，更新其均值和协方差估计
 */
class IncNdt3d {
   public:
    enum class NearbyType {
        CENTER,   // 只考虑中心
        NEARBY6,  // 上下左右前后
    };

    struct Options {
        int max_iteration_ = 4;        // 最大迭代次数
        double voxel_size_ = 1.0;      // 体素大小
        double inv_voxel_size_ = 1.0;  // 体素大小之逆
        int min_effective_pts_ = 10;   // 最近邻点数阈值
        int min_pts_in_voxel_ = 5;     // 每个栅格中最小点数
        int max_pts_in_voxel_ = 50;    // 每个栅格中最大点数
        double eps_ = 1e-3;            // 收敛判定条件
        double res_outlier_th_ = 5.0;  // 异常值拒绝阈值
        size_t capacity_ = 100000;     // 缓存的体素数量

        NearbyType nearby_type_ = NearbyType::NEARBY6;
    };

    using KeyType = Eigen::Matrix<int, 3, 1>;  // 体素的索引

    /// 体素内置结构
    struct VoxelData {
        VoxelData() {}
        VoxelData(const Vec3d& pt) {
            pts_.emplace_back(pt);
            num_pts_ = 1;
        }

        void AddPoint(const Vec3d& pt) {
            pts_.emplace_back(pt);
            if (!ndt_estimated_) {
                num_pts_++;
            }
        }

        std::vector<Vec3d> pts_;       // 内部点，多于一定数量之后再估计均值和协方差
        Vec3d mu_ = Vec3d::Zero();     // 均值
        Mat3d sigma_ = Mat3d::Zero();  // 协方差
        Mat3d info_ = Mat3d::Zero();   // 协方差之逆

        bool ndt_estimated_ = false;  // NDT是否已经估计
        int num_pts_ = 0;             // 总共的点数，用于更新估计
    };

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

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

    /// 获取一些统计信息
    int NumGrids() const { return grids_.size(); }

    /// 在voxel里添加点云，
    void AddCloud(CloudPtr cloud_world);

    /// 设置被配准的Scan
    void SetSource(CloudPtr source) { source_ = source; }

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

    /**
     * 计算给定Pose下的雅可比和残差矩阵，符合IEKF中符号（8.17, 8.19）
     * @param pose
     * @param HTVH
     * @param HTVr
     */
    void ComputeResidualAndJacobians(const SE3& pose, Mat18d& HTVH, Vec18d& HTVr);

    /**
     * 根据估计的NDT建立edges
     * @param v
     * @param edges
     */
    void BuildNDTEdges(VertexPose* v, std::vector<EdgeNDT*>& edges);

   private:
    /// 根据最近邻的类型，生成附近网格
    void GenerateNearbyGrids();

    /// 更新体素内部数据, 根据新加入的pts和历史的估计情况来确定自己的估计
    void UpdateVoxel(VoxelData& v);

    CloudPtr source_ = nullptr;
    Options options_;

    using KeyAndData = std::pair<KeyType, VoxelData>;  // 预定义
    std::list<KeyAndData> data_;                       // 真实数据，会缓存，也会清理
    std::unordered_map<KeyType, std::list<KeyAndData>::iterator, hash_vec<3>> grids_;  // 栅格数据，存储真实数据的迭代器
    std::vector<KeyType> nearby_grids_;                                                // 附近的栅格

    bool flag_first_scan_ = true;  // 首帧点云特殊处理
};

}  // namespace sad

#endif  // SLAM_IN_AUTO_DRIVING_NDT_INC_H

ndt_inc.cc

在IncNdt3d::AddCloud函数中，对被送进来的点云计算其键值，在体素数据中查找是否存在该体素，若不存在，则将键值和点云存入data_的前面，把键值和迭代器存入grid_。如果体素存在，则将点云放入该体素中，将更新的体素放到data_的前面，并更新迭代器。最后并行更行所有被修改的体素。

void IncNdt3d::AddCloud(CloudPtr cloud_world) {
    std::set<KeyType, less_vec<3>> active_voxels;  // 记录哪些voxel被更新
    for (const auto& p : cloud_world->points) {
        auto pt = ToVec3d(p);
        auto key = CastToInt(Vec3d(pt * options_.inv_voxel_size_));
        auto iter = grids_.find(key);
        if (iter == grids_.end()) {
            // 栅格不存在
            data_.push_front({key, {pt}});
            grids_.insert({key, data_.begin()});

            if (data_.size() >= options_.capacity_) {
                // 删除一个尾部的数据
                grids_.erase(data_.back().first);
                data_.pop_back();
            }
        } else {
            // 栅格存在，添加点，更新缓存
            iter->second->second.AddPoint(pt);
            data_.splice(data_.begin(), data_, iter->second);  // 更新的那个放到最前
            iter->second = data_.begin();                      // grids时也指向最前
        }

        active_voxels.emplace(key);
    }

    // 更新active_voxels
    std::for_each(std::execution::par_unseq, active_voxels.begin(), active_voxels.end(),
                  [this](const auto& key) { UpdateVoxel(grids_[key]->second); });
    flag_first_scan_ = false;
}

incremental_ndt_lo.cc——增量式NDT激光雷达里程计

只需要取关键帧，通过ndt_.AddCloud(scan_world)把关键帧点云加入到局部地图，并更新体素数据，在下一次新的扫描到来时，可以直接进行NDT配准。配准后也无需重建局部地图。

void IncrementalNDTLO::AddCloud(CloudPtr scan, SE3& pose, bool use_guess) {
    if (first_frame_) {
        // 第一个帧，直接加入local map
        pose = SE3();
        last_kf_pose_ = pose;
        ndt_.AddCloud(scan);
        first_frame_ = false;
        return;
    }

    // 此时local map位于NDT内部，直接配准即可
    SE3 guess;
    ndt_.SetSource(scan);
    if (estimated_poses_.size() < 2) {
        ndt_.AlignNdt(guess);
    } else {
        if (!use_guess) {
            // 从最近两个pose来推断
            SE3 T1 = estimated_poses_[estimated_poses_.size() - 1];
            SE3 T2 = estimated_poses_[estimated_poses_.size() - 2];
            guess = T1 * (T2.inverse() * T1);
        } else {
            guess = pose;
        }

        ndt_.AlignNdt(guess);
    }

    pose = guess;
    estimated_poses_.emplace_back(pose);

    CloudPtr scan_world(new PointCloudType);
    pcl::transformPointCloud(*scan, *scan_world, guess.matrix().cast<float>());

    if (IsKeyframe(pose)) {
        last_kf_pose_ = pose;
        cnt_frame_ = 0;
        // 放入ndt内部的local map
        ndt_.AddCloud(scan_world);
    }

    if (viewer_ != nullptr) {
        viewer_->SetPoseAndCloud(pose, scan_world);
    }
    cnt_frame_++;
}