景洪网站建设,汽车零部件公司网站建设方案,建设机械网站方案设计,昆明网站建设猫咪科技文章目录前言一、ICP算法基础1.1 提取待匹配点对1.2 计算旋转平移矩阵1.3 计算变换后的点和目标点之间的偏差二、ICP算法变种2.1 PLICP2.2 PointToPlane ICP2.3 NICP2.4 LM_ICP三、程序示例1. 传统方法2. PointToPlane ICP总结前言
ICP#xff08;Iterative Closest PointIterative Closest Point最近邻点迭代是应用最广泛的三维点云配准算法之一网上讲ICP算法原理的非常多这里列举几个个人觉得讲的比较好的。 三维点云配准 – ICP 算法原理及推导 ICP迭代最近点算法 PCL学习笔记二Registration (ICP算法)
原始的ICP算法的问题在于点对之间只分析距离之间的关系从而引起迭代次数高最终导致的计算时间过长所以很多学者提出了各种各样的改进算法如PLICPPointToPlane ICPNICPLM_ICP算法等。 本文对各种ICP算法的原理以及其简单的应用进行分析。 一、ICP算法基础
ICP算法的基本逻辑是先通过对应关系估计、关键点检测等方法找到源点云和目标点云的待匹配点对然后计算旋转和平移矩阵对source点云进行旋转平移到target点云上根据设置的阈值进行判断如果不满足阈值要求就重复以上过程继续计算最终达到最大迭代次数或者满足设定的均方差阈值才能停止迭代。 可以用一个公式表示
1.1 提取待匹配点对
首先按需要进行blob。 然后进行Correspondences estimation对应关系估计得到共同部分的点云。
/** \brief A CorrespondenceEstimation object, used to estimate correspondences between the source and the target cloud. */CorrespondenceEstimationPtr correspondence_estimation_;/** \brief The minimum number of correspondences that the algorithm needs before attempting to estimate the * transformation. The default value is 3.*/int min_number_correspondences_;correspondence_estimation_-setInputTarget (target_);if (correspondence_estimation_-requiresTargetNormals ())correspondence_estimation_-setTargetNormals (target_blob);具体源码自行查看下面列出Correspondences estimation的计算代码,里面包含了两种计算方法分别为determineCorrespondences和determineReciprocalCorrespondences 。 determineCorrespondences会计算所有点的对应关系。 determineReciprocalCorrespondences计算两个点云共同部分的对应关系。
最后进行一个CorrespondenceRejector去除错误的估计。 for (std::size_t i 0; i correspondence_rejectors_.size (); i){registration::CorrespondenceRejector::Ptr rej correspondence_rejectors_[i];if (rej-requiresTargetPoints ())rej-setTargetPoints (target_blob);if (rej-requiresTargetNormals () target_has_normals_)rej-setTargetNormals (target_blob);}1.2 计算旋转平移矩阵
我们默认变换为刚性变换通过空间中两点间的变换关系计算R和T矩阵。假定第一次计算的矩阵为R0R_0R0和T0T_0T0。PCL中ICP的初始矩阵为
Eigen::Vector4f pt (0.0f, 0.0f, 0.0f, 1.0f), pt_t;
Eigen::Matrix4f tr transform.template castfloat ()也就是 [1010101]\begin{bmatrix} 10\\ 10\\10\\1\end{bmatrix}⎣⎢⎢⎡1110001⎦⎥⎥⎤ 公式如下 pip_ipiR0R_0R0*qiq_iqiT0T_0T0 其中 T [txtytz]\begin{bmatrix} txtytz\end{bmatrix}[txtytz] 具体的计算采用奇异值分解的方法具体计算过程前言部分推荐的文章有写。
1.3 计算变换后的点和目标点之间的偏差
对点集p进行变换计算变换后的点集p1p_1p1和q的距离值。 Distance∑i1n∣∣p1\displaystyle\sum_{i1}^{n}||p_1i1∑n∣∣p1-q ||2^22 Distance和设定的阈值进行对比如果大于则跳到第一步重新开始循环迭代如果达到最大迭代次数还没有满足阈值要求也会停止迭代保留最近一次的迭代结果。 PCL中还有一个收敛条件设置setTransformationEpsilon意思是上一个变换矩阵和当前变换矩阵的差异值如果差异值小于阈值也认为达到收敛。
PCL迭代部分的代码如下 // Estimate the transformtransformation_estimation_-estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);// Transform the datatransformCloud (*input_transformed, *input_transformed, transformation_);// Obtain the final transformationfinal_transformation_ transformation_ * final_transformation_;nr_iterations_;二、ICP算法变种
因为计算点对间的最小距离的方法过于耗时和低效所以出现了很多加速方法例如先提取点云特征然后进行特征间的匹配可以极大减少匹配时间或者对计算源点云中点到目标点云对应点线或者面的最小距离可以降低。
2.1 PLICP
PLICP计算当前帧中的点到参考帧中最近邻两点连线的最小距离值在slam中应用较多可以或者更小的迭代次数和更高的精度。 原理可以参考论文 A. Censi, “An ICP variant using a point-to-line metric,” 2008 IEEE International Conference on Robotics and Automation, Pasadena, CA, 2008, pp. 19-25, doi: 10.1109/ROBOT.2008.4543181
2.2 PointToPlane ICP
计算Source点云中点到目标点云对应点形成的面的最小距离值。 这里ni为qi的法线,minE为最小欧式距离。
2.3 NICP
NICP与传统ICP的不同之处在于NICP会多考虑曲率和法线的方向一致问题如果对应点的曲率和法线方向超过设定阈值不会建立对应点的匹配。所以在计算的时候需要计算点云的法线信息然后进行匹配时需要额外对对应点对的法线和曲率限定阈值。
2.4 LM_ICP
LM_ICP在计算最小距离的时候采用LM模型来进行算法原理可以查看论文 结合Kinect的双目视觉场景三维重建。
三、程序示例
1. 传统方法
传统方法计算全部点云的对应关系导致计算时间非常长。
icp.setInputSource(source);
icp.setInputTarget(target);
icp.setTransformationEpsilon(1e-8);
icp.setMaxCorrespondenceDistance(1); //添加一个最大距离的阈值超过最大距离的点不作为对应点。
icp.setEuclideanFitnessEpsilon(0.00005);
icp.setMaximumIterations(150);
icp.setUseReciprocalCorrespondences(true);迭代1次 迭代134次
2. PointToPlane ICP
PointToPlane ICP的核心类是IterativeClosestPointWithNormals默认情况下此实现使用传统的点对面目标并使用目标点云的法线计算点对面距离。 另外在计算法线的时候采用OpenMP来进行多线程加速。
pcl::IterativeClosestPointWithNormalspcl::PointNormal, pcl::PointNormalptoplane_icp;ptoplane_icp.setInputSource(source_with_normals);
ptoplane_icp.setInputTarget(target_with_normals);
ptoplane_icp.setTransformationEpsilon(1e-8);
ptoplane_icp.setMaxCorrespondenceDistance(1);
ptoplane_icp.setEuclideanFitnessEpsilon(0.0001);
ptoplane_icp.setMaximumIterations(150); 可见只进行了7次迭代用时1.6s. 总结
ICP算法功能强大算法种类也很多在实际使用时需要根据实际应用场景开发适合的ICP自适应算法。
