ICP代码分析
PCL点云库的传统ICP代码分析
- ICP算法的介绍
- PCL里面的源码分析
ICP算法的介绍
ICP（Iterative Closest Point），即最近点迭代算法，是最为经典的数据配准算法。其特征在于，通过求取源点云和目标点云之间的对应点对，基于对应点对构造旋转平移矩阵，并利用所求矩阵，将源点云变换到目标点云的坐标系下，估计变换后源点云与目标点云的误差函数，若误差函数值大于阀值，则迭代进行上述运算直到满足给定的误差要求.
ICP算法采用最小二乘估计计算变换矩阵，原理简单且具有较好的精度，但是由于采用了迭代计算，导致算法计算速度较慢，而且采用ICP进行配准计算时，其对待配准点云的初始位置有一定要求，若所选初始位置不合理，则会导致算法陷入局部最优。。
PCL里面的源码分析
我接下来对pcl里面的源码了解了下，大体有些地方做了备注，但是未必万全正确。
- 首先要介绍的是主体的ICP启动程序：
#include 其中主要的功能就是在align（）这个函数中实现的。这个函数的大体位置是
registration/include/pcl/registraion/impl/registration.hpp这里。代码接下如下
- align（）函数
//函数里调用这个真正的函数
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output, const Matrix4& guess)
{//分配input点云的下标，函数在common/include/pcl/impl/pcl_base.hppif (!initCompute ()) return;// Resize the output dataset//如果output的下标数量和input不一样，那就成为一样的if (output.points.size () != indices_->size ())output.points.resize (indices_->size ());// Copy the headeroutput.header = input_->header;// Check if the output will be computed for all points or only a subset//这里没搞懂，感觉肯定是相等的呀？if (indices_->size () != input_->points.size ()){output.width = static_cast<uint32_t> (indices_->size ());output.height = 1;}else{output.width = static_cast<uint32_t> (input_->width);output.height = input_->height;}output.is_dense = input_->is_dense;// Copy the point data to output//这里的output就是final，也就是最后由input转化过来的点云，不是匹配的目标点云//因为没有被初试化的，所以直接拷贝点云for (size_t i = 0; i < indices_->size (); ++i)output.points[i] = input_->points[(*indices_)[i]];// Set the internal point representation of choice unless otherwise notedif (point_representation_ && !force_no_recompute_) tree_->setPointRepresentation (point_representation_);// Perform the actual transformation computationconverged_ = false;final_transformation_ = transformation_ = previous_transformation_ = Matrix4::Identity ();// Right before we estimate the transformation, we set all the point.data[3] values to 1 to aid the rigid // transformation//其实因为坐标是齐次坐标，所以第四个元素是1，前面三个元素是x,y,zfor (size_t i = 0; i < indices_->size (); ++i)output.points[i].data[3] = 1.0;//实现的icp.hpp里面，这个函数是重载函数，所以要找对//变种icp的更改基本都在这里，改动h,hpp文件，以及改动主要的computeTransformation函数，前面的都是预备工作，关系不大computeTransformation (output, guess);//这个函数仅仅是返回一个布尔值truedeinitCompute ();
}在这个align里面的最重要的函数就是computeTransformation (output, guess)。而这个函数就在registration/include/pcl/registraion/icp.hpp这里。
- computeTransformation（）函数
///
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (PointCloudSource &output, const Matrix4 &guess)
{// Point cloud containing the correspondences of each point in 在computeTransformation（）函数中主要用到的就是transformCloud（）函数以及estimateRigidTransformation（）函数还有determineCorrespondences（）的函数。
transformCloud（）位置
函数的位置也同样在icp.hpp里面。
determineCorrespondences（）位置 registration/include/pcl/registraion/correspondence_estimation.hpp里面。estimateRigidTransformation（）函数位置registration\include\pcl\registration\impl\transformation_estimation_svd.hpp。
- transformCloud（）函数
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformCloud ( const PointCloudSource &input, PointCloudSource &output, const Matrix4 &transform)
{//这里的input和output在第一次的时候还是相同的值//但是在第二次迭代的时候才是正常的步骤Eigen::Vector4f pt (0.0f, 0.0f, 0.0f, 1.0f), pt_t;Eigen::Matrix4f tr = transform.template cast<float> ();// XYZ is ALWAYS present due to the templatization, so we only have to check for normalsif (source_has_normals_){Eigen::Vector3f nt, nt_t;Eigen::Matrix3f rot = tr.block<3, 3> (0, 0);for (size_t i = 0; i < input.size (); ++i){将input的数据填充到pt里const uint8_t* data_in = reinterpret_cast<const uint8_t*> (&input[i]);uint8_t* data_out = reinterpret_cast<uint8_t*> (&output[i]);memcpy (&pt[0], data_in + x_idx_offset_, sizeof (float));memcpy (&pt[1], data_in + y_idx_offset_, sizeof (float));memcpy (&pt[2], data_in + z_idx_offset_, sizeof (float));if (!pcl_isfinite (pt[0]) || !pcl_isfinite (pt[1]) || !pcl_isfinite (pt[2])) continue;//这里就是转换的公式，是齐次的转换pt_t = tr * pt;//把pt_t的值给data_outmemcpy (data_out + x_idx_offset_, &pt_t[0], sizeof (float));memcpy (data_out + y_idx_offset_, &pt_t[1], sizeof (float));memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float));memcpy (&nt[0], data_in + nx_idx_offset_, sizeof (float));memcpy (&nt[1], data_in + ny_idx_offset_, sizeof (float));memcpy (&nt[2], data_in + nz_idx_offset_, sizeof (float));if (!pcl_isfinite (nt[0]) || !pcl_isfinite (nt[1]) || !pcl_isfinite (nt[2])) continue;//这里是非齐次的转换nt_t = rot * nt;//把转换后的nt_t给data_outmemcpy (data_out + nx_idx_offset_, &nt_t[0], sizeof (float));memcpy (data_out + ny_idx_offset_, &nt_t[1], sizeof (float));memcpy (data_out + nz_idx_offset_, &nt_t[2], sizeof (float));}}else{for (size_t i = 0; i < input.size (); ++i){const uint8_t* data_in = reinterpret_cast<const uint8_t*> (&input[i]);uint8_t* data_out = reinterpret_cast<uint8_t*> (&output[i]);memcpy (&pt[0], data_in + x_idx_offset_, sizeof (float));memcpy (&pt[1], data_in + y_idx_offset_, sizeof (float));memcpy (&pt[2], data_in + z_idx_offset_, sizeof (float));if (!pcl_isfinite (pt[0]) || !pcl_isfinite (pt[1]) || !pcl_isfinite (pt[2])) continue;//这里是齐次的转换pt_t = tr * pt;memcpy (data_out + x_idx_offset_, &pt_t[0], sizeof (float));memcpy (data_out + y_idx_offset_, &pt_t[1], sizeof (float));memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float));}}}- determineCorrespondences()函数
///
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences (pcl::Correspondences &correspondences, double max_distance)
{if (!initCompute ())return;double max_dist_sqr = max_distance * max_distance;correspondences.resize (indices_->size ());std::vector<int> index (1);std::vector<float> distance (1);pcl::Correspondence corr;unsigned int nr_valid_correspondences = 0;// Check if the template types are the same. If true, avoid a copy.// Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!if (isSamePointType<PointSource, PointTarget> ()){// Iterate over the input set of source indicesfor (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx){tree_->nearestKSearch (input_->points[*idx], 1, index, distance);if (distance[0] > max_dist_sqr)continue;corr.index_query = *idx;corr.index_match = index[0];corr.distance = distance[0];correspondences[nr_valid_correspondences++] = corr;}}else{PointTarget pt;// Iterate over the input set of source indicesfor (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx){// Copy the source data to a target PointTarget format so we can search in the treecopyPoint (input_->points[*idx], pt);tree_->nearestKSearch (pt, 1, index, distance);if (distance[0] > max_dist_sqr)continue;corr.index_query = *idx;corr.index_match = index[0];corr.distance = distance[0];correspondences[nr_valid_correspondences++] = corr;}}correspondences.resize (nr_valid_correspondences);deinitCompute ();
}
- estimateRigidTransformation（）
#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_
#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_#include is the source dataset transformation_matrix.setIdentity ();Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;// Estimate the centroids of source, targetcompute3DCentroid (source_it, centroid_src);compute3DCentroid (target_it, centroid_tgt);source_it.reset (); target_it.reset ();// Subtract the centroids from source, targetEigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;demeanPointCloud (source_it, centroid_src, cloud_src_demean);demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean);getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);}
}///
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,const Eigen::Matrix<Scalar, 4, 1> ¢roid_src,const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,const Eigen::Matrix<Scalar, 4, 1> ¢roid_tgt,Matrix4 &transformation_matrix) const
{transformation_matrix.setIdentity ();// Assemble the correlation matrix H = source * target'Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3);// Compute the Singular Value DecompositionEigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU ();Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV ();// Compute R = V * U'if (u.determinant () * v.determinant () < 0){for (int x = 0; x < 3; ++x)v (x, 2) *= -1;}Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose ();// Return the correct transformationtransformation_matrix.topLeftCorner (3, 3) = R;const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.head (3));transformation_matrix.block (0, 3, 3, 1) = centroid_tgt.head (3) - Rc;
}//#define PCL_INSTANTIATE_TransformationEstimationSVD(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimationSVD; #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_ */当初的代码注释，但是也是依照个人理解初次写，如果有问题，还请见谅！
本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！