ICP算法进行点云匹配
【原文：http://www.cnblogs.com/yhlx125/p/5234156.html】
上一篇：http://www.cnblogs.com/yhlx125/p/4924283.html截图了一些ICP算法进行点云匹配的类图。
但是将对应点剔除这块和ICP算法的关系还是没有理解。RANSAC算法可以实现点云剔除，但是ICP算法通过稳健性的算法实现匹配，似乎不进行对应点剔除。是不是把全局的点云匹配方法和局部点云匹配方法搞混了？
ICP算法可以通过三种方式处理噪声、部分重叠的问题：剔除、权重和稳健估计方法。
下面分析一下PCL中关于ICP算法的实现。
首先是IterativeClosestPoint模板类继承自Registration模板类。
查看icp.h中的定义：
template <typename PointSource, typename PointTarget, typename Scalar = float>class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar>
查看registration.h中的定义：
template <typename PointSource, typename PointTarget, typename Scalar = float>class Registration : public PCLBase<PointSource>
同样的IterativeClosestPointNonLinear 继承自Registration，查看icp_nl.h
template <typename PointSource, typename PointTarget, typename Scalar = float> class IterativeClosestPointNonLinear : publicIterativeClosestPoint<PointSource, PointTarget, Scalar>
区别在于ICP算法的求解方式不同，非线性求解采用的是LM算法：http://www.cnblogs.com/yhlx125/p/4955337.html
下面重点说明IterativeClosestPoint模板类。匹配的关键方法Align在Registration中实现。
1 template <typename PointSource, typename PointTarget, typename Scalar> inline void
2 pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output, const Matrix4& guess)
3 {
4 if (!initCompute ())
5 return;
6
7 // Resize the output dataset
8 if (output.points.size () != indices_->size ())
9 output.points.resize (indices_->size ());
10 // Copy the header
11 output.header = input_->header;
12 // Check if the output will be computed for all points or only a subset
13 if (indices_->size () != input_->points.size ())
14 {
15 output.width = static_cast<uint32_t> (indices_->size ());
16 output.height = 1;
17 }
18 else
19 {
20 output.width = static_cast<uint32_t> (input_->width);
21 output.height = input_->height;
22 }
23 output.is_dense = input_->is_dense;
24
25 // Copy the point data to output
26 for (size_t i = 0; i < indices_->size (); ++i)
27 output.points[i] = input_->points[(*indices_)[i]];
28
29 // Set the internal point representation of choice unless otherwise noted
30 if (point_representation_ && !force_no_recompute_)
31 tree_->setPointRepresentation (point_representation_);
32
33 // Perform the actual transformation computation
34 converged_ = false;
35 final_transformation_ = transformation_ = previous_transformation_ = Matrix4::Identity ();
36
37 // Right before we estimate the transformation, we set all the point.data[3] values to 1 to aid the rigid
38 // transformation
39 for (size_t i = 0; i < indices_->size (); ++i)
40 output.points[i].data[3] = 1.0;
41
42 computeTransformation (output, guess);
43
44 deinitCompute ();
45 }
重点关注computeTransformation虚方法。显然IterativeClosestPoint重载了这个方法。
virtual void computeTransformation (PointCloudSource &output, const Matrix4& guess) = 0;
代码如下：
1 template <typename PointSource, typename PointTarget, typename Scalar> void
2 pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (
3 PointCloudSource &output, const Matrix4 &guess)
4 {
5 // Point cloud containing the correspondences of each point in <input, indices>
6 PointCloudSourcePtr input_transformed (new PointCloudSource);
7
8 nr_iterations_ = 0;
9 converged_ = false;
10
11 // Initialise final transformation to the guessed one
12 final_transformation_ = guess;
13
14 // If the guessed transformation is non identity
15 if (guess != Matrix4::Identity ())
16 {
17 input_transformed->resize (input_->size ());
18 // Apply guessed transformation prior to search for neighbours
19 transformCloud (*input_, *input_transformed, guess);
20 }
21 else
22 *input_transformed = *input_;
23
24 transformation_ = Matrix4::Identity ();
25
26 // Make blobs if necessary
27 determineRequiredBlobData ();
28 PCLPointCloud2::Ptr target_blob (new PCLPointCloud2);
29 if (need_target_blob_)
30 pcl::toPCLPointCloud2 (*target_, *target_blob);
31
32 // Pass in the default target for the Correspondence Estimation/Rejection code
33 correspondence_estimation_->setInputTarget (target_);
34 if (correspondence_estimation_->requiresTargetNormals ())
35 correspondence_estimation_->setTargetNormals (target_blob);
36 // Correspondence Rejectors need a binary blob
37 for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
38 {
39 registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
40 if (rej->requiresTargetPoints ())
41 rej->setTargetPoints (target_blob);
42 if (rej->requiresTargetNormals () && target_has_normals_)
43 rej->setTargetNormals (target_blob);
44 }
45
46 convergence_criteria_->setMaximumIterations (max_iterations_);
47 convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);
48 convergence_criteria_->setTranslationThreshold (transformation_epsilon_);
49 convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
50
51 // Repeat until convergence
52 do
53 {
54 // Get blob data if needed
55 PCLPointCloud2::Ptr input_transformed_blob;
56 if (need_source_blob_)
57 {
58 input_transformed_blob.reset (new PCLPointCloud2);
59 toPCLPointCloud2 (*input_transformed, *input_transformed_blob);
60 }
61 // Save the previously estimated transformation
62 previous_transformation_ = transformation_;
63
64 // Set the source each iteration, to ensure the dirty flag is updated
65 correspondence_estimation_->setInputSource (input_transformed);
66 if (correspondence_estimation_->requiresSourceNormals ())
67 correspondence_estimation_->setSourceNormals (input_transformed_blob);
68 // Estimate correspondences
69 if (use_reciprocal_correspondence_)
70 correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);
71 else
72 correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);
73
74 //if (correspondence_rejectors_.empty ())
75 CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_));
76 for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
77 {
78 registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
79 PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ());
80 if (rej->requiresSourcePoints ())
81 rej->setSourcePoints (input_transformed_blob);
82 if (rej->requiresSourceNormals () && source_has_normals_)
83 rej->setSourceNormals (input_transformed_blob);
84 rej->setInputCorrespondences (temp_correspondences);
85 rej->getCorrespondences (*correspondences_);
86 // Modify input for the next iteration
87 if (i < correspondence_rejectors_.size () - 1)
88 *temp_correspondences = *correspondences_;
89 }
90
91 size_t cnt = correspondences_->size ();
92 // Check whether we have enough correspondences
93 if (static_cast<int> (cnt) < min_number_correspondences_)
94 {
95 PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
96 convergence_criteria_->setConvergenceState(pcl::registration::DefaultConvergenceCriteria<Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
97 converged_ = false;
98 break;
99 }
100
101 // Estimate the transform
102 transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);
103
104 // Tranform the data
105 transformCloud (*input_transformed, *input_transformed, transformation_);
106
107 // Obtain the final transformation
108 final_transformation_ = transformation_ * final_transformation_;
109
110 ++nr_iterations_;
111
112 // Update the vizualization of icp convergence
113 //if (update_visualizer_ != 0)
114 // update_visualizer_(output, source_indices_good, *target_, target_indices_good );
115
116 converged_ = static_cast<bool> ((*convergence_criteria_));
117 }
118 while (!converged_);
119
120 // Transform the input cloud using the final transformation
121 PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n",
122 final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3),
123 final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3),
124 final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3),
125 final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3));
126
127 // Copy all the values
128 output = *input_;
129 // Transform the XYZ + normals
130 transformCloud (*input_, output, final_transformation_);
131 }
该方法的主体是一个do-while循环。这里要说三个内容：correspondence_estimation_ 、correspondence_rejectors_ 和 convergence_criteria_ 。
三个变量的作用分别是查找最近点，剔除错误的对应点，收敛准则。因为是do-while循环，因此收敛准则的作用是跳出循环。
transformation_estimation_是求解ICP的具体算法：
1 transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);
查看类图可以知道包括SVD奇异值分解算法，查看transformation_estimation_svd.hpp中的TransformationEstimationSVD类的estimateRigidTransformation 方法：
template <typename PointSource, typename PointTarget, typename Scalar> inline void pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it,ConstCloudIterator<PointTarget>& target_it,Matrix4 &transformation_matrix) const
其中通过调用下面的代码实现了SVD求解，具体方法内部实现时通过Eigen3实现的。需要具体查看，可以借鉴。
transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false);
ICP算法进行点云匹配相关推荐
- 在医学图像分析中使用ICP算法进行点云配准
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 本文转载自「计算机视觉工坊」,该公众号重点在于介绍深度学习.智能驾驶等领域,一个小众的公众号. 论文标 ...
- 点云配准2：icp算法在PCL1.10.0上的实现+源码解析
目录 本文最后实现的配准实例 点云配准系列 准备 程序结构 主程序 1.为什么要降采样 2.体素降采样原理 3.点云更新 icp 配准前的参数设置 icp配准算法内部 对应点对确定(determine ...
- ICP算法MATLAB仿真
2022.4.15再次附上数据satellite.txt新链接: 链接:https://pan.baidu.com/s/1mEN-FQbTlHOWfxCHyu0Dxg 提取码:02i4 (永久有效) ...
- ICP算法概述以及使用SVD进行算法推导
本文转载于B站UP主 摆烂世家的视频ICP算法概述及使用SVD推导(组会录像) 作者:苏浩田 (注:如有侵权,私信我下立马删咯) 1 ICP 算法 三维点云拼接技术在不同场合亦被称为重定位.配准或 ...
- icp点云匹配迭代最近邻算法
一.含义: 1.icp算法能够使两个不同坐标系下的点集匹配到一个坐标系中,这个过程就是配准,配准的操作就是找到从坐标系1变换到坐标系2的刚性变换. 2.icp的本质就是配准,但有不同的配准方案,icp ...
- 点云匹配和ICP算法概述
[原文:http://www.cnblogs.com/yhlx125/p/4955337.html] Iterative Closest Point (ICP) [1][2][3] is an alg ...
- 多帧点云拼接的全局ICP算法【附Matlab代码链接】
用RGBD相机采集一组多视角深度点云,假设多帧点云之间有共视邻接关系,通常会先进行Pair-wise的帧间点云匹配,具体方法见另外一个帖子: 两帧点云刚性配准的ICP算法 连续的帧间ICP可以把点云变 ...
- 干货 | 三维点云配准：ICP 算法原理及推导
编者荐语 点云配准可以分为粗配准(Coarse Registration)和精配准(Fine Registration)两步.粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准,目的主 ...
- 两帧点云刚性配准的ICP算法
点云配准的一般思路是根据两个点云的匹配点,估计刚性变换矩阵[R t]. 空间刚性变换的3×4矩阵[R t]虽然包含12个数,但只有旋转和平移6个自由度(参数).在SLAM中相机的位姿也用[R t]矩阵 ...
最新文章
- 适配器(GenericServlet)改造Servlet
- JAVA_SE之内部类
- 中国科技巨擘（BAT）竞逐人工智能
- linux 嵌入式 交叉 环境搭建 实验原理,实验三 嵌入式Linux开发环境的搭建
- 入门科普：什么时候要用Python？用哪个版本？什么时候不能用？
- python画椭圆形_手残党福音：用Python画出机器人Dev
- 【Flink】Flink消费Kafka数据时指定offset的五种方式
- SpringMVC+MyBatis整合——事务管理
- 推算周期软件有哪些,推算周期软件怎么用
- 使用 CP2102通过串口下载程序到STM32F103中 （MCUISP）
- 理清contactsprovider
- jmeter使用.jmx脚本
- 读书笔记 - 《六神磊磊读唐诗》
- 软件测试——透过表象看本质
- 极化SAR图像特征提取与分类方法研究
- 冰箱味道很臭？那你真的该学学这些除臭妙招
- Android踩坑记录：This view is not constrained vertically
- 如何在国内快速访问Github
- 旧手机怎么当文件服务器,用旧手机做云存储服务器
- 什么是远程桌面连接？win11系统如何启用远程桌面连接？