点到点ICP-基于SVD分解的帧间点云匹配

目录1.寻找匹配点对2.直接法求解R和t2.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::KdTreeFLANNpcl::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::vectorEigen::Vector3f xs, std::vectorEigen::Vector3f ys ) { const float MAX_CORR_DIST_SQR max_corr_dist_ * max_corr_dist_; size_t num_corr 0; std::vectorint pointSearchIndex; std::vectorfloat 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::vectorEigen::Vector3f xs, const std::vectorEigen::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::JacobiSVDEigen::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_.block3,1(0,3) t; delta_transformation_.block3,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::vectorEigen::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_.block3,3(0,0); Eigen::Quaternionf q(R); q.normalize(); transformation_.block3,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))。结尾推荐深蓝学院的《多传感器融合定位》课程。