点云注册

namespace: registration

点云注册的任务和视觉里程计很像，只不过视觉里程计的输入是一对Frame，而点云注册的输入是两个点云，没有别的假设，也就没法利用重投影到二维图片这样的操作了。一般来说用来注册的点云的规模比Frame要更大，可以是几十个帧提取的model，甚至是两个房间的model。

对于点云注册，和视觉里程计的Sparse和Dense类似，也有两种：基于特征与不基于特征。基于特征匹配的，一般称为Global Registration，不需要有初始位姿估计，也叫粗匹配，而后者的比较经典的算法就是ICP了，也叫精细匹配。

Global Registration

Global Registration不需要初始位姿，基本步骤为： - 提取3D特征 - 特征匹配 - outlier过滤 - 计算变换矩阵

实际上和SparseTracking是一样的。函数原型为：

std::shared_ptr<RegistrationResult> 
    RansacRegistration(const geometry::PointCloud &source_pcd, 
    const geometry::PointCloud &target_pcd, 
    const RANSACParameter &r_para = RANSACParameter());
std::shared_ptr<RegistrationResult> 
    RansacRegistration(const geometry::PointCloud &source_feature_pcd, 
    const geometry::PointCloud &target_feature_pcd, 
    const FeatureSet & source_features,
    const FeatureSet & target_features,
    const RANSACParameter &r_para);

上面两个函数的区别在于第一个是直接对两个点云进行注册，第二个是对特征点进行注册，也就是跳过了提取3D特征这一步。在构建一个系统时候，保存3D特征可以避免重复计算，提高效率。

提取3D特征的库一般在PCL或者Open3D这种库中有实现，但是这样的库太大，OpenCV没有实现3D特征的提取，所以OnePiece实现了这一部分，提取FPFH特征，可以通过下面的函数计算得到：

std::tuple<geometry::PointCloud, FeatureSet> DownSampleAndExtractFeature(const geometry::PointCloud &pcd, 
    const RANSACParameter &r_para = RANSACParameter()); 

上面函数输入为点云以及相关参数，返回提取到的特征点位置以及特征（33维向量）。在match时候，只要进行knnsearch，搜索最近点得到匹配结果。outlier的过滤与SparseTracking中原理很相似，在这里就不多介绍了。最后通过RANSAC来计算得到变换矩阵以及inliers。

ICP

ICP(iterative closest point)是很经典的算法，变种很多，OnePiece实现了ICP比较基础的两种，一种是点对点（PointToPoint），一种是点对面（PointToPlane）。

PointToPoint

优化点到点的距离，优化的目标函数为：\(\Vert T \cdot p_s - p_t\Vert^2\)。根据OpenCV的KDtree找到最近点，可通过geometry::EstimateRigidTransformation函数直接求得最优的\(T\)（也可以通过求雅可比矩阵优化），接着根据新的\(T\)重新寻找最近点，不断迭代到收敛。

PointToPlane

优化点到面的距离，需要知道target点云的法向量。经过\(p_t\)的，法向量为\(n_t\)的平面方程为：

\(n_t^T (\cdot p - p_t)\)为点到该平面的距离，因此优化的目标函数为：\(\Vert n_t^T\cdot(T\cdot p_s - p_t)\Vert^2\)。对于这个cost function的线性化，可以查看related_paper/lowk_point-to-plane_icp_techrep.pdf。通过雅可比矩阵优化得到新的\(T\)，接着根据新的\(T\)重新寻找最近点，不断迭代到收敛。

实验和理论分析都证明了，PointToPlane比PointToPoint有更快的收敛速度。调用方法为：

std::shared_ptr<RegistrationResult> PointToPoint(const geometry::PointCloud &source, const geometry::PointCloud &target, 
    const geometry::TransformationMatrix &init_T = geometry::TransformationMatrix::Identity(), const ICPParameter &icp_para = ICPParameter() );
std::shared_ptr<RegistrationResult> PointToPlane(const geometry::PointCloud &source, const geometry::PointCloud &target,
    const geometry::TransformationMatrix &init_T = geometry::TransformationMatrix::Identity(), const ICPParameter &icp_para = ICPParameter() );

其中icp_para是相关参数，直接使用默认的就是很好的选择。有两个类RegistrationResult类包含了:

class RegistrationResult
{
    public:
    //相对变换
    geometry::TransformationMatrix T;
    //匹配点ID
    geometry::FMatchSet correspondence_set_index;
    //匹配点
    geometry::PointCorrespondenceSet correspondence_set;
    //rmse
    double rmse;
};

在这里，是否匹配成功交给使用者来判断。