PCL 点云配准 3D-NDT算法（精配准）

目录

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

[2.1.1 加载点云数据函数](#2.1.1 加载点云数据函数)

[2.1.2 执行 NDT 算法配准函数](#2.1.2 执行 NDT 算法配准函数)

[2.1.3 可视化配准结果函数](#2.1.3 可视化配准结果函数)

2.2完整代码

三、实现效果

---

PCL点云算法汇总及实战案例汇总的目录地址链接：

PCL点云算法与项目实战案例汇总（长期更新）

---

一、概述

**3D-NDT (3D Normal Distributions Transform)**是一种基于概率分布的点云配准算法。在 3D-NDT 中，点云被划分为多个体素单元，每个体素内的点云分布被建模为一个高斯分布，配准的目标是通过最大化源点云在目标点云的高斯模型下的对数似然来优化位姿变换矩阵。

与经典的 ICP (Iterative Closest Point) 算法相比，3D-NDT 对噪声点云的鲁棒性较强，且可以更高效地进行大规模点云数据的配准。

1.1原理

NDT 算法的主要步骤包括：

1. **网格划分：**将空间划分为固定大小的网格。
2. **高斯分布拟合：**对目标点云的每个网格单元拟合一个高斯分布。
3. **源点云匹配：**通过源点云的点与目标点云对应网格的高斯分布进行匹配，评估配准质量。
4. **优化刚体变换：**通过优化使得源点云与目标点云的高斯分布尽量匹配。

NDT 配准的核心是找到能够最小化源点云在目标点云高斯分布中的分布差异的刚体变换矩阵。

1.2实现步骤

1. **加载点云数据：**加载源点云和目标点云。
2. **初始化 NDT 对象：**设置 NDT 配准对象，并指定所需的参数，如分辨率、迭代次数、最小转换差异等。
3. **执行配准：**通过 3D-NDT 进行点云配准，输出最终的变换矩阵。
4. 可视化：通过 PCL 的 PCLVisualizer 对源点云、目标点云和配准后的点云进行可视化。

1.3应用场景

1. 大规模环境建模： 在 3D 地图重建中，使用 3D-NDT 可以对多帧点云数据进行有效配准。
2. 自动驾驶环境感知： 在自驾驶系统中，3D-NDT 可用于精确对齐激光雷达的点云数据。
3. 机器人导航： 在室内或室外的导航过程中，通过 3D-NDT对当前点云与已知地图进行配准，以确定机器人的姿态和位置。

二、代码实现

2.1关键函数

2.1.1 加载点云数据函数

pcl::PointCloud<pcl::PointXYZ>::Ptr load_point_cloud(const std::string& 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("无法读取文件 %s\n", file_path.c_str());
        return nullptr;
    }
    std::cout << "从 " << file_path << " 读取了 " << cloud->size() << " 个点." << std::endl;
    return cloud;
}

2.1.2 执行 NDT 算法配准函数

void run_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr& source, pcl::PointCloud<pcl::PointXYZ>::Ptr& target, Eigen::Matrix4f& final_transform, pcl::PointCloud<pcl::PointXYZ>::Ptr& aligned_cloud)
{
    // 初始化 NDT 对象
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
    ndt.setResolution(1.0);  // 设置 NDT 的分辨率
    ndt.setStepSize(0.1);  // 设置优化步长
    ndt.setTransformationEpsilon(1e-6);  // 设置终止条件
    ndt.setMaximumIterations(35);  // 设置最大迭代次数

    // 设置输入点云
    ndt.setInputSource(source);
    ndt.setInputTarget(target);

    // 执行 NDT 配准
    ndt.align(*aligned_cloud);
    final_transform = ndt.getFinalTransformation();  // 获取最终变换矩阵

    std::cout << "NDT 配准完成，最终得分: " << ndt.getFitnessScore() << std::endl;
}

2.1.3 可视化配准结果函数

void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source, pcl::PointCloud<pcl::PointXYZ>::Ptr& target, pcl::PointCloud<pcl::PointXYZ>::Ptr& aligned)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("NDT 配准结果"));

    viewer->setBackgroundColor(0, 0, 0);  // 设置背景颜色为黑色

    // 目标点云为红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);
    viewer->addPointCloud(target, target_color, "target cloud");

    // 源点云为绿色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);
    viewer->addPointCloud(source, source_color, "source cloud");

    // 配准后的点云为蓝色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned, 0, 0, 255);
    viewer->addPointCloud(aligned, aligned_color, "aligned cloud");

    viewer->spin();
}

2.2完整代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>    // NDT 头文件
#include <pcl/visualization/pcl_visualizer.h> // 可视化
#include <boost/thread/thread.hpp>

using namespace std;

// NDT配准函数
void ndt_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud,
    Eigen::Matrix4f& final_transform, pcl::PointCloud<pcl::PointXYZ>::Ptr& aligned_cloud)
{
    // 初始化NDT对象
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

    // 设置NDT参数
    ndt.setResolution(1.0);                  // 设置分辨率
    ndt.setMaximumIterations(35);            // 设置最大迭代次数
    ndt.setTransformationEpsilon(1e-8);      // 为终止条件设置最小转换差异
    ndt.setStepSize(0.1);                    // 设置步长
    ndt.setInputSource(source_cloud);        // 设置输入源点云
    ndt.setInputTarget(target_cloud);        // 设置输入目标点云

    // 设置初始变换矩阵
    Eigen::Matrix4f initial_guess = Eigen::Matrix4f::Identity();

    // 执行NDT配准
    ndt.align(*aligned_cloud, initial_guess);

    // 检查配准是否成功
    if (ndt.hasConverged()) {
        final_transform = ndt.getFinalTransformation();
        std::cout << "NDT配准成功，配准得分: " << ndt.getFitnessScore() << std::endl;
    }
    else {
        std::cerr << "NDT配准失败！" << std::endl;
    }
}

// 可视化函数
void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& aligned)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("配准结果"));
    viewer->setBackgroundColor(0, 0, 0);  // 设置背景颜色为黑色

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);
    viewer->addPointCloud(target, target_color, "target cloud");

    /*pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);
    viewer->addPointCloud(source, source_color, "source cloud");*/

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned, 0, 0, 255);
    viewer->addPointCloud(aligned, aligned_color, "aligned cloud");

    viewer->spin();
}

int main()
{
    // --------------------加载点云数据-----------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("1.pcd", *source);
    pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("2.pcd", *target);

    // 输出点云数据个数
    std::cout << "源点云点数: " << source->points.size() << std::endl;
    std::cout << "目标点云点数: " << target->points.size() << std::endl;

    // NDT配准
    pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>);
    Eigen::Matrix4f final_transform;
    ndt_registration(source, target, final_transform, aligned);

    // 输出变换矩阵
    std::cout << "最终变换矩阵:\n" << final_transform << std::endl;

    // 可视化配准结果
    visualize_registration(source, target, aligned);

    return 0;
}

三、实现效果

NDT配准成功，配准得分: 2.24138e-05
最终变换矩阵:
  0.923224   -0.37732   0.072716  0.0179218
  0.353408   0.908046   0.224846 -0.0605567
 -0.150868  -0.181885   0.971677  0.0367289
         0          0          0          1