无人驾驶学习笔记-NDT 配准

已于 2022-05-20 01:08:00 修改
阅读量3k 收藏 40
点赞数 4
分类专栏： 无人驾驶学习笔记 文章标签： 学习 自动驾驶 算法
于 2022-05-19 15:34:34 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/p942005405/article/details/124863090
版权

目录

1. NDT 的算法处理流程

2. NDT 公式推导 

3. NDT 实例

        3.1 常规NDT的位姿估计

        3.2 front_end_node

1、ROS常规初始化

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

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

4、前端里程计

3.3 front_end

update()

UpdateNewFrame（）

1. NDT 的算法处理流程

 

 

2. NDT 公式推导 

下面部分引自无人驾驶汽车系统入门（十三）——正态分布变换（NDT）配准与无人车定位_AdamShan的博客-CSDN博客_ndt无人驾驶

使用正态分布来表示原本离散的点云有诸多好处，这种分块（通过 一个个cell）的光滑的表示是连续可导的，每一个概率密度函数可以被认为是一个局部表面的近似，它不但描述了这个表面在空间中的位置，同时还包含了这个表面的方向和光滑性等信息。

这里写图片描述

        上图中立方体的边长为1米，其中越明亮的位置表示概率越高。此外，局部表面的方向和光滑性则可以通过协方差矩阵的特征值和特征向量反映出来。我们以三维的概率密度函数为例，如果三个特征值很接近，那么这个正态分布描述的表面是一个球面，如果一个特征值远大于另外两个特征值，则这个正态分布描述的是一条线，如果一个特征值远小于另外两个特征值，则这个正态分布描述的是一个平面。下图描述了协方差矩阵特征值与表面形状之间的关系：

这里写图片描述

变换参数和最大似然

 3. NDT 实例

        NDT并没有比较两个点云点与点之间的差距，而是先将参考点云（即高精度地图）转换为多维变量的正态分布（使用正态分布来表示原本离散的点云），如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。因此，可以考虑用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好。

PCL 中ndt相关参数

1. setInputCloud (cloud_source) 设置原始点云 
2. setInputTarget (cloud_target) 设置目标点云 
3. setMaxCorrespondenceDistance（） 设置最大对应点的欧式距离，只有对应点之间的距离小于该设置值的对应点才作为ICP计算的点对。默认值为：1.7976931348623157e+308，基本上对所有点都计算了匹配点。 
4. 三个迭代终止条件设置： 
1) setMaximumIterations（） 设置最大的迭代次数。迭代停止条件之一 
2) setTransformationEpsilon（） 设置前后两次迭代的转换矩阵的最大容差（epsilion），一旦两次迭代小于这个最大容差，则认为已经收敛到最优解，迭代停止。迭代停止条件之二，默认值为：0 
3) setEuclideanFitnessEpsilon（） 设置前后两次迭代的点对的欧式距离均值的最大容差，迭代终止条件之三，默认值为：-std::numeric_limits::max () 
迭代满足上述任一条件，终止迭代。
5.getFinalTransformation () 获取最终的配准的转化矩阵，即原始点云到目标点云的刚体变换，返回Matrix4数据类型，该数据类型采用了另一个专门用于矩阵计算的开源c++库eigen。 
6. align (PointCloudSource &output) 进行ICP配准，输出变换后点云
7. hasConverged（） 获取收敛状态，注意，只要迭代过程符合上述三个终止条件之一，该函数返回true。 
8. min_number_correspondences_  最小匹配点对数量，默认值为3，即由空间中的非共线的三点就能确定刚体变换，建议将该值设置大点，否则容易出现错误匹配。 
9. 最终迭代终止的原因可从convergence_criteria_变量中获取
注意：getFitnessScore（）用于获取迭代结束后目标点云和配准后的点云的最近点之间距离的均值。

参考下面作者的代码示例 NDT在slam中前端的用法

从零开始做自动驾驶定位(四): 前端里程计之初试 - 知乎

3.1 常规NDT的位姿估计

#include <iostream>
#include <pcl/io/pcd_io.h>

#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include <pcl/visualization/pcl_visualizer.h>

// 读取点云pcd文件
pcl::PointCloud<pcl::PointXYZ>::Ptr read_cloud_point(std::string const &file_path){
    // Loading first scan.
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ> (file_path, *cloud) == -1)
    {
        PCL_ERROR ("Couldn't read the pcd file\n");
        return nullptr;
    }
    return cloud;
}

void visualizer(pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud){
    // 1、初始化点云可视化
    boost::shared_ptr<pcl::visualization::PCLVisualizer>
            viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer_final->setBackgroundColor (0, 0, 0);

    ///2、对目标点云和匹配后的点云上色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
            target_color (target_cloud, 255, 0, 0);
    viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
    viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                    1, "target cloud");

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
            output_color (output_cloud, 0, 255, 0);
    viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
    viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                    1, "output cloud");

    // 3、开始可视化，坐标系和相机参数
    viewer_final->addCoordinateSystem (1.0, "global");
    viewer_final->initCameraParameters ();

    ///4、Wait until visualizer window is closed.
    while (!viewer_final->wasStopped ())
    {
        viewer_final->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
}

int main(int argc, char **argv) {
    // 1、读取pcd文件（source和target）
    auto target_cloud = read_cloud_point(argv[1]);
    std::cout << "Loaded " << target_cloud->size () << " data points from cloud1.pcd" << std::endl;

    auto input_cloud = read_cloud_point(argv[2]);
    std::cout << "Loaded " << input_cloud->size () << " data points from cloud2.pcd" << std::endl;

    // 2、过滤输入点云（减少数据量到10%）
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
    approximate_voxel_filter.setLeafSize(0.2, 0.2, 0.2);
    approximate_voxel_filter.setInputCloud(input_cloud);
    approximate_voxel_filter.filter(*filtered_cloud);
    std::cout<<"Filtered cloud contains "<< filtered_cloud->size() << "data points from cloud2.pcd" << std::endl;
    
    // 3、初始化NDT并设置参数
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
    ndt.setTransformationEpsilon(0.01);// 优化过程是否收敛到的阈值
    ndt.setStepSize(0.1);// 设置牛顿法优化的最大步长
    ndt.setResolution(1.0);// 设置网格化时立方体的边长
    ndt.setMaximumIterations(35); // 优化的迭代次数

    ndt.setInputSource(filtered_cloud); // 待匹配点云
    ndt.setInputTarget(target_cloud); // 目标点云

    // 4、初始化变换参数
    Eigen::AngleAxisf init_rotation(0.6931, Eigen::Vector3f::UnitZ());
    Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
    Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();

    // 5、开始配准优化并保存配准后的点云图，保存下来
    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    ndt.align(*output_cloud, init_guess);
    std::cout << "Normal Distribution Transform has converged:" << ndt.hasConverged()
              << "score: " << ndt.getFitnessScore() << std::endl;

    pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation());
    pcl::io::savePCDFileASCII("cloud3.pcd", *output_cloud);

    // 6、可视化
    visualizer(target_cloud, output_cloud);

    return 0;
}

3.2 示例的前端激光雷达里程计流程

前端激光雷达里程计主要分为：点云下采样滤波、点云匹配、位姿估计、关键帧选取等步骤。具体流程为：

  • 第一帧点云数据设置为地图；

  • 提取关键帧点云，拼接成地图，保证稀疏点云数据；

  • 除了全局地图，还需要在当前帧附近形成滑动窗局部地图，减小计算量；

  • 匹配之前需要滤波，对点云稀疏化；可以使用PCL中的voxel_filter基本原理是把三维空间划分未等尺寸的立方体格子，在一个立方体格子内最多只留一个点，这样就起到稀疏作用。

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

3.3 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初始化

    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;

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

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

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

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

3.4 front_end

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

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

重要变量：

// 三个滤波器
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();// 获得位姿

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（）

主要分为四步骤

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


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

ppipp1109
关注
专栏目录
PCL学习：NDT配准
zfjBIT的专栏
07-03 2686
正态分布变换进行配准（normal Distributions Transform） 正态分布变换算法是一个配准算法，它应用于三维点的统计模型，使用标准最优化技术来确定两个点云间的最优匹配，因为其在配准的过程中不利用对应点的特征计算和匹配，所以时间比其他方法比较快。 点云下载：http://www.pointclouds.org/documentation/tutorials/normal_d...
点云配准3：3d-ndt算法在pcl上的实现以及参数设置
诺有缸的高飞鸟的博客
12-25 7264
3d-ndt算法在pcl上的使用方法以及参数设置
无人驾驶汽车系统入门（十三）——正态分布变换（NDT）配准与无人车定位
热门推荐
AdamShan的博客
02-01 2万+
无人驾驶汽车系统入门（十三）——正态分布变换（NDT）配准与无人车定位 定位即确定无人车在这个世界中的哪个位置，是无人驾驶技术栈中必不可少的一部分。对于无人车而言，对定位的要求极高，一般情况下，我们希望我们的无人车能够达到 厘米级 的定位精度，单纯使用GPS能够达到米级别的定位，显然，无人车的定位模块需要其他的技术支持以进一步提高定位的精度。本文介绍一种依赖于高精度地图和激光雷达的定位技术
Autoware 定位之ndt定位（八）
最新发布
lovely_yoshino的博客
09-07 7103
这一讲按照《》梳理的顺序，我们来说一说Autoware中的ndt定位，这个软件包有两个主要功能：1. 通过扫描匹配进行位置估计，2. 通过ROS服务使用蒙特卡洛方法估计初始位置。
NDT配准介绍
WIT牛逼的博客
02-03 306
应该认真总结一下这方面内容
点云匹配 5之 正态分布变换配准(NDT)
weixin_39354845的博客
12-21 1409
正态分布变换配准(NDT)
NDT配准原理
xinxiangwangzhi_的博客
05-29 2334
文章目录NDT预备知识正态分布高斯牛顿法求解非线性最小二乘NDT原理(1)目标函数(2)简化目标函数(3)数值求解（4）算法流程：（5）相比ICP的优势NDT源码解析与其在PCL应用参考资料 NDT预备知识 正态分布 n维正态随机过程中，其概率密度函数为： p(x⃗)=1(2π)D/2∣Σ∣exp⁡(−(x⃗−μ⃗)TΣ−1(x⃗−μ⃗)2)(1) p(\vec{x})=\frac{1}{(2 \pi)^{D / 2} \sqrt{|\boldsymbol{\Sigma}|}} \exp \left(-\f
点云匹配方法NDT(正态分布变换)
人无远虑，必有近忧
05-07 5306
在点云匹配中，ICP基于距离直接最优化变换矩阵的参数，由于是欠定方程且旋转矩阵的约束，使得结果很难优化，为此在新的维度优化变换矩阵的参数，被很好的提出：先将参考点云（目标点云）转换为多维变量的正态分布，匹配的点云如果采用某组变换参数后，使得新的点云和目标点云的正态分布参数匹配很好，那么变换点在参考系中的概率密度将会很大。因此，可以考虑用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好。
05-点云配准数学原理 - 黑马机器人 _ PCL-3D点云1
08-04
点云配准算法通常包括迭代最近点（ICP）、正态分布变换（NDT）以及基于特征不变性的配准（如ORB、FPFH、NARF）等。 1. **迭代最近点（ICP）**：ICP是一种常用的配准方法，其基本思想是不断迭代地寻找源点云中每个点...
NDT.zip_8MF_NDT点云配准_NDT算法_ndt配准 c++_点云ndt
09-21
通过阅读代码，可以学习到NDT算法的具体实现细节，以及如何将理论知识转化为实际工程应用。 总之，NDT算法在点云配准中发挥着重要作用，它的C++实现不仅展示了算法的精妙，也为我们提供了在实际项目中应用点云处理...
ICP+NDT点云配准
05-28
PCL1.8.0+VS2013编写，利用ICP进行粗略配准，利用NDT进行精确配准，当两个点云重叠率较大时有较好的效果，点云数据是用bun000和bun045，今后可能会上传NDT+ICP进行配的代码。
点云粗匹配ndt.zip
12-25
括NDT粗配准算法，并计算出误差。
Ndt+ICP配准算法亲测可用
04-19
点云配准代码，读取PCD文件，若要读取PLY文件，需要自己改代码，或者将ply文件转换成pcd的，我之后会上传转换的代码，积分都是为了交换下载资源用
SAC-IA粗配准+NDT精配准
peach_blossom的博客
11-11 1万+
接上一篇讲的SAC-IA+ICP的配准方案，这一篇讲一下SAC-IA+ICP的方案。正态分布变换算法（3D-NDT） 正态分布变换算法应用于三维点的统计模型，使用标准最优化技术来确定两个点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快，可用于点较多时的配准过程。其原理可参考Martin Magnusson的The Three-Dimensional Norm
激光雷达点云配准NDT算法
qq_53576285的博客
03-23 2306
点云配准主流的有ICP和NDT算法，都是为了通过刚性变化实现两帧点云的对准。主要目的是在一个优化目标下，求出刚性变化的旋转矩阵R和平移矩阵T。关于ICP算法的推导，可以参考前段时间更新的博客：激光雷达点云配准算法_ego_vehicle的博客-CSDN博客。
【定位】正态分布变换（NDT）配准与无人车定位
笑扬轩逸的博客
07-05 9606
文章出处：http://blog.csdn.net/adamshan/article/details/79230612 定位即确定无人车在这个世界中的哪个位置，是无人驾驶技术栈中必不可少的一部分。对于无人车而言，对定位的要求极高，一般情况下，我们希望我们的无人车能够达到 厘米级 的定位精度，单纯使用GPS能够达到米级别的定位，显然，无人车的定位模块需要其他的技术支持以进一步提高定位的精度。...
3d激光雷达开发（ndt匹配）
嵌入式-老费，一个分享专业嵌入式知识的blog
03-09 1003
【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】 除了icp匹配之外，ndt匹配也是使用比较多的一种方法。相比较icp而言，ndt匹配花的时间要少一些。此外，ndt匹配还需要输入估计的yaw、pitch、roll、x、y、z，这个可以根据经验值给出。猜的越准，算法收敛的越快。参考的代码出处在这，https://pcl.readthedocs.io/projects/tutorials/en/master/normal_distrib...
NDT点云配准
m0_48748418的博客
09-05 3195
NDT配准介绍
改进3D-NDT算法提升室内移动机器人点云配准性能
总结起来，这项研究对于增强室内移动机器人在各种环境下的适应能力具有重要意义，特别是在纹理匮乏的区域，改进的3D-NDT点云配准算法能够提供更稳定和高效的定位服务，这对于无人驾驶、服务机器人等领域有着实际应用...
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

ppipp1109

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值