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 加载点云数据函数

cpp 复制代码
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 算法配准函数

cpp 复制代码
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 可视化配准结果函数

cpp 复制代码
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完整代码

cpp 复制代码
#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;
}

三、实现效果

cpp 复制代码
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
相关推荐
charley.layabox2 小时前
8月1日ChinaJoy酒会 | 游戏出海高端私享局 | 平台 × 发行 × 投资 × 研发精英畅饮畅聊
人工智能·游戏
DFRobot智位机器人2 小时前
AIOT开发选型:行空板 K10 与 M10 适用场景与选型深度解析
人工智能
想成为风筝4 小时前
从零开始学习深度学习—水果分类之PyQt5App
人工智能·深度学习·计算机视觉·pyqt
F_D_Z4 小时前
MMaDA:多模态大型扩散语言模型
人工智能·语言模型·自然语言处理
大知闲闲哟5 小时前
深度学习G2周:人脸图像生成(DCGAN)
人工智能·深度学习
m0_535064605 小时前
C++模版编程:类模版与继承
java·jvm·c++
飞哥数智坊5 小时前
Coze实战第15讲:钱都去哪儿了?Coze+飞书搭建自动记账系统
人工智能·coze
wenzhangli75 小时前
低代码引擎核心技术:OneCode常用动作事件速查手册及注解驱动开发详解
人工智能·低代码·云原生
今天背单词了吗9805 小时前
算法学习笔记:19.牛顿迭代法——从原理到实战,涵盖 LeetCode 与考研 408 例题
笔记·学习·算法·牛顿迭代法
千宇宙航6 小时前
闲庭信步使用图像验证平台加速FPGA的开发:第十课——图像gamma矫正的FPGA实现
图像处理·计算机视觉·缓存·fpga开发