基于NDT的前端里程计代码解析

本文主要对Lidar_Localization文件中的三个源文件，front_end_node.cpp、front_end.h/cpp进行详细代码解读。
代码整体部分为下图：

其中，NDT的里程计主函数是src中front_end_node.cpp函数，里面有对前端激光雷达点云里程计进行框架流程描述，具体的函数是在front_end.cpp中进行实现。
头文件include中publisher分为发布点云数据cloud和里程计数据odometry两部分，主要包含Pubish()函数。sensor_data中包含三个传感器数据：激光雷达点云cloud、GNSS、IMU。subscriber中同样包含对于三种传感器数据的接收，主要通过回调函数将数据填充到new_XXX_data中。

front_end_node.cpp中前端激光雷达点云里程计主要分为：点云下采样滤波、点云匹配、位姿估计、关键帧选取等步骤。具体流程为：
1）第一帧点云数据设置为地图
2）提取关键帧点云，拼接成地图，保证稀疏点云数据。
3）除了全局地图，还需要在当前帧附近形成滑动窗局部地图，减小计算量
4）匹配之前需要滤波，对点云稀疏化。

NDT是一种适用于三维点云的配准算法，可以在PCL中调用正态分布变换NDT(Normal Distributions Transform)进行配准。ICP算法作为点对点的匹配点云算法，当出现小部分差异的环境变化时，使用NDT算法可以很好的消除这种不确定性。NDT没有像ICP一样计算两个点云之间点对点的差距，而是先将参考点云（高精度地图）转换为多维变量的正态分布，利用牛顿法等优化算法根据变换点在参考系中的概率密度之和最大判断匹配。
NDT基本思想为把空间划分为立方体，点云划分到网格中，每个立方体中存储的是这个立方体被占据的概率密度。

接下来开始分析源代码

front_end_node

激光雷达点云前端里程计主要包含以下步骤

1. ROS常规初始化
2. 初始化操作：读取传感器、获得lidar-to-imu变换矩阵、保证三个传感器时间对齐
3. GNSS位姿初始化，更新里程计位姿
4. 前端里程计

1、ROS常规初始化

• 订阅发布节点，包含订阅sub 3个传感器消息：点云、imu、GNSS。
• 发布pub 5个节点：点云、局部地图、全局地图、点云里程计、GNSS，
• 坐标转换的Lidar-to-imu变换矩阵T

参数定义上订阅和发布的指针分别命名为XXX_sub_ptr或者XXX_pub_ptr，传感器数据队列命名为XXX_data_ptr，针对地图指针为XXX_map_ptr，最重要的前端里程计指针为front_end_ptr。
重要变量：

// 三个传感器sub消息
cloud_sub_ptr,imu_sub_ptr,gnss_sub_ptr
// 两个传感器发布pub节点
cloud_pub_ptr,gnss_pub_ptr
// 发布的局部、全局地图
local_map_pub_ptr,global_map_pub_ptr
// 发布的点云里程计
laser_odom_pub_ptr
// 前端
front_end_ptr

// 三个传感器队列buff
cloud_data_buff，imu_data_buff，gnss_data_buff
// lidar-to-imu变换矩阵
lidar_to_imu

// 三个指针：扫描点云、局部，全局地图
current_scan_ptr,local_map_ptr，global_map_ptr

ROS初始化基本框架为：

#include 
int main(){
   // ROS初始化，句柄
   ros::init(argc, argv, "front_end_node");
   ros::NodeHandle nh;
   ......
   ros::Rate rate(100);// 自循环频率为100HZ
   while (ros::ok()) {
       // 调用一次，回调函数还会被调用,和while()一起用
        ros::spinOnce();
        .........
}
  rate.sleep();// 休眠一段时间来使得发布频率为100HZ
}

具体代码为：

    ros::init(argc, argv, "front_end_node");
    ros::NodeHandle nh;
    // 三个订阅sub消息：点云、imu、GNSS
    std::shared_ptr<CloudSubscriber> cloud_sub_ptr = std::make_shared<CloudSubscriber>(nh, "/kitti/velo/pointcloud", 100000);
    std::shared_ptr<IMUSubscriber> imu_sub_ptr = std::make_shared<IMUSubscriber>(nh, "/kitti/oxts/imu", 1000000);
    std::shared_ptr<GNSSSubscriber> gnss_sub_ptr = std::make_shared<GNSSSubscriber>(nh, "/kitti/oxts/gps/fix", 1000000);
    // lidar-to-imu
    std::shared_ptr<TFListener> lidar_to_imu_ptr = std::make_shared<TFListener>(nh, "velo_link", "imu_link");
    // 发布pub节点：点云、局部地图、全局地图
    std::shared_ptr<CloudPublisher> cloud_pub_ptr = std::make_shared<CloudPublisher>(nh, "current_scan", 100, "/map");
    std::shared_ptr<CloudPublisher> local_map_pub_ptr = std::make_shared<CloudPublisher>(nh, "local_map", 100, "/map");
    std::shared_ptr<CloudPublisher> global_map_pub_ptr = std::make_shared<CloudPublisher>(nh, "global_map", 100, "/map");
    // 点云里程计、GNSS
    std::shared_ptr<OdometryPublisher> laser_odom_pub_ptr = std::make_shared<OdometryPublisher>(nh, "laser_odom", "map", "lidar", 100);
    std::shared_ptr<OdometryPublisher> gnss_pub_ptr = std::make_shared<OdometryPublisher>(nh, "gnss", "map", "lidar", 100);
    // 前端
    std::shared_ptr<FrontEnd> front_end_ptr = std::make_shared<FrontEnd>();

    // 三个数据队列buff
    std::deque<CloudData> cloud_data_buff;
    std::deque<IMUData> imu_data_buff;
    std::deque<GNSSData> gnss_data_buff;
    // lidar到imu的转换矩阵
    Eigen::Matrix4f lidar_to_imu = Eigen::Matrix4f::Identity();

    bool transform_received = false;
    bool gnss_origin_position_inited = false;
    bool front_end_pose_inited = false;
    // 三个指针：点云、局部地图、全局地图
    CloudData::CLOUD_PTR local_map_ptr(new CloudData::CLOUD());
    CloudData::CLOUD_PTR global_map_ptr(new CloudData::CLOUD());
    CloudData::CLOUD_PTR current_scan_ptr(new CloudData::CLOUD());
    
    double run_time = 0.0;
    double init_time = 0.0;
    bool time_inited = false;
    bool has_global_map_published = false;

以激光雷达点云数据CloudData类为例，点云数据类包含两变量：时间戳time+点云指针cloud_ptr。

std::shared_ptr<CloudSubscriber> cloud_sub_ptr = std::make_shared<CloudSubscriber>(nh, "/kitti/velo/pointcloud", 100000);

CloudSubscriber类

这里定义了一个CloudSubscriber类，声明中主要包含：

// 成员变量：新扫描的点云队列
deque<CloudData> new_cloud_data_;

// 成员函数
void ParseData();// 读取数据
void msg_callback// 回调函数

函数定义为：

// 构造函数：句柄，话题名称，buff缓冲区大小
CloudSubscriber::CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size)
    :nh_(nh) {
    subscriber_ = nh_.subscribe(topic_name, buff_size, &CloudSubscriber::msg_callback, this);
}
// 回调函数：给点云CloudData赋值，填充进new_cloud_data_中
void CloudSubscriber::msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr) {
    // 点云数据：时间戳+点云指针
    CloudData cloud_data;
    cloud_data.time = cloud_msg_ptr->header.stamp.toSec();
    // ROS数据转成PCL数据
    pcl::fromROSMsg(*cloud_msg_ptr, *(cloud_data.cloud_ptr));
    new_cloud_data_.push_back(cloud_data);
}
// 读取数据：从new_cloud_data_转移到cloud_data_buff中
void CloudSubscriber::ParseData(std::deque<CloudData>& cloud_data_buff) {
    if (new_cloud_data_.size() > 0) {
        cloud_data_buff.insert(cloud_data_buff.end(), new_cloud_data_.begin(), new_cloud_data_.end());
        new_cloud_data_.clear();
    }
}

2、初始化操作：读取传感器数据、获取lidar-to-imu变换矩阵、保证三个传感器时间对齐

主要是为了取出三个传感器数据，在取出之前先把最新扫描的传感器数据添加到队列buff中，然后取出过程中保证三个传感器时间对齐，最后要把读取的传感器数据从队列buff中剔除。

重要变量：

// lidar-to-imu变换矩阵
lidar_to_imu
// 三个传感器数据
cloud_data，imu_data ，gnss_data

    ros::Rate rate(100);
    while (ros::ok()) {
        ros::spinOnce();
        // 1.读取新扫描的数据添加到三个队列buff中，即更新传感器buff
        cloud_sub_ptr->ParseData(cloud_data_buff);
        imu_sub_ptr->ParseData(imu_data_buff);
        gnss_sub_ptr->ParseData(gnss_data_buff);

        if (!transform_received) {
            if (lidar_to_imu_ptr->LookupData(lidar_to_imu)) {
                transform_received = true;
            }
        } else {
            while (cloud_data_buff.size() > 0 && imu_data_buff.size() > 0 && gnss_data_buff.size() > 0) {
                // 1.读取三个传感器数据
                CloudData cloud_data = cloud_data_buff.front();
                IMUData imu_data = imu_data_buff.front();
                GNSSData gnss_data = gnss_data_buff.front();

                if (!time_inited) {
                    time_inited = true;
                    init_time = cloud_data.time;
                } else {
                    run_time = cloud_data.time - init_time;// 点云运行时间
                }
                // 2.保证三个传感器时间对齐
                double d_time = cloud_data.time - imu_data.time;
                if (d_time < -0.05) {
                    cloud_data_buff.pop_front();
                } else if (d_time > 0.05) {
                    imu_data_buff.pop_front();
                    gnss_data_buff.pop_front();
                } else
                 {  // 剔除读取出的传感器数据 
                    cloud_data_buff.pop_front();
                    imu_data_buff.pop_front();
                    gnss_data_buff.pop_front();

三种传感器：雷达点云、imu、GNSS的变量内容为：
激光雷达点云cloud_data.h
构成分为两部分：时间戳+点云指针

  //时间戳+点云指针
class CloudData {
  public:
    using POINT = pcl::PointXYZ;// 三维点
    using CLOUD = pcl::PointCloud<POINT>;// 点云
    using CLOUD_PTR = CLOUD::Ptr;// 点云指针

  public:
    CloudData()
      :cloud_ptr(new CLOUD()) {
    }

  public:
    double time = 0.0;
    CLOUD_PTR cloud_ptr;
};

imu_data.h
imu包含时间戳、线速度、角速度、方向角类（四元数+归一化）
类成员函数：四元数转旋转矩阵GetOrientationMatrix()

class IMUData {
  public:
    struct LinearAcceleration {
      double x = 0.0;
      double y = 0.0;
      double z = 0.0;
    };

    struct AngularVelocity {
      double x = 0.0;
      double y = 0.0;
      double z = 0.0;
    };
    // 方向角类（四元数+归一化）
    class Orientation {
      public:
        double x = 0.0;
        double y = 0.0;
        double z = 0.0;
        double w = 0.0;
      
      public:
        void Normlize() {
          double norm = sqrt(pow(x, 2.0) + pow(y, 2.0) + pow(z, 2.0) + pow(w, 2.0));
          x /= norm;
          y /= norm;
          z /= norm;
          w /= norm;
        }
    };

    double time = 0.0;
    LinearAcceleration linear_acceleration;
    AngularVelocity angular_velocity;
    Orientation orientation;
  
  public:
    // 把四元数转换成旋转矩阵送出去
    Eigen::Matrix3f GetOrientationMatrix();

gnss_data.h
GNSS数据包括：时间戳、地理坐标系、东北天坐标系。

class GNSSData {
  public:
    double time = 0.0;// 时间戳
    // 地理坐标系：经纬度、高度
    double longitude = 0.0;
    double latitude = 0.0;
    double altitude = 0.0;
    // 东北天坐标系
    double local_E = 0.0;
    double local_N = 0.0;
    double local_U = 0.0;

    int status = 0;
    int service = 0;

  private:
    static GeographicLib::LocalCartesian geo_converter;
    static bool origin_position_inited;

类成员函数：初始化位置、更新位姿。

    // 初始化位置reset
void GNSSData::InitOriginPosition() {
    geo_converter.Reset(latitude, longitude, altitude);
    origin_position_inited = true;
}
// 更新位姿
void GNSSData::UpdateXYZ() {
    if (!origin_position_inited) {
        LOG(WARNING) << "GeoConverter has not set origin position";
    }
    geo_converter.Forward(latitude, longitude, altitude, local_E, local_N, local_U);
}

获取Lidar-to-IMU的转换矩阵
test_frame_node.cpp中：初始化为

// 一个坐标系转换指针ptr:基坐标系雷达、子坐标系imu
    std::shared_ptr<TFListener> lidar_to_imu_ptr = std::make_shared<TFListener>(nh, "velo_link", "imu_link");

利用了

#include
tf::StampedTransform

主要利用tf_lisener.cpp中的两个函数:
LookupData()

// 给lidar-to-imu赋值
bool TFListener::LookupData(Eigen::Matrix4f& transform_matrix) {
    try {
        tf::StampedTransform transform;
        listener_.lookupTransform(base_frame_id_, child_frame_id_, ros::Time(0), transform);
        TransformToMatrix(transform, transform_matrix);
        return true;
    } catch (tf::TransformException &ex) {
        return false;
    }
}

TransformToMatrix()

bool TFListener::TransformToMatrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix) {
    Eigen::Translation3f tl_btol(transform.getOrigin().getX(), transform.getOrigin().getY(), transform.getOrigin().getZ());
    
    double roll, pitch, yaw;
    tf::Matrix3x3(transform.getRotation()).getEulerYPR(yaw, pitch, roll);
    Eigen::AngleAxisf rot_x_btol(roll, Eigen::Vector3f::UnitX());
    Eigen::AngleAxisf rot_y_btol(pitch, Eigen::Vector3f::UnitY());
    Eigen::AngleAxisf rot_z_btol(yaw, Eigen::Vector3f::UnitZ());

    // 此矩阵为 child_frame_id 到 base_frame_id 的转换矩阵 zyx
    transform_matrix = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();

    return true;
}

3、GNSS初始化位姿、更新里程计位姿

利用GNSS数据作为初始化位姿，然后从LIdar坐标系转化到imu坐标系，最后把估计位姿发布出去。
重要变量：

// 里程计估计位姿
odometry_matrix

                   Eigen::Matrix4f odometry_matrix = Eigen::Matrix4f::Identity();
                    // 1.GNSS数据初始化位姿
                    if (!gnss_origin_position_inited) {
                        gnss_data.InitOriginPosition();
                        gnss_origin_position_inited = true;
                    }
                    // 2.GNSS数据更新里程计位姿
                    gnss_data.UpdateXYZ();
                    odometry_matrix(0,3) = gnss_data.local_E;
                    odometry_matrix(1,3) = gnss_data.local_N;
                    odometry_matrix(2,3) = gnss_data.local_U;
                    odometry_matrix.block<3,3>(0,0) = imu_data.GetOrientationMatrix();
                    // 从lidar坐标系转换到imu系
                    odometry_matrix *= lidar_to_imu;
                    // 3、发布消息
                    gnss_pub_ptr->Publish(odometry_matrix);

使用GNSS数据（平移）和IMU（旋转）数据初始化位姿、更新位姿的两个函数为
gnss_data.InitOriginPosition();
gnss_data.UpdateXYZ();
函数内部实现为：

geo_converter.Reset(latitude, longitude, altitude);
geo_converter.Forward(latitude, longitude, altitude, local_E, local_N, local_U);

4、前端里程计

这是最重要的里程计部分，其实就是对front_end文件进行详细解读。

1. 前端里程计初始化
2. 更新预测位姿为当前估计位姿
3. 更新并发布位姿
4. 获得当前帧点云和局部地图并发布（都是下采样）

// 1.前端里程计初始化
                    if (!front_end_pose_inited) {
                        front_end_pose_inited = true;
                        front_end_ptr->SetInitPose(odometry_matrix);
                    }
                    // 2.更新预测位姿为当前估计位姿
                    front_end_ptr->SetPredictPose(odometry_matrix);
                    // 3.更新位姿
                    Eigen::Matrix4f laser_matrix = front_end_ptr->Update(cloud_data);
                    // 4.发布位姿
                    laser_odom_pub_ptr->Publish(laser_matrix);
                    
                    // 5.获得当前帧点云和局部地图并发布（都是下采样）
                    // 获得当前扫描并发布
                    front_end_ptr->GetCurrentScan(current_scan_ptr);
                    cloud_pub_ptr->Publish(current_scan_ptr);
                    // 获得新局部地图并发布
                    if (front_end_ptr->GetNewLocalMap(local_map_ptr))
                        local_map_pub_ptr->Publish(local_map_ptr);
                }
                // 6.如果运行时间超了且全局地图还没发布，获得下采样后的全局地图并发布
                if (run_time > 460.0 && !has_global_map_published) {
                    if (front_end_ptr->GetNewGlobalMap(global_map_ptr)) {
                        global_map_pub_ptr->Publish(global_map_ptr);
                        has_global_map_published = true;
                    }
                }

其中，初始化里程计位姿 SetInitPose 和预测位姿 SetPredictPose 函数内部如下，就是以GNSS和IMU数据作为初始化位姿。

front_end_ptr->SetInitPose(odometry_matrix){
        init_pose_ = odometry_matrix;
}

针对获得当前帧点云和局部地图发布，都是对点云进行了下采样操作，保证内存比较小。

front_end_ptr->GetCurrentScan(current_scan_ptr){
    display_filter_.setInputCloud(result_cloud_ptr_);
    display_filter_.filter(*current_scan_ptr);
}

接下来专门对front_end.cpp进行讲解。

front_end

首先建立一个Frame类数据结构：
Frame类两个：位姿+点云数据

class Frame {
      public:  
        Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
        CloudData cloud_data;
    };

主要函数及其含义如下表所示。

函数名称	含义
Update()	主函数：更新数据
UpdateNewFrame()	更新新帧
SetInitPose()	初始化位姿
SetPredictPose()	设置预测位姿
GetNewLocalMap()	获得新的局部地图
GetNewGlobalMap()	获得新的全局地图
GetCurrentScan()	获得当前扫描

重要变量：

// 三个滤波器
cloud_filter_,local_map_filter_,display_filter_
// ndt匹配方法的对象
pcl::NormalDistributionsTransform<>::PTR ndt_ptr_
// 局部、全局地图队列
local_map_frames_,global_map_frames_
// 指针：局部、全局地图、点云结果
local_map_ptr_，global_map_ptr_，result_cloud_ptr_
// Frame对象
current_frame_
// 初始位姿，预测位姿
init_pose,predict_pose

构造函数初始化

    // 构造函数初始化
FrontEnd::FrontEnd()
    :ndt_ptr_(new pcl::NormalDistributionsTransform<CloudData::POINT, CloudData::POINT>()),
     local_map_ptr_(new CloudData::CLOUD()),
     global_map_ptr_(new CloudData::CLOUD()),
     result_cloud_ptr_(new CloudData::CLOUD()) {

    // 给个默认参数，以免类的使用者在匹配之前忘了设置参数
    cloud_filter_.setLeafSize(1.3, 1.3, 1.3);
    local_map_filter_.setLeafSize(0.6, 0.6, 0.6);
    display_filter_.setLeafSize(0.5, 0.5, 0.5);

    ndt_ptr_->setResolution(1.0);// NDT网格结构的分辨率
    ndt_ptr_->setStepSize(0.1);// 为More-Thuente线搜索设置最大步长
    ndt_ptr_->setTransformationEpsilon(0.01);// 为终止条件设置最大转换差异
    ndt_ptr_->setMaximumIterations(30);// 匹配迭代最大次数
}

主要的两个函数为更新位姿Update（）和更新关键帧UpdateNewFrame（）

Update（）

输入当前帧扫描的点云图，对位姿进行更新
主要分为以下流程：

1. 参数初始化
2. 第一帧作为地图
3. 其余帧进行正常匹配
4. 每隔2米设置一个关键帧

1、参数初始化
获得时间戳，剔除无效点云，对点云下采样滤波，初始化四个位姿。

 // 1.参数初始化
    current_frame_.cloud_data.time = cloud_data.time;// 时间戳
    // 剔除无效点云：输入点云，输出点云，索引
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*cloud_data.cloud_ptr, *current_frame_.cloud_data.cloud_ptr, indices);
    // 下采样滤波
    CloudData::CLOUD_PTR filtered_cloud_ptr(new CloudData::CLOUD());// 过滤的点云指针
    cloud_filter_.setInputCloud(current_frame_.cloud_data.cloud_ptr);
    cloud_filter_.filter(*filtered_cloud_ptr);
    // 四个位姿
    static Eigen::Matrix4f step_pose = Eigen::Matrix4f::Identity();// 初始化下一个位姿
    static Eigen::Matrix4f last_pose = init_pose_;// 更新上一帧位姿
    static Eigen::Matrix4f predict_pose = init_pose_;// 更新预测位姿
    static Eigen::Matrix4f last_key_frame_pose = init_pose_;// 更新上一帧位姿

2、第一帧为地图
此时把当前帧数据作为第一个关键帧，并更新局部地图容器和全局地图容器

 if (local_map_frames_.size() == 0) {
        current_frame_.pose = init_pose_;
        UpdateNewFrame(current_frame_);// 关键函数
        return current_frame_.pose;
    }

3、正常匹配

    ndt_ptr_->setInputSource(filtered_cloud_ptr);// 要匹配的点云
    ndt_ptr_->align(*result_cloud_ptr_, predict_pose);// 点云配准，变换后的点云在result中
    current_frame_.pose = ndt_ptr_->getFinalTransformation();// 获得位姿

这里是没有解决的根据k-2帧和第k-1帧得到第k帧的代码？？？

// 更新相邻两帧的相对运动
    step_pose = last_pose.inverse() * current_frame_.pose;// ？？？？
    predict_pose = current_frame_.pose * step_pose;// ？？？？
    last_pose = current_frame_.pose;// 更新上一帧位姿

4、关键帧选取

// 4.隔2米设置一个关键帧，匹配之后根据距离判断是否需要生成新的关键帧，如果需要，则做相应更新
    if (fabs(last_key_frame_pose(0,3) - current_frame_.pose(0,3)) + 
        fabs(last_key_frame_pose(1,3) - current_frame_.pose(1,3)) +
        fabs(last_key_frame_pose(2,3) - current_frame_.pose(2,3)) > 2.0) {
        //更新帧
        UpdateNewFrame(current_frame_);
        last_key_frame_pose = current_frame_.pose;
    }
    return current_frame_.pose;

UpdateNewFrame（）

主要分为四步骤

1. 创建Frame对象，保存关键帧点云
2. 更新局部地图
3. 更新ndt匹配的目标点云
4. 更新全局地图

1、创建Frame对象，保存关键帧点云
由于用的是共享指针，所以直接复制只是复制了一个指针而已，此时无论你放多少个关键帧在容器里，这些关键帧点云指针都是指向的同一个点云

    Frame key_frame = new_key_frame;
    
    key_frame.cloud_data.cloud_ptr.reset(new CloudData::CLOUD(*new_key_frame.cloud_data.cloud_ptr));
    CloudData::CLOUD_PTR transformed_cloud_ptr(new CloudData::CLOUD());

2.更新局部地图

// 先加到滑窗队列中
    local_map_frames_.push_back(key_frame);
    while (local_map_frames_.size() > 20) {// 滑窗大小固定
        local_map_frames_.pop_front();
    }
    // 再把局部地图指针进行更新
    local_map_ptr_.reset(new CloudData::CLOUD());
    for (size_t i = 0; i < local_map_frames_.size(); ++i) {// 遍历滑动窗
       // 恢复下采样的点云数据，参数：输入，输出，估计的姿态
        pcl::transformPointCloud(*local_map_frames_.at(i).cloud_data.cloud_ptr, 
                                 *transformed_cloud_ptr, 
                                 local_map_frames_.at(i).pose);
        // 局部地图指针
        *local_map_ptr_ += *transformed_cloud_ptr;
    }
    // 最后表示局部地图更新完成
    has_new_local_map_ = true;

3.更新ndt匹配的目标点云

// 如果滑动窗数量小于10个，直接设置目标点云
    if (local_map_frames_.size() < 10) {
        ndt_ptr_->setInputTarget(local_map_ptr_);
    } 
    else // 否则，先对局部地图进行下采样，再加进去。
    {
        CloudData::CLOUD_PTR filtered_local_map_ptr(new CloudData::CLOUD());
        local_map_filter_.setInputCloud(local_map_ptr_);
        local_map_filter_.filter(*filtered_local_map_ptr);
        ndt_ptr_->setInputTarget(filtered_local_map_ptr);
    }

4.更新全局地图

  global_map_frames_.push_back(key_frame);
    if (global_map_frames_.size() % 100 != 0) {
        return;
    } else {
        global_map_ptr_.reset(new CloudData::CLOUD());
        for (size_t i = 0; i < global_map_frames_.size(); ++i) {
            pcl::transformPointCloud(*global_map_frames_.at(i).cloud_data.cloud_ptr, 
                                    *transformed_cloud_ptr, 
                                    global_map_frames_.at(i).pose);
            *global_map_ptr_ += *transformed_cloud_ptr;
        }
        has_new_global_map_ = true;
    }

本文主要参考 知乎专栏《从零开始自动驾驶定位》----by 任乾
PCL使用正态分布变换NDT(Normal Distributions Transform)进行配准