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
相关推荐
白榆maple8 分钟前
(蓝桥杯C/C++)——基础算法(下)
算法
喵~来学编程啦9 分钟前
【论文精读】LPT: Long-tailed prompt tuning for image classification
人工智能·深度学习·机器学习·计算机视觉·论文笔记
JSU_曾是此间年少12 分钟前
数据结构——线性表与链表
数据结构·c++·算法
starsongda20 分钟前
科技成果跃然“屏”上,虚拟展厅引领科技展示新风尚
科技·3d·虚拟现实
深圳市青牛科技实业有限公司22 分钟前
【青牛科技】应用方案|D2587A高压大电流DC-DC
人工智能·科技·单片机·嵌入式硬件·机器人·安防监控
水豚AI课代表43 分钟前
分析报告、调研报告、工作方案等的提示词
大数据·人工智能·学习·chatgpt·aigc
几两春秋梦_43 分钟前
符号回归概念
人工智能·数据挖掘·回归
此生只爱蛋1 小时前
【手撕排序2】快速排序
c语言·c++·算法·排序算法
何曾参静谧1 小时前
「C/C++」C/C++ 指针篇 之 指针运算
c语言·开发语言·c++
用户691581141652 小时前
Ascend Extension for PyTorch的源码解析
人工智能