点云配准方法原理(NDT、ICP)
配准是点云处理中的一个基础问题,众多学者此问题进行了广泛而深入的研究,也出现了一系列优秀成熟的算法,在三维建模、自动驾驶等领域发挥着重要的作用。
本文主要介绍粗配准NDT (Normal Distribution Transform) 与 精配准ICP (Iterative Closest Point)两种经典算法。
NDT (Normal Distribution Transform)点云配准
在去年曾经对NDT原理进行了简单总结,大家可以参考链接:点云配准NDT (P2D)算法详解。
ICP (Iterative Closest Point)点云配准
参考:
基于SVD求解 3D-3D 点对匹配
该如何学习三维点云配准的相关知识?
ICP是经典的精配准算法,其以“点”为配准基元,不断迭代求得最优的位姿,最终使得目标函数最小。
点云集合
假设存在两个点云集合,
目标点云 (target cloud ): P = { p 1 , p 2 , p 3 , . . . , p n } P=\{p_1, p_2, p_3,..., p_n\} P={p1,p2,p3,...,pn}
源点云 (source cloud): Q = { q 1 , q 2 , q 3 , . . . , q n } Q=\{q_1, q_2, q_3,..., q_n\} Q={q1,q2,q3,...,qn}
两者叠加显示:
可以看到上图中的两帧点云之间存在一个偏差,这个偏差需要一个位姿变换 ( R , t ) (R, t) (R,t)进行转换。
构造目标函数
ICP算法则希望得到一个最优的位姿变换 ( R ∗ , t ∗ ) (R^*, t^*) (R∗,t∗)使得下式目标函数最小:
f ( R ∗ , t ∗ ) = M I N ( 1 N p ∑ i = 1 N p ∣ p i t − ( R ∗ q i s + t ∗ ) ∣ 2 ) f(R^*, t^*)=MIN(\frac{1}{N_p}{\textstyle \sum_{i=1}^{N_p}\left | p_{i}^{t}-(R^*q_{i}^{s}+t^*) \right |^2 } ) f(R∗,t∗)=MIN(Np1∑i=1Np∣pit−(R∗qis+t∗)∣2)
其中 N p N_p Np为配对点云个数, p i t p_{i}^{t} pit与 q i s q_{i}^{s} qis是目标点云与源点云中的一对配对点。
寻找对应点对
起始步骤中,我们只有一系列无序的三维点,并没有对应的 p i t p_{i}^{t} pit与 q i s q_{i}^{s} qis点对。ICP中定义了“最近邻点”的定义。
- 使用一个初始位姿 ( R 0 , t 0 ) (R^0, t^0) (R0,t0)对源点云 Q Q Q进行变换,得到一个变换后的点云 Q ′ Q' Q′。
- 对变换后的点云 Q ′ Q' Q′中的点 q i ′ q_{i}^{'} qi′在目标点云中查找最近邻点 p j p_{j} pj,如果两点之间的距离小于预先设置的阈值,则认为点 q i ′ q_{i}^{'} qi′与点 p j p_{j} pj为对应的点对。
优化位姿变换 ( R , t ) (R, t) (R,t)
找到所有合理的最近邻点对之后,则每一个点对都可以构建一个函数(每个点对类似于一个观测),而待求变量 ( R , t ) (R, t) (R,t)只有6个自由度。所以我们有了 N p N_p Np个观测,6个待求变量。使用最小二乘进行优化求解。
这样我们就更新了初始位姿 ( R 0 , t 0 ) (R^0, t^0) (R0,t0)为新的 ( R 1 , t 1 ) (R^1, t^1) (R1,t1)
迭代优化
得到新的位姿变换 ( R 1 , t 1 ) (R^1, t^1) (R1,t1)之后,我们再次回到寻找对应点对步骤中,重新转换源点云 Q Q Q为 Q 2 Q^2 Q2,查找 Q 2 Q^2 Q2与 P P P新的点对。
接着,进行新的 “优化位姿变换 ( R , t ) (R, t) (R,t)” 步骤,重复得到新的优化位姿 ( R 2 , t 2 ) (R^2, t^2) (R2,t2)
直到达到迭代停止条件,如:1)达到最大迭代次数,或2)位姿变化量小于阈值,或3)最近邻点对不再变化等则终止迭代。
调用PCL库
参考链接:Interactive Iterative Closest Point
1#include <iostream>2#include <string>34#include <pcl/io/ply_io.h>5#include <pcl/point_types.h>6#include <pcl/registration/icp.h>7#include <pcl/visualization/pcl_visualizer.h>8#include <pcl/console/time.h> // TicToc910typedef pcl::PointXYZ PointT;11typedef pcl::PointCloud<PointT> PointCloudT;1213bool next_iteration = false;1415void16print4x4Matrix (const Eigen::Matrix4d & matrix)17{18 printf ("Rotation matrix :\n");19 printf (" | %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2));20 printf ("R = | %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2));21 printf (" | %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2));22 printf ("Translation vector :\n");23 printf ("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix (0, 3), matrix (1, 3), matrix (2, 3));24}2526void27keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event,28 void*)29{30 if (event.getKeySym () == "space" && event.keyDown ())31 next_iteration = true;32}3334int35main (int argc,36 char* argv[])37{38 // The point clouds we will be using39 PointCloudT::Ptr cloud_in (new PointCloudT); // Original point cloud40 PointCloudT::Ptr cloud_tr (new PointCloudT); // Transformed point cloud41 PointCloudT::Ptr cloud_icp (new PointCloudT); // ICP output point cloud4243 // Checking program arguments44 if (argc < 2)45 {46 printf ("Usage :\n");47 printf ("\t\t%s file.ply number_of_ICP_iterations\n", argv[0]);48 PCL_ERROR ("Provide one ply file.\n");49 return (-1);50 }5152 int iterations = 1; // Default number of ICP iterations53 if (argc > 2)54 {55 // If the user passed the number of iteration as an argument56 iterations = atoi (argv[2]);57 if (iterations < 1)58 {59 PCL_ERROR ("Number of initial iterations must be >= 1\n");60 return (-1);61 }62 }6364 pcl::console::TicToc time;65 time.tic ();66 if (pcl::io::loadPLYFile (argv[1], *cloud_in) < 0)67 {68 PCL_ERROR ("Error loading cloud %s.\n", argv[1]);69 return (-1);70 }71 std::cout << "\nLoaded file " << argv[1] << " (" << cloud_in->size () << " points) in " << time.toc () << " ms\n" << std::endl;7273 // Defining a rotation matrix and translation vector74 Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity ();7576 // A rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)77 double theta = M_PI / 8; // The angle of rotation in radians78 transformation_matrix (0, 0) = std::cos (theta);79 transformation_matrix (0, 1) = -sin (theta);80 transformation_matrix (1, 0) = sin (theta);81 transformation_matrix (1, 1) = std::cos (theta);8283 // A translation on Z axis (0.4 meters)84 transformation_matrix (2, 3) = 0.4;8586 // Display in terminal the transformation matrix87 std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;88 print4x4Matrix (transformation_matrix);8990 // Executing the transformation91 pcl::transformPointCloud (*cloud_in, *cloud_icp, transformation_matrix);92 *cloud_tr = *cloud_icp; // We backup cloud_icp into cloud_tr for later use9394 // The Iterative Closest Point algorithm95 time.tic ();96 pcl::IterativeClosestPoint<PointT, PointT> icp;97 icp.setMaximumIterations (iterations);98 icp.setInputSource (cloud_icp);99 icp.setInputTarget (cloud_in);
100 icp.align (*cloud_icp);
101 icp.setMaximumIterations (1); // We set this variable to 1 for the next time we will call .align () function
102 std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl;
103
104 if (icp.hasConverged ())
105 {106 std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;
107 std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
108 transformation_matrix = icp.getFinalTransformation ().cast<double>();
109 print4x4Matrix (transformation_matrix);
110 }
111 else
112 {113 PCL_ERROR ("\nICP has not converged.\n");
114 return (-1);
115 }
116
117 // Visualization
118 pcl::visualization::PCLVisualizer viewer ("ICP demo");
119 // Create two vertically separated viewports
120 int v1 (0);
121 int v2 (1);
122 viewer.createViewPort (0.0, 0.0, 0.5, 1.0, v1);
123 viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2);
124
125 // The color we will be using
126 float bckgr_gray_level = 0.0; // Black
127 float txt_gray_lvl = 1.0 - bckgr_gray_level;
128
129 // Original point cloud is white
130 pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h (cloud_in, (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl,
131 (int) 255 * txt_gray_lvl);
132 viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
133 viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v2", v2);
134
135 // Transformed point cloud is green
136 pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h (cloud_tr, 20, 180, 20);
137 viewer.addPointCloud (cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);
138
139 // ICP aligned point cloud is red
140 pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h (cloud_icp, 180, 20, 20);
141 viewer.addPointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);
142
143 // Adding text descriptions in each viewport
144 viewer.addText ("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);
145 viewer.addText ("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);
146
147 std::stringstream ss;
148 ss << iterations;
149 std::string iterations_cnt = "ICP iterations = " + ss.str ();
150 viewer.addText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);
151
152 // Set background color
153 viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
154 viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);
155
156 // Set camera position and orientation
157 viewer.setCameraPosition (-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
158 viewer.setSize (1280, 1024); // Visualiser window size
159
160 // Register keyboard callback :
161 viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);
162
163 // Display the visualiser
164 while (!viewer.wasStopped ())
165 {166 viewer.spinOnce ();
167
168 // The user pressed "space" :
169 if (next_iteration)
170 {171 // The Iterative Closest Point algorithm
172 time.tic ();
173 icp.align (*cloud_icp);
174 std::cout << "Applied 1 ICP iteration in " << time.toc () << " ms" << std::endl;
175
176 if (icp.hasConverged ())
177 {178 printf ("\033[11A"); // Go up 11 lines in terminal output.
179 printf ("\nICP has converged, score is %+.0e\n", icp.getFitnessScore ());
180 std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl;
181 transformation_matrix *= icp.getFinalTransformation ().cast<double>(); // WARNING /!\ This is not accurate! For "educational" purpose only!
182 print4x4Matrix (transformation_matrix); // Print the transformation between original pose and current pose
183
184 ss.str ("");
185 ss << iterations;
186 std::string iterations_cnt = "ICP iterations = " + ss.str ();
187 viewer.updateText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");
188 viewer.updatePointCloud (cloud_icp, cloud_icp_color_h, "cloud_icp_v2");
189 }
190 else
191 {192 PCL_ERROR ("\nICP has not converged.\n");
193 return (-1);
194 }
195 }
196 next_iteration = false;
197 }
198 return (0);
199}
配准结果
图中红色为目标帧点云,蓝色为转换后的源点云。
点云配准方法原理(NDT、ICP)相关推荐
- 点云配准的传统算法ICP与NDT概述
公众号致力于分享点云处理,SLAM,三维视觉,高精地图相关的文章与技术,欢迎各位加入我们,一起交流一起进步,有兴趣的可联系微信:920177957.本文来自点云PCL博主的分享,未经作者允许请勿转载, ...
- GICP:基于体素泛化ICP方式的准确快速点云配准方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 标题: Voxelized GICP for Fast and Accurate 3D Point C ...
- 针对地图可压缩性的点云配准方法评估(IROS 2021)
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 作者丨Penny@知乎 来源丨https://zhuanlan.zhihu.com/p/2009241 ...
- 基于“分布 —— 多分布” 的点云配准方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 作者:iceytan | 来源:知乎 https://zhuanlan.zhihu.com/p/135 ...
- KSS-ICP: 基于形状分析技术的点云配准方法
目录 1. 概述 2. 算法实现 3. 实验结果 总结 Reference 三维点云配准是三维视觉领域一个经典问题,涉及三维重建,定位,SLAM等具体应用问题.传统的配准可以被分为两条技术路线,即基于 ...
- 视频+课件| PointDSC:基于特征匹配的点云配准方法(CVPR2021)
写在前面 感谢「3D视觉从入门到精通」知识星球嘉宾白旭阳博士为我们带来的主题为PointDSC:基于特征匹配的点云配准方法(CVPR2021)视频讲解,星球成员可免费观看学习.备注:白旭阳,香港科技大 ...
- 点云配准方法:ICP与GICP
ICP已经成为点云配准的主流算法,因此,这两天测试了PCL库中的两个ICP函数: pcl::GeneralizedIterativeClosestPoint< PointSource, Poin ...
- 两种常见的点云配准方法ICPNDT
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 作者:于凡 https://zhuanlan.zhihu.com/p/96908474 本文转载自知乎 ...
- 点云配准--4PCS原理与应用
文章目录 1预备知识 1.0 wide base 1.1 LCP(largest common pointset) 1.2 RANSAC配准过程 1.3 Randomized Alignment 2 ...
最新文章
- vb打开ftp服务器文件路径,VB上传指定文件到FTP指定目录。。
- 第四范式变“硬”,联手浪潮推出AI一体机,挑战BAT
- 寒冬之下,做好这六点
- mysql一个死锁分析
- SQL Server -- LIKE模糊查询
- 网游运营基础知识与专业术语
- react实现svg实线、虚线、方形进度条
- Python进阶:如何将字符串常量转化为变量?
- windows2008 服务器时间格式改不过来_我用VNPY 1天时间搭建自主量化交易(程序化交易)平台...
- 查询服务器硬件配置的命令
- 简单神经网络结构一键可视化
- scrapy框架使用教程
- 多目标优化_学习笔记(三)MOEA/D
- JS短文 | 3分钟了解下 JS Sets 集合
- 安庆集团-冲刺日志(第六天)
- 天玥系列微型计算机,【简讯】AMD正式发布RX 6000系列显卡;OPPO K7x宣布…
- Firefox火狐浏览器主页被360篡改了
- 胶装一般多少钱一本?网上打印资料胶装便宜的地方
- java计算机毕业设计基于安卓Android的人在旅途旅行出行APP(源码+系统+mysql数据库+Lw文档)
- Android模拟器的使用
热门文章
- 学PS基础:Photoshop 技能167个
- P3802 小魔女帕琪 期望
- 关于自动化测试的定位及一些实践思考
- 汉字应用水平测试软件,汉字应用水平测试(HZC)试点将在11个省市进行
- css 图片变大缩小,css3实现图片的变大变小
- 关于学习的时间定律-21小时、1000小时、5000小时、10000小时
- 树状数组详解(附图解,模板及经典例题分析)
- 上面两点下面一个三角形_【知识点】三角形全等的判定+性质+辅助线技巧都在这里了!...
- 谭的c语言,c语言 谭
- XML学习-方立勋视频学习