關于目前ICP算法的理解和總結

基本概念之配準

有兩個點集，source和target，target不變，source經過旋轉（Rotation）和平移（Translation）甚至加上尺度（Scale）變換，使得變換后的source點集盡量和target點集重合，這個變換的過程就叫點集配準。

兩個點集的對應，輸出通常是一個4×4剛性變換矩陣：代表旋轉和平移，它應用于源數據集，結果是完全與目標數據集匹配。下圖是“雙對應”算法中一次迭代的步驟：

image.png

對兩個數據源a，b匹配運算步驟如下：

1. 從其中一個數據源a出發，分析其最能代表兩個數據源場景共同點的關鍵點k
2. 在每個關鍵點ki處，算出一個特征描述子fi
3. 從這組特征描述子{fi}和他們在a和b中的XYZ坐標位置，基于fi和xyz的相似度，找出一組對應
4. 由于實際數據源是有噪的，所以不是所有的對應都有效，這就需要一步一步排除對匹配起負作用的對應
   從剩下的較好對應中，估計出一個變換

icp優化思路

首先icp是一步一步迭代得到較好的結果，解的過程其實是一個優化問題，并不能達到絕對正解。這個過程中求兩個點云之間變換矩陣是最重要的，PCL里是用奇異值分解SVD實現的

從上面的icp流程中我們可以看到的可以用不同的算法對1、2、3這三個步驟進行優化

關于PLICP調研和優化方案，后續給出

目前我們的代碼中的重要參數如下，需要我們設置的參數為加粗部分

inline void inline void setSearchMethodTarget(const KdTreePtr &tree) kdtree

加速搜索，還有一個Target的函數，用法與之一致。

inline void setInputSource (constPointCloudSourceConstPtr &cloud)

需要匹配的點云。

inline void setInputTarget (constPointCloudTargetConstPtr &cloud)

基準點云，也就是從Source到Target的匹配。

inline void setMaxCorrespondenceDistance (doubledistance_threshold) 

忽略在此距離之外的點，如果兩個點云距離較大，這個值要設的大一些（PCL默認距離單位是m）。目前我們的launch文件中mcpd設置的大小為0.05

inline void setTransformationEpsilon (doubleepsilon) 

第2個約束，這個值一般設為1e-6或者更小。（目前在launch文件中并沒有設置，代碼1e-6，這個值可以更?。?/h4>

inline void setEuclideanFitnessEpsilon (doubleepsilon) 

第3個約束，前后兩次迭代誤差的差值。（目前在launch文件中并沒有設置，代碼1e-6，太小，可優化）

inline void setMaximumIterations (intnr_iterations) 

第1個約束，迭代次數，幾十上百都可能出現。目前在launch文件中maxIterations為250

inline void align (PointCloudSource &output)

輸出配準后點云。

inline Matrix4 getFinalTransformation () 

獲取最終的轉換矩陣。

bool ScanMatch::icp(const pcl::PointCloud<pcl::PointXYZ>::Ptr &scan_cloud,
                    const pcl::PointCloud<pcl::PointXYZ>::Ptr &map_cloud, tf2::Transform &pose_corrections_out) {
    pose_corrections_out = tf2::Transform::getIdentity();
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> reg;
    //第2個約束，這個值一般設為1e-6或者更小
    reg.setTransformationEpsilon(1e-6);
    //第3個約束，前后兩次迭代誤差的差值
    reg.setEuclideanFitnessEpsilon(1e-6);
    //忽略在此距離之外的點，如果兩個點云距離較大，這個值要設的大一些
    reg.setMaxCorrespondenceDistance(0.02);//0.02
    // 第1個約束，迭代次數，幾十上百都可能出現
    reg.setMaximumIterations(maxIterations_);
    //進行RANSAC迭代
    reg.setRANSACIterations(maxIterations_);
    //剔除錯誤估計，可用 RANSAC 算法，或減少數量
    reg.setRANSACOutlierRejectionThreshold(0.05);
    //設置查找近鄰時是否雙向搜索
    reg.setUseReciprocalCorrespondences(true);
    {
        pcl::search::KdTree<pcl::PointXYZ>::Ptr target_pointcloud_search_method(
            new pcl::search::KdTree<pcl::PointXYZ>());
        target_pointcloud_search_method->setInputCloud(map_cloud);
        reg.setInputTarget(map_cloud);
        reg.setSearchMethodTarget(target_pointcloud_search_method, true);
    }

    pcl::search::KdTree<pcl::PointXYZ>::Ptr reference_pointcloud_search_method(new pcl::search::KdTree<pcl::PointXYZ>());
    reference_pointcloud_search_method->setInputCloud(map_cloud);
    {
        pcl::search::KdTmree<pcl::PointXYZ>::Ptr source_pointcloud_search_method(new pcl::search::KdTree<pcl::PointXYZ>());
        source_pointcloud_search_method->setInputCloud(scan_cloud);
        reg.setSearchMethodSource(source_pointcloud_search_method, true);
        reg.setInputSource(scan_cloud);
    }
    pcl::PointCloud<pcl::PointXYZ>::Ptr unused(new pcl::PointCloud<pcl::PointXYZ>());
    //調用該函數開配準
    reg.align(*unused);
    const Eigen::Matrix4f &transf = reg.getFinalTransformation();
    tf::Transform tf;
    matrixAsTransfrom(transf, tf);
    if (reg.hasConverged()) {
        tfToTF2(tf, pose_corrections_out);
    }
    return false;
}