slam定位学习笔记(二)

参考学习文章，感谢大佬的无私奉献。

主要学习内容是前端里程计的结构设计与实现。抽象概括里程计的核心其实是一个匹配问题。

一、里程计的工作流程

1）从接收到一帧点云开始，首先是要匹配，和地图匹配，如果它是第一帧数据，那么它就是地图，供下一帧匹配使用。

2）我们不能把每一帧匹配好的点云都加入地图，那样太大了，所以要提取关键帧，即每隔一段距离取一帧点云，用关键帧拼接成地图即可。

3）到这里，会想到一个问题，那就是地图会一直累加，那么我们一直用它匹配会导致很多不必要的计算量，所以应该还需要一个小地图，即把和当前帧一定距离范围内的关键帧找出来拼接即可，可以叫它滑窗。

4）在匹配之前需要滤波，对点云稀疏化，不然匹配会非常慢。

5) 点云匹配还有一个特性，就是它对位姿的预测值比较敏感，所以在载体运动时，不能以它上一帧的位姿作为这一帧的预测值，可以使用IMU预测，也可以使用运动模型预测。

 大佬总结的很清晰了，主要说一下我的理解：

关于第2)点，因为实际采取数据的时候，lidar每一秒会采集很多帧的点云所有使用每一帧点云会带来巨大的计算量，而且数据的冗余不好解决，所有隔一段距离来取关键帧比较合理。这里采用的是曼哈顿距离2m。

关于第3)点我是这样理解的，他主要是寻找关键帧的一种方法，第2)说的是全局的地图，而第3)说就是全局地图用来匹配会有很多不必要的计算量，因为匹配的只会是地图的一部分，其它部分用不上，这样的话只使用当前帧来寻找和它一段距离内的关键帧就比较合理，然后在把当前帧和全局地图进行拼接，这个关键帧就变成了当前帧，然后寻找它的关键帧，依次迭代。

关于第4)点，这里类似于视觉里面的角点、特征点的概念。

第5)点还不太理解，以后懂了补上。

二、点云匹配

主要使用pcl库中的ndt（Normal Distribution Transform）正态分布变换匹配方法，关于ndt匹配方法原理、相关的参数与概念可以参考：文章一，文章二，文章三

代码部分设计ndt的主要是关于ndt的定义和voxel滤波器的设置，我把他们全部提取出来：
 

//front_end.hpp
    //voxel滤波器：
    pcl::VoxelGrid<CloudData::POINT> cloud_filter_;
    pcl::VoxelGrid<CloudData::POINT> local_map_filter_;
    pcl::VoxelGrid<CloudData::POINT> display_filter_;
    //ndt初始化：
    pcl::NormalDistributionsTransform<CloudData::POINT, CloudData::POINT>::Ptr ndt_ptr_;

//front_end.cpp
   //voxel滤波器：
    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：
    ndt_ptr_->setResolution(1.0);
    ndt_ptr_->setStepSize(0.1);
    ndt_ptr_->setTransformationEpsilon(0.01);
    ndt_ptr_->setMaximumIterations(30);

关于voxel这篇文章讲的很详细，setLeafSize后面的三个参数表示体素栅格叶大小，分别表示体素在XYZ方向的尺寸，以米为单位。所有关于cloud_filter_设置的就是1.3m，local_map_filter_设置的是0.6米，display_filter_设置的是0.5米。

关于ndt的参数在前面的参考文章中讲的很详细，setResolution是设置网格体化时的边长，网格大小设置在NDT中非常重要，太大会导致精度不高，太小导致内存过高，并且只有两幅点云相差不大的情况才能匹配。setStepSize为More-Thuente线搜索设置最大步长，即设置牛顿法优化的最大步长。setTransformationEpsilon即设置变换的（两个连续变换之间允许的最大差值），这是判断我们的优化过程是否已经收敛到最终解的阈值。setMaximumlterations设置匹配迭代的最大次数，即当迭代次数达到30次或者收敛到阈值时，停止优化。 一般来说在这个限制值之前，优化程序会在epsilon（最大容差）变换阀值下终止，添加最大迭代次数限制能够增加程序的鲁棒性阻止了它在错误的方向上运行时间过长。

然后是初始化后的ndt与voxel函数的使用部分，全部提取出来：

//front_end.cpp 重复部分忽略
  //voxel滤波器部分
    cloud_filter_.setInputCloud(current_frame_.cloud_data.cloud_ptr);
    cloud_filter_.filter(*filtered_cloud_ptr);

  //ndt部分
     ndt_ptr_->setInputSource(filtered_cloud_ptr);
     ndt_ptr_->align(*result_cloud_ptr_, predict_pose);
     current_frame_.pose = ndt_ptr_->getFinalTransformation();
     ndt_ptr_->setInputTarget(local_map_ptr_);

其中，voxel部分的setInputCloud部分是设置需要被voxel滤波器过滤的点云，filter是过滤后的点云赋值给它的参数。ndt部分的setInputSource是输入需要ndt匹配的输入点云1，setInputTarget是输入目标点云2，就是将点云1匹配到点云2中。align函数是通过设置的预测初值进行ndt配准，如果初值给的好配准的时间就短，然后结果放在了第一个参数上面。

准备学完这篇文章后做一个关于ndt变换的练习。

三、关键帧

主要是前面定义的CloudData定义的点云结构加上一个位姿pose，将它定义在了FrontEnd的类中：

//FrontEnd class 中
    public:
    class Frame {
      public:  
        Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
        CloudData cloud_data;
    };
    private:
    void UpdateNewFrame(const Frame& new_key_frame);

    std::deque<Frame> local_map_frames_;
    std::deque<Frame> global_map_frames_;

    Frame current_frame_;

关于UpdataNewFrame函数的定义：

void FrontEnd::UpdateNewFrame(const Frame& new_key_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());
    
    // 更新局部地图
    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;

    // 更新ndt匹配的目标点云
    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);
    }

    // 更新全局地图
    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;
    }
}

其中：

key_frame.cloud_data.cloud_ptr.reset(new CloudData::CLOUD(*new_key_frame.cloud_data.cloud_ptr));

reset是智能指针的函数，作用是类内初始化，具体可以看文章一、文章二。

local_map_frames_队列存的是局部的地图，然后就是这个for循环：
 

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

使用到了pcl::transformPointCloud函数，这个函数之前在frame_test_node.cpp里面遇到过，就是将参数一的点云按照参数三转换，然后将转换后的结果赋值给参数二。然后就是将参数二累加到local_map_ptr_中去，这个是局部点云地图的指针。之后就是更新ndt匹配的目标点云，使用了ndt_ptr_的setInputTarget函数参数是前面累加的局部点云指针。这里还用到了点云数量的判断，如果小于10帧就直接使用局部点云指针，如果大于10帧就先用voxel滤波器先降采样，然后就使用采样后的。然后全局地图的更新与局部地图的更新类似只是没有ndt匹配的环节。

然后就是三个函数，分别对应当前帧，局部点云地图帧和全局点云地图帧：

bool FrontEnd::GetNewLocalMap(CloudData::CLOUD_PTR& local_map_ptr) {
    if (has_new_local_map_) {
        display_filter_.setInputCloud(local_map_ptr_);
        display_filter_.filter(*local_map_ptr);
        return true;
    }
    return false;
}

bool FrontEnd::GetNewGlobalMap(CloudData::CLOUD_PTR& global_map_ptr) {
    if (has_new_global_map_) {
        display_filter_.setInputCloud(global_map_ptr_);
        display_filter_.filter(*global_map_ptr);
        return true;
    }
    return false;
}

bool FrontEnd::GetCurrentScan(CloudData::CLOUD_PTR& current_scan_ptr) {
    display_filter_.setInputCloud(result_cloud_ptr_);
    display_filter_.filter(*current_scan_ptr);
    return true;
}

主要作用就是将得到的点云地图利用voxel滤波器过滤然后将结果赋值给参数，再返回bool值。

四、滑窗

使用deque将一个个关键帧存起来，然后设置一个数量就当超过这个数量就把最前面的数据踢出去。

    // 更新局部地图,以20帧为界
    local_map_frames_.push_back(key_frame);
    while (local_map_frames_.size() > 20) {
        local_map_frames_.pop_front();
    }

五、点云滤波

使用的是pcl中的voxel_filter器，具体分析在第三节写的很详细。主要作用是对于点云进行降采样，做到稀疏的作用。

六、位姿预测

具体代码实现是在front_end.cpp中的Updata函数里面：

Eigen::Matrix4f FrontEnd::Update(const CloudData& cloud_data) {
    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_;

    // 局部地图容器中没有关键帧，代表是第一帧数据
    // 此时把当前帧数据作为第一个关键帧，并更新局部地图容器和全局地图容器
    if (local_map_frames_.size() == 0) {
        current_frame_.pose = init_pose_;
        UpdateNewFrame(current_frame_);
        return current_frame_.pose;
    }

    // 不是第一帧，就正常匹配
    ndt_ptr_->setInputSource(filtered_cloud_ptr);
    // 这里是初值吗？
    ndt_ptr_->align(*result_cloud_ptr_, predict_pose);
    current_frame_.pose = ndt_ptr_->getFinalTransformation();

    // 更新相邻两帧的相对运动
    // 假设来运速运动，前一帧和后一帧之间的位移是相同的
    step_pose = last_pose.inverse() * current_frame_.pose;
    predict_pose = current_frame_.pose * step_pose;
    last_pose = current_frame_.pose;

    // 匹配之后根据距离判断是否需要生成新的关键帧，如果需要，则做相应更新
    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;
}

 Updata函数传入的参数是点云结构，输出的是当前帧点云记录的pose(就是frame类中定义的pose)。整个函数运行流程如下：首先将输入的参数去无效点，然后使用voxel定义的cloud_fliter_过滤得到flitered_cloud_ptr。

这里还有一个核心的函数：

    current_frame_.pose = ndt_ptr_->getFinalTransformation();

 我是这样理解的，这里得到的应该是这一帧点云在ndt配准后得到的这一帧点云与前一帧点云的变换位姿。

然后就是关于位姿预测的矩阵定义：

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

step_pose是两帧点云之间的步长；

last_pose是之前帧点云的pose；

predict_pose是根据前一帧和当前帧来预测的未来一帧点云的pose

last_key_frame_pose是前一个关键帧的pose。

七、front_end_node.cpp部分

front_end_node.cpp:

#include <ros/ros.h>
#include <pcl/common/transforms.h>
#include "glog/logging.h"

#include "lidar_localization/global_defination/global_defination.h"
#include "lidar_localization/subscriber/cloud_subscriber.hpp"
#include "lidar_localization/subscriber/imu_subscriber.hpp"
#include "lidar_localization/subscriber/gnss_subscriber.hpp"
#include "lidar_localization/tf_listener/tf_listener.hpp"
#include "lidar_localization/publisher/cloud_publisher.hpp"
#include "lidar_localization/publisher/odometry_publisher.hpp"
#include "lidar_localization/front_end/front_end.hpp"

using namespace lidar_localization;

int main(int argc, char *argv[]) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_log_dir = WORK_SPACE_PATH + "/Log";
    FLAGS_alsologtostderr = 1;

    ros::init(argc, argv, "front_end_node");
    ros::NodeHandle nh;

    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);
    std::shared_ptr<TFListener> lidar_to_imu_ptr = std::make_shared<TFListener>(nh, "velo_link", "imu_link");

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

    std::deque<CloudData> cloud_data_buff;
    std::deque<IMUData> imu_data_buff;
    std::deque<GNSSData> gnss_data_buff;
    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;

    ros::Rate rate(100);
    while (ros::ok()) {
        ros::spinOnce();

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

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

                    Eigen::Matrix4f odometry_matrix = Eigen::Matrix4f::Identity();

                    if (!gnss_origin_position_inited) {
                        gnss_data.InitOriginPosition();
                        gnss_origin_position_inited = true;
                    }
                    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();
                    odometry_matrix *= lidar_to_imu;
                    gnss_pub_ptr->Publish(odometry_matrix);

                    if (!front_end_pose_inited) {
                        front_end_pose_inited = true;
                        front_end_ptr->SetInitPose(odometry_matrix);
                    }
                    front_end_ptr->SetPredictPose(odometry_matrix);
                    Eigen::Matrix4f laser_matrix = front_end_ptr->Update(cloud_data);
                    laser_odom_pub_ptr->Publish(laser_matrix);

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

        rate.sleep();
    }

    return 0;
}

和frame_test_node.cpp相比多出了local_map_pub_ptr、global_map_pub_ptr、laser_odom_pub_ptr、gnss_pub_ptr还有front_end_ptr前端结构。

其中local_map_pub_ptr是用来发布那20帧的局部地图的。global_map_pub_ptr是用来发布全局地图，每100帧收集融合一次最后融合到一起形成全局点云地图显示。laser_odom_pub_ptr发布的是每一帧点云和之前的点云进行ndt匹配然后得到的pose，然后将它的值赋值给odometry在发布。然后就是分别对前面一节最后的几个bool函数，如果判断是true就发布点云地图，以及最后的当时间大于460s后发布全局地图。这些地图最后都是经过降采样的。

八、总结

从发布的里程计结果来看，总共发布了两个odometry：gnss_pub_ptr和laser_odom_pub_ptr。其中gnss_pub_ptr是kitti数据集中采集的gnss信息是作为真值来对比使用。而laser_odom_pub_ptr发布的是每一帧点云和上一帧点云经过ndt配准后得到的转换位姿矩阵。

从发布的点云结果来看，总共发布了三个点云地图：cloud_pub_ptr、local_map_pub_ptr和global_map_pub_ptr。

cloud_pub_ptr发布的是当前帧的点云，注意这里的当前帧点云并不是完整的kitti包发布的当前帧点云，还经过了两个voxel滤波器cloud_filter_和display_filter_过滤的点云指针。

//cloud_pub_ptr_部分:
 //front_end_node.cpp
   front_end_ptr->GetCurrentScan(current_scan_ptr);
   cloud_pub_ptr->Publish(current_scan_ptr);

 //front_ned.cpp
   ndt_ptr_->align(*result_cloud_ptr_, predict_pose);
   bool FrontEnd::GetCurrentScan(CloudData::CLOUD_PTR& current_scan_ptr) {
        display_filter_.setInputCloud(result_cloud_ptr_);
        display_filter_.filter(*current_scan_ptr);
        return true;
}

local_map_pub_ptr发布的是局部小地图，它的发布是有条件的是，条件是has_new_local_map_是true，这个判断的是在函数UpdateNewFrame中，它里面还有对于局部地图的区分，如果帧数在10以内就直接作为ndt配准的target，如果大于10就先使用local_map_filter_滤波器进行降采样，然后将降采样的结果进行ndt陪准(做为target)。最后输出也要使用display_filter_进行滤波器过滤。

//local_map_pub_ptr_部分：
//front_end_node.cpp:
  if (front_end_ptr->GetNewLocalMap(local_map_ptr))
  local_map_pub_ptr->Publish(local_map_ptr);

//front_end.cpp:
bool FrontEnd::GetNewLocalMap(CloudData::CLOUD_PTR& local_map_ptr) {
    if (has_new_local_map_) {
        display_filter_.setInputCloud(local_map_ptr_);
        display_filter_.filter(*local_map_ptr);
        return true;
    }
    return false;
}
//在UpdataNewFrame里面：
    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;

global_map_pub_ptr，它是每100帧点云叠加一次，但后面只会使用一个display_filter_进行一次滤波器过滤，最后经过460s才会输出且只输出一次。

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


bool FrontEnd::GetNewGlobalMap(CloudData::CLOUD_PTR& global_map_ptr) {
    if (has_new_global_map_) {
        display_filter_.setInputCloud(global_map_ptr_);
        display_filter_.filter(*global_map_ptr);
        return true;
    }
    return false;
}



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

另外的核心是front_end_ptr指针，它的作用是将当前帧点云传入自己的Updata函数，然后在这个函数里面进行所有的数据处理，包含上面说的所有的点云地图的处理，在前面都写的比较详细了。

总结：所以主要就是发布两个里程计一个用的是gnss作为真值，另一个是用ndt配准的算法得到的里程计，两个对比。然后就是发布的点云环节，发布三个点云地图，对应于前面的整体框架流程，点云的处理，对于关键帧的选取是按照它们两帧之间的距离为2来选取的。然后就是对于局部地图的处理，局部地图的处理，局部地图限制为20帧，当0到10帧的时候就是直接使用叠加的局部点云地图进行ndt配准，当大于10帧的时候就要先使用voxel滤波器进行过滤然后进行ndt配准。并且把相应的flag位置为true。全局地图是一样的流程。比较核心的是两个函数Updara和UpdataNewFrame这两个函数，Updata是公有类成员函数，UpdataNewFrame是私有的，并且只在Updata函数中出现，出现了两次，第一次是对第一帧点云进行处理，第二次是对非第一帧点云进行处理，这里就对应了前面的流程步骤。在函数中，如果没有到距离2的点云帧不会更新关键帧，但还是会输出它的pose，这里有一个隐含条件是在2米内估算vehicle是匀速运动的。这里就有不同的地方，不是每一帧点云都会输出它的pose。但只有关键帧才会进行UpdataNewFrame从而得到新的位姿估计。这也导致了输出的两个odometry不同。