目录

1.寻找匹配点对

2.直接法求解R和t

2.1 求解旋转

2.2 求解平移

2.3 对应代码实现

3.迭代上述步骤

4.完整流程

注：本篇笔记的主体内容来源于对深蓝学院《多传感器融合定位》课程的学习，推荐有一定基础的SLAM初学者学习该课程。

两帧点云的配准过程如上图过程所示。假设当前存在两个点云集合（相邻两帧点云scan）：

其中X为k-1时刻激光雷达采集到的三维点云（目标点集），Y为k时刻采集到的三维点云（源点集合）。配准的目的为，寻找一对合适的旋转R与平移t，即帧间相对位姿T=[R,t]，对Y进行位姿变换后：

使Y‘可以完美贴合至X（理想情况）：

配准的目标，即残差函数，可以描述为寻找一对合适的[R, t]，使旋转后的yi与xi之间的距离最小：

其中，m为集合Y中能找到对应点的点的数量，为在集合X中的对应点。算法流程大致为：

1.寻找匹配点对

配准过程需要知道两个点集中点的匹配关系，所以需要对集合Y中的点在目标集合X中寻找对应点。由于普通的激光点只包含三维坐标数据，无法像视觉特征点通过比较描述子的相似度来显示地找到对应点，所以一般通过将Y集合的点根据初始的相对位姿估计投影到k-1时刻，并根据三维坐标距离，寻找离距离最近的作为对应点。（如LOAM）

点云投影：

// input_source_：k时刻的源点云 // transformed_input_source：用于保存转换后的点云 // predict_pose：预测的帧间相对位姿T(k-1_k) pcl::transformPointCloud(*input_source_, *transformed_input_source, predict_pose);

对于一个点在另一团点云中寻找最近点，可以利用kd-tree加速：

pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr input_target_kdtree_; // input_target_：k-1时刻的目标点云 input_target_kdtree_->setInputCloud(input_target_);

通过kd-tree在X中为yi寻找xi：

// input_source：投影到k-1时刻的Y点云 // ys：保存能在X点云中找到对应点xi的点yi // xs：保存xi size_t ICPSVDRegistration::GetCorrespondence( const CloudData::CLOUD_PTR &input_source, // Y集合 std::vector<Eigen::Vector3f> &xs, std::vector<Eigen::Vector3f> &ys ) { const float MAX_CORR_DIST_SQR = max_corr_dist_ * max_corr_dist_; size_t num_corr = 0; std::vector<int> pointSearchIndex; std::vector<float> pointSearchDistance; for(size_t i = 0; i < input_source->size(); ++i) { // 1：只找最近的1个点 input_target_kdtree_->nearestKSearch(input_source->points[i], 1, pointSearchIndex, pointSearchDistance); // MAX_CORR_DIST_SQR：限制点的最远距离 if(pointSearchDistance.size() == 1 && pointSearchDistance[0] < MAX_CORR_DIST_SQR) { CloudData::POINT point = input_source->points[i]; ys.push_back(Eigen::Vector3f{point.x, point.y, point.z}); point = input_target_->points[pointSearchIndex[0]]; xs.push_back(Eigen::Vector3f{point.x, point.y, point.z}); ++num_corr; // pointSearchIndex.clear(); // pointSearchDistance.clear(); } } return num_corr; }

因为并不能为Y集合中的每个点在X集合中都找到合适的最近点，所以只保存能找到的合适距离之内的yi和xi。

2.直接法求解R和t

残差函数E可以化简：

由于：

则：

其中，只与旋转有关。根据求解出旋转R后，就可以代入，并令求解平移t。

2.1 求解旋转

继续看：

因为R是正交矩阵，，所以可以化简成如上形式。由于与为常量，可以忽略，则：

此时为了将R从两个向量的夹心中提取出来，利用矩阵的迹的性质。由于：

是标量，并且标量的迹等于其本身，则：

迹（trace）：

而标量的迹是其本身，所以最大化等价于最大化。

再根据迹的循环性质，得：

因为和Trace()都是线性算子，它们可以交换顺序。所以上式中将求和符号放进迹的内部。并且令：

H矩阵为去中心化后的协方差矩阵，则：

此时，问题转化为，寻找合适的R，使Trace(RH)的值最大。当前R和H都是3x3的矩阵。根据定理：

若有正定矩阵，则对于任何正交矩阵B，有。

若能寻找到一个R，能将转换成的形式，则该R就是能使值最大的R。

此时，对H进行SVD（奇异值分解）：

其中，为3x3的正交矩阵；为3x3的对角矩阵；为3x3的正交矩阵。取：

则有：

这就得到了能使值最大的R。

2.2 求解平移

令，则：

2.3 对应代码实现

// xs：对应目标点集 // ys：对应源点集 // transformation_：待求解的相对位姿变换增量 void ICPSVDRegistration::GetTransform( const std::vector<Eigen::Vector3f> &xs, const std::vector<Eigen::Vector3f> &ys, Eigen::Matrix4f &delta_transformation_ ) { const size_t N = xs.size(); // 1.计算两个点集各自的质心 Eigen::Vector3f mu_x = Eigen::Vector3f::Zero(), mu_y = Eigen::Vector3f::Zero(); for(size_t i = 0; i < N; ++i) { mu_x += xs[i]; mu_y += ys[i]; } mu_x /= N; mu_y /= N; // 2.构建H矩阵 Eigen::Matrix3f H = Eigen::Matrix3f::Zero(); for(size_t i = 0; i < N; ++i) { H += (ys[i] - mu_y) * (xs[i] - mu_x).transpose(); } // 3.对H执行SVD分解 Eigen::JacobiSVD<Eigen::Matrix3f> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3f U = svd.matrixU(); Eigen::Matrix3f V = svd.matrixV(); // 4.得到R，并检查行列式符号以防止镜像反射 Eigen::Matrix3f R = V * U.transpose(); if(R.determinant() < 0) { Eigen::Matrix3f I = Eigen::Matrix3f::Identity(); I(2,2) = -1; R = V * I * U.transpose(); } // 5.计算t Eigen::Vector3f t; t = mu_x - R * mu_y; // TODO: set output: delta_transformation_.setIdentity(); delta_transformation_.block<3,1>(0,3) = t; delta_transformation_.block<3,3>(0,0) = R; }

3.迭代上述步骤

因为寻找对应点时，并不能准确地为每个yi寻找到其真正对应的xi，此时只是用距离最近作为一个匹配关系，但并不准确。所以需要循环上述过程，直至残差或计算出的相对变换小于设定的阈值，则认为找到了两帧的相对位姿变换。

4.完整流程

// 当前帧点云，先验位姿，结果点云，结果位姿 bool ICPSVDRegistration::ScanMatch( const CloudData::CLOUD_PTR& input_source, const Eigen::Matrix4f& predict_pose, CloudData::CLOUD_PTR& result_cloud_ptr, Eigen::Matrix4f& result_pose ) { input_source_ = input_source; CloudData::CLOUD_PTR transformed_input_source(new CloudData::CLOUD()); // 将点云根据先验位姿T(lidar(k-1)_lidar(k))进行旋转 pcl::transformPointCloud(*input_source_, *transformed_input_source, predict_pose); // init estimation: transformation_.setIdentity(); int curr_iter = 0; std::vector<Eigen::Vector3f> xs,ys; // 目标点，源点 Eigen::Matrix4f delta_transformation = Eigen::Matrix4f::Identity(); while (curr_iter < max_iter_) { pcl::transformPointCloud(*transformed_input_source, *transformed_input_source, delta_transformation); // 获取对应点集 size_t num_corr = GetCorrespondence(transformed_input_source, xs, ys); // TODO: do not have enough correspondence -- break: size_t MIN_CORR_NUM = input_source_->size() * 0.5; if(num_corr < MIN_CORR_NUM) { break; } // 计算R,t GetTransform(xs, ys, delta_transformation); xs.clear(); ys.clear(); // 叠加位姿增量 transformation_ = delta_transformation * transformation_; Eigen::Matrix3f R = transformation_.block<3,3>(0,0); Eigen::Quaternionf q(R); q.normalize(); transformation_.block<3,3>(0,0) = q.toRotationMatrix(); // 阈值判断 if(!IsSignificant(delta_transformation, trans_eps_)) { break; } ++curr_iter; } // set output: result_pose = transformation_ * predict_pose; pcl::transformPointCloud(*input_source_, *result_cloud_ptr, result_pose); return true; }

注：目标点集也可以是一团局部地图点云，先验位姿predict_pose也可以是T(map_lidar(k))。

结尾，推荐深蓝学院的《多传感器融合定位》课程。