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

關于目前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;
}
最后編輯于
?著作權歸作者所有,轉載或內容合作請聯系作者
平臺聲明:文章內容(如有圖片或視頻亦包括在內)由作者上傳并發布,文章內容僅代表作者本人觀點,簡書系信息發布平臺,僅提供信息存儲服務。

推薦閱讀更多精彩內容