当前位置：首页 > news >正文

NDT和ICP构建点云地图 |【点云建图、Ubuntu、ROS】

news 来源：原创 2025/9/11 3:51:18

###

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

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

PCL中点云配准（非常详细，建议收藏）_pcl点云配准-CSDN博客

代码可自取：Xz/NDT构建点云地图 - Gitee.com

###

一、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参数。

通过匹配结果可知，其匹配的效果并不好。随后我也修改了NDT参数设置，但是最终的结果依旧不理想。然后我又利用ICP算法进行点云地图构建，如下图所示，为点云逐渐迭代的过程。其创建工作空间及操作过程与前面ICP操作相同。这里不在重复该过程，结果如下：

随着不断匹配新的点云，其点云范围逐渐增大。

程序执行完之后，会在该目录下生成配准后的pcd文件，可用pcl_viewer命令查看pcd文件。取出其中一个pcd文件查看，左边为配准前的点云，右边为匹配了一个pcd文件后的点云，还是存在一些区别。

pcl_viewer <pcd文件名>.pcd

参考博客：

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

PCL中点云配准（非常详细，建议收藏）_pcl点云配准-CSDN博客

NDT和ICP构建点云地图 |【点云建图、Ubuntu、ROS】

### 本博客记录学习NDT，ICP构建点云地图的实验过程，参考的以下两篇博客： 无人驾驶汽车系统入门（十三）——正态分布变换（NDT）配准与无人车定位_settransformationepsilon-CSDN博客 PCL中点云配…...

编程日记 2025/9/11 3:51:18

第 1 篇✅ 用 AI 编程之前，你得先搞清楚你和 AI 是啥关系

程序员不是被替代的，是要学会主导 AI 的人 🧠 那些把 AI 当兄弟的程序员，后来都踩了坑最近的一次线下开发者聚会，我们聊到“AI 编程”，现场笑声不断，也点醒了不少人。有个朋友说： “我让 AI 写一个 Web 服务，它写得飞快，我一激动就上线了，结果上线后一堆坑，日志…...

编程日记 2025/9/6 6:55:35

Android Jetpack Compose 高级开发核心技术

Android Compose 高级技术总结 1. 性能优化 1.1 状态管理优化状态提升原则：将状态提升到共享的最近共同父组件derivedStateOf：当需要基于多个状态计算派生状态时使用 val scrollState rememberScrollState() val showButton by remember {derivedS…...

编程日记 2025/9/10 2:44:56

Go小技巧易错点100例（二十五）

本期分享： 1. 使用atomic包实现无锁并发控制 2. Gin框架的中间件机制 3. 搞懂nil切片和空切片使用atomic包实现无锁并发控制 sync/atomic包提供了原子操作，用于在多goroutine环境下安全地操作共享变量，避免使用锁带来的性能开销。代码…...

编程日记 2025/9/6 17:11:06

如何用海伦公式快速判断点在直线的哪一侧

一、海伦公式的定义与推导 1. 海伦公式的定义海伦公式（Heron’s Formula）是用于计算三角形面积的一种方法，适用于已知三角形三边长度的情况。公式如下： S s ( s − a ) ( s − b ) ( s − c ) S \sqrt{s(s - a)(s - b)(s - c…...

编程日记 2025/9/4 19:34:57

【异常处理】Clion IDE中cmake时头文件找不到头文件飘红

如图所示是我的clion项目目录我自定义的data_structure.h和func_declaration.h在unit_test.c中无法检索到 cmakelists.txt配置文件如下所示： cmake_minimum_required(VERSION 3.30) project(noc C) #设置头文件的目录 include_directories(${CMAKE_SOURCE_DIR}/…...

编程日记 2025/9/11 3:51:13

自动驾驶技术关键技术梳理

一、硬件 1、传感器系统设计主要注意以下几个问题： 1.时间同步一般包括多传感器之间时钟同源、帧同步触发的问题。首先要解决时钟同源问题，然后为了帧同步触发，可以让所有传感器整秒触发。常用GPS（最多分2路）给激光雷…...

编程日记 2025/9/5 17:05:03

MySQL索引介绍

索引的定义扇区：磁盘存储的最小单位，扇区一般大小为512Byte。磁盘块：文件系统与磁盘交互的的最小单位（计算机系统读写磁盘的最小单位），一个磁盘块由连续几个（2^n）扇区组成&#xf…...

编程日记 2025/9/10 5:17:59

2025认证杯一阶段各题需要使用的模型或算法（冲刺阶段）

A题（小行星轨迹预测） 问题一：三角测量法、最小二乘法、空间几何算法、最优化方法问题二：Gauss/Laplace轨道确定方法、差分校正法、数值积分算法（如Runge-Kutta法）、卡尔曼滤波器 B题（谣言在…...

编程日记 2025/9/5 10:17:50

每天学一个 Linux 命令（13）：touch

Linux 文件管理命令：touch touch 是 Linux 中一个简单但高频使用的命令，主要用于创建空文件或修改文件的时间戳（访问时间、修改时间）。它是文件管理和脚本操作的实用工具。 1. 命令作用创建空文件：快速生成一个或多个空白文件。更新时间戳：修改文件的访问时间（Access …...

编程日记 2025/9/8 0:21:06

Flutter常用组件实践

Flutter常用组件实践 1、MaterialApp 和 Center（组件居中)2、Scaffold3、Container（容器）4、BoxDecoration（装饰器）5、Column（纵向布局）及Icon（图标）6、Column/Row（横向/横向布局）+CloseButton/BackButton/IconButton（简单按钮）7、Expanded和Flexible8、Stack和Po…...

编程日记 2025/9/7 19:24:57

Python 实现最小插件框架

文章目录 Python 实现最小插件框架1. 基础实现项目结构plugin_base.py - 插件基类plugins/hello.py - 示例插件1plugins/goodbye.py - 示例插件2main.py - 主程序 2. 更高级的特性扩展2.1 插件配置支持2.2 插件依赖管理2.3 插件热加载 3. 使用 setuptools 的入口点发现插件3.1 …...

编程日记 2025/9/4 9:11:26

AUTOSAR_SWS_MemoryDriver图解

AUTOSAR 存储驱动程序（Memory Driver）详解 AUTOSAR存储驱动规范 - 技术解析与架构详解目录 1. 概述2. Memory Driver架构设计 2.1 整体架构 3. Memory Driver核心组件4. 作业管理5. Memory Driver错误处理6. 时序流程7. 配置与设置8. 总结 1. 概述 A…...

编程日记 2025/9/11 3:51:16

AI结合VBA提升EXCEL办公效率尝试

文章目录前言一、开始VBA编程二、主要代码三、添加到所有EXCEL四、运行效果五、AI扩展前言 EXCEL右击菜单添加一个选项，点击执行自己逻辑的功能。然后让DeepSeek帮我把我的想法生成VBA代码一、开始VBA编程我的excel主菜单没有’开发工具‘ 选项，…...

编程日记 2025/9/8 8:33:19

Python中NumPy的索引和切片

在数据科学和科学计算领域，NumPy是一个功能强大且广泛使用的Python库。它提供了高效的多维数组对象以及丰富的数组操作函数，其中索引和切片是NumPy的核心功能之一。通过灵活运用索引和切片操作，我们可以轻松访问和操作数组中的元素&#xff0…...

编程日记 2025/9/10 0:56:50

普通通话CSFB方式（2g/3g）

一、CSFB的触发条件当模块（或手机）驻留在 4G LTE网络时，若发生以下事件，会触发CSFB流程： 主叫场景：用户主动拨打电话。被叫场景：接收到来电（MT Call）。紧急呼叫&…...

编程日记 2025/9/11 1:13:42

daily routines 日常生活

总结 🛏 起床相关（Waking Up）动作常用表达示例句子醒来wake upI usually wake up around 6:30.起床（离床）get up / get out of bedI got out of bed at 6:45.赖床stay in bed / lay thereI stayed in bed for another 10 minutes.关闭闹钟turn off the alarm / hit snoo…...

编程日记 2025/9/7 18:18:04

系分论文《论面向服务开发方法在设备租赁行业的应用》

系统分析师论文系列【摘要】 2022年5月，我司承接某工程机械租赁企业"智能租赁运营管理平台"建设项目，我作为系统分析师主导系统架构设计。该项目需整合8大类2000余台设备资产，覆盖全国15个区域运营中心与300家代理商，实…...

编程日记 2025/9/9 4:08:55

深度解析python生成器和关键字yield

一、生成器概述生成器（Generator）是Python中用于创建迭代器的工具，通过yield关键字实现。与普通函数不同，生成器函数返回的是迭代器对象，具有以下核心特性： 内存效率：只在需要时生成值&#x…...

编程日记 2025/9/9 22:59:52

蓝桥杯大模板

init.c void System_Init() {P0 0x00; //关闭蜂鸣器和继电器P2 P2 & 0x1f | 0xa0;P2 & 0x1f;P0 0x00; //关闭LEDP2 P2 & 0x1f | 0x80;P2 & 0x1f; } led.c #include <LED.H>idata unsigned char temp_1 0x00; idata unsigned char temp_old…...

编程日记 2025/9/4 3:48:07

Python装饰器的基本使用详解

各类资料学习下载合集 https://pan.quark.cn/s/8c91ccb5a474 装饰器是Python中的一个强大且灵活的特性，它允许我们在不修改函数代码的情况下为其添加额外功能。装饰器广泛应用于日志记录、性能测试、权限验证等场景。本文将详细介绍装饰器的基本使用&…...

编程日记 2025/9/3 16:34:11

5Why分析法

1. 基本概念 5Why分析法是一种通过连续追问"为什么"来探究问题根本原因的思考工具，由丰田生产方式创始人丰田喜一郎提出。其核心思想是：通过至少5次连续的"为什么"追问，穿透表面现象，直达问题本质。 2. 实施…...

编程日记 2025/9/8 13:41:36

AI Agent入门指南

图片来源网络 ‌一、开箱暴击：你以为的"智障音箱"，其实是赛博世界的007‌ ‌1.1 从人工智障到智能叛逃：Agent进化史堪比《甄嬛传》‌ ‌青铜时代（2006-2015）‌ “小娜同学，关灯” “抱歉&…...

编程日记 2025/9/10 4:21:06

华为机试—最大最小路

题目对于给定的无向无根树，第 i 个节点上有一个权值 wi 。我们定义一条简单路径是好的，当且仅当：路径上的点的点权最小值小于等于 a ，路径上的点的点权最大值大于等于 b 。保证给定的 a<b，你需要计算有多少条简…...

编程日记 2025/9/11 0:38:32

java之多线程

目录创建多线程的三种创建方式常用的成员方法守护线程多线程的声明周期编辑同步代码块编辑同步方法死锁等待唤醒机制（线程协调） 线程池创建多线程的三种创建方式继承 Thread 类通过继承 Thread 类并重写 run() 方法创建线程。 …...

编程日记 2025/9/5 11:46:00

php伪协议

PHP 伪协议（PHP Stream Wrapper） PHP 的伪协议（Protocol Wrapper）是一种机制，允许开发者通过统一的文件访问函数（如 file_get_contents、fopen、include 等）访问不同类型的数据源，包…...

编程日记 2025/9/4 10:41:22

六、测试分类

设计测试用例万能公式：功能测试性能测试界面测试兼容性测试安全性测试易用性测试弱网测试：fiddler上行速率和下行速率安装卸载测试在工作中： 1.基于需求文档来设计测试用例（粗粒度） 输入字段长度为6~15位功…...

编程日记 2025/9/4 10:43:23

【AM2634】启动和调试

目录【AM2634】启动和调试1. 上电流程1.1 BootFlow and Bootloader1.2 Rom Code1.2.1 功能介绍1.2.2 模式选择1.2.2.1 QSPI Boot1.2.2.2 UART Boot1.2.2.3 Dev Boot 1.3 SBL1.3.1 文件构成1.3.2 文件构建1.3.3 appimage解析和core启动流程 1.4 Appimage1.4.1 RPRC文件构成1.4.…...

编程日记 2025/9/10 22:43:32

鲁大师绿色版,纯净无广告

鲁大师是我们常用的硬件跑分软件,可以非常准确的识别电脑硬件,对电脑性能进行评估但他的流氓行为:广告弹窗,捆绑下载其他软件,疯狂的吃硬件性能,无法卸载等因素，又使我们大家既享用又不敢用我为大家整理了一款纯净的绿色版鲁大师主要实现了以下功能: 01屏蔽了…...

编程日记 2025/9/8 6:19:28

Python数组（array）学习之旅：数据结构的奇妙冒险

Python数组学习之旅：数据结构的奇妙冒险第一天：初识数组的惊喜阳光透过窗帘缝隙洒进李明的房间，照亮了他桌上摊开的笔记本和笔记本电脑。作为一名刚刚转行的金融分析师，李明已经坚持学习Python编程一个月了。他的眼睛因为昨晚熬夜编程而微微发红，但脸上却挂着期待的微…...

编程日记 2025/9/4 10:51:35

spring cloud微服务API网关详解及各种解决方案详解

微服务API网关详解 1. 核心概念定义：API网关作为微服务的统一入口，负责请求路由、认证、限流、监控等功能，简化客户端与后端服务的交互。核心功能： 路由与转发：将请求分发到对应服务。协议转换：HTTP/HTTP…...

编程日记 2025/9/9 14:00:05

工程师 - 场效应管分类

What Are the Different Types of FETs? Pulse Octopart Staff Jul 31, 2021 Field effect transistors (FETs) are today’s workhorses for digital logic, but they enjoy plenty of applications outside of digital integrated circuits, everything from motor driver…...

编程日记 2025/9/9 4:08:59