NDT和ICP构建点云地图 |【点云建图、Ubuntu、ROS】-EW帮帮网

###

本博客记录学习NDT，ICP构建点云地图的实验过程，参考的以下两篇博客：

无人驾驶汽车系统入门（十三）——正态分布变换（NDT）配准与无人车定位_settransformationepsilon-CSDN博客

###

一、bag包点云数据处理

1.1 首先查看bag包的信息

rosbag info <包名>

通过rosbag info 可以查看bag数据包的信息，主要是一些话题名称和消息类型。

1.2 对点云数据转化为pcd格式保存

PCD（Point Cloud Data）是专为点云设计的文件格式，支持存储多维数据（如坐标、颜色、强度、法向量、时间戳等）。其核心优势在于：

标准化
高效存储
完整保留属性
生态支持广泛

rosrun pcl_ros bag_to_pcd <bag数据包名称>.bag <点云话题名称> <保存的文件夹名>.pcd

随后，在同目录下生成data.pcd文件夹，存放点云数据数据：

这样，点云数据处理完成。

二、NDT构建点云地图

2.1 NDT介绍

正态分布变换（Normal Distributions Transform，NDT）是一种依赖于高精地图和激光雷达的定位技术。是一类利用已有的高精度地图和激光雷达实时测量数据实现高精度定位的技术。其核心思想是将点云空间划分为多个网格（Cell），对每个网格中的点云拟合一个正态分布（高斯分布），然后用概率模型描述整个点云。通过优化匹配概率，找到两个点云之间的最佳变换（旋转和平移）。

2.2 NDT算法步骤

2.2.1 划分体素网格

将参考点云划分为均匀的立方体网格（体素）。每个体素的大小需根据场景调整通常在0.1m到数米之间。对每个体素内的点云，计算均值向量 $\mu$ 和协方差矩阵 $\sum$ ：

2.2.2 初始化变换参数

给定初始猜测变换 $T_{init}$ （如里程计估计或前一帧位姿），将当前点云 $P_{current}$ 投影到参考坐标系。

2.2.3 构建目标函数

对于变换后的当前点 $T_{init}$ ，找到其所在参考体素，计算其在该体素分布下的概率密度：

其中k为维度（2D为3，3D为6）。

目标函数：

2.2.4 优化求解变换参数

在NDT（正态分布变换）算法中，构建完目标函数后，意味着将点云配准问题转化为一个数值优化问题，接下来需要通过优化算法求解最优的变换参数（旋转和平移），使得目标函数值最大化（或最小化，取决于定义）。这一步骤是NDT的核心，直接决定了配准的精度和效率。

使用迭代优化，在每次迭代中，首先计算目标函数的梯度和Hessian矩阵以确定优化方向与步长，随后利用牛顿法或Levenberg-Marquardt等算法更新变换参数，并通过收敛条件（如梯度阈值、迭代次数或参数变化量）判断是否终止。这一过程逐步将当前点云对齐至参考点云的高概率区域，最终输出最优变换矩阵，完成配准。迭代优化的效率与精度高度依赖初始位姿、体素大小及优化算法的选择。

三、代码介绍

3.1 读取点云与NaN过滤

从PCD文件加载点云，并移除无效点（NaN）

pcl::PointCloud<pcl::PointXYZ>::Ptr read_cloud_point(std::string const &file_path) {
    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;
    }
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
    return cloud;
}

3.2 可视化函数

显示目标点云（红色）和配准结果点云（绿色）。

void visualizer(pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud, 
               pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud) {
    // ...（颜色设置、坐标系、相机参数等）
    while (!viewer_final->wasStopped()) {
        viewer_final->spinOnce(100);
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
}

3.3 主函数：数据加载与预处理

加载目标点云和待配准点云。对输入点云进行体素滤波降采样（减少计算量）。

auto target_cloud = read_cloud_point(argv[1]);
auto input_cloud = read_cloud_point(argv[2]);
pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
approximate_voxel_filter.setLeafSize(0.2, 0.2, 0.2);
approximate_voxel_filter.filter(*filtered_cloud);

3.4 主函数：NDT配准核心配置

首先创建NDT配准对象并设置关键参数（包括收敛阈值0.01、优化步长0.05、网格分辨率0.5和最大迭代次数50），然后指定待配准的输入点云（降采样后的filtered_cloud）和目标点云（target_cloud）；接着通过初始旋转（绕Z轴0.6931弧度）和平移（X=1.79387,Y=0.720047）构造初始位姿猜测init_guess，最后调用align方法执行配准，输出配准后的点云output_cloud，其中配准过程会不断优化变换矩阵直到满足收敛条件或达到最大迭代次数。

pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
ndt.setTransformationEpsilon(0.01);  // 收敛阈值
ndt.setStepSize(0.05);              // 优化步长
ndt.setResolution(0.5);             // NDT网格分辨率
ndt.setMaximumIterations(50);       // 最大迭代次数
ndt.setInputSource(filtered_cloud); // 输入点云（降采样后）
ndt.setInputTarget(target_cloud);   // 目标点云
 
// 初始位姿猜测
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();
 
// 执行配准
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
ndt.align(*output_cloud, init_guess);

四、操作步骤

首先创建一个工作空间NDT_ws，并在该工作空间下创建src文件夹存放源码：

 mkdir -p NDT_ws/src

然后再src文件夹下创建ndt_node.cpp文件，将代码写入改文件中，然后在NDT_ws目录下编写CMakeLists.txt文件：

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(ndt_node)
 
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
find_package(PCL 1.5 REQUIRED)
find_package(Boost REQUIRED COMPONENTS system)  
 
include_directories(
    ${PCL_INCLUDE_DIRS}
    ${Boost_INCLUDE_DIRS} 
)
 
link_directories(${PCL_LIBRARY_DIRS})
 
add_executable(ndt src/ndt_node.cpp)
 
target_link_libraries(ndt
    ${PCL_LIBRARIES}
    ${Boost_LIBRARIES} 
)

在工作空间目录下创建build文件夹，存放编译过程文件，然后进行编译：

mkdir build
cd build
Cmake ..
make

编译完成后生成可执行文件ndt，并将之前处理过的pcd文件放到该目录下：

随后执行可执行文件即可：

输出显示，从文件1.pcd加载了51574个数据点，从文件2.pcd加载了51626个数据点，原始点云可能经过体素网格滤波（Voxel Grid Filter）或统计离群值滤波（Statistical Outlier Removal）等处理，最终保留了 22,837 个数据点（减少约 55%）。滤波的目的是去除冗余或噪声数据，提升后续计算效率。

NDT配准，NDT converged ：1表示算法成功收敛，找到了最优变换矩阵。配准分数为56.2459。若是最终匹配的结果不准确，可调整NDT参数。