本文首發于微信公眾號「3D視覺工坊」：3Ｄ點云配準（二多幅點云配準）

在上一篇文章 點云配準（一 兩兩配準）中我們介紹了兩兩點云之間的配準原理。本篇文章，我們主要介紹一下PCL中對于多幅點云連續配準的實現過程，重點請關注代碼行的注釋。

對于多幅點云的配準，它的主要思想是對所有點云進行變換，使得都與第一個點云在統一坐標系中。在每個連貫的、有重疊的點云之間找到最佳的變換，并累積這些變換到全部的點云。能夠進行ICP算法的點云需要進行粗略的預匹配，并且一個點云與另一個點云需要有重疊部分。

此處我們以郭浩主編的《點云庫PCL從入門到精通》提供的示例demo來介紹一下多幅點云進行配準的過程。

/* ---[ */ int main (int argc, char** argv) {// 加載數據 std::vector<PCD, Eigen::aligned_allocator<PCD> > data; //存儲管理所有打開的點云 loadData (argc, argv, data); //加載所有點云文件到data//檢查用戶輸入 if (data.empty ()){PCL_ERROR ("Syntax is: %s <source.pcd> <target.pcd> [*]", argv[0]);PCL_ERROR ("[*] - multiple files can be added. The registration results of (i, i+1) will be registered against (i+2), etc");PCL_INFO ("Example: %s `rospack find pcl`/test/bun0.pcd `rospack find pcl`/test/bun4.pcd", argv[0]);return (-1);}PCL_INFO ("Loaded %d datasets.", (int)data.size ());//創建一個PCL可視化對象 p = new pcl::visualization::PCLVisualizer (argc, argv, "Pairwise Incremental Registration example"); //創建一個PCL可視化對象 p->createViewPort (0.0, 0, 0.5, 1.0, vp_1); //用左半窗口創建視口vp_1 p->createViewPort (0.5, 0, 1.0, 1.0, vp_2); //用右半窗口創建視口vp_2 PointCloud::Ptr result (new PointCloud), source, target;Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform;for (size_t i = 1; i < data.size (); ++i) //循環處理所有點云 {source = data[i-1].cloud; //連續配準 target = data[i].cloud; //相鄰兩組點云//添加可視化數據 showCloudsLeft(source, target); //可視化為配準的源和目標點云 PointCloud::Ptr temp (new PointCloud);PCL_INFO ("Aligning %s (%d) with %s (%d).n", data[i-1].f_name.c_str (), source->points.size (), data[i].f_name.c_str (), target->points.size ());//調用子函數完成一組點云的配準，temp返回配準后兩組點云在第一組點云坐標下的點云，pairTransform返回從目標點云target到源點云source的變換矩陣。//現在我們開始進行實際的匹配，由子函數pairAlign具體實現，//其中參數有輸入一組需要配準的點云，以及是否進行下采樣的設置項，其他參數輸出配準后的點云及變換矩陣。 pairAlign (source, target, temp, pairTransform, true);//把當前的兩兩配對轉換到全局變換//把當前的兩兩配對后的點云temp轉換到全局坐標系下（第一個輸入點云的坐標系）返回result pcl::transformPointCloud (*temp, *result, GlobalTransform);//update the global transform更新全局變換//用當前的兩組點云之間的變換更新全局變換 GlobalTransform = pairTransform * GlobalTransform;//保存轉換到第一個點云坐標下的當前配準后的兩組點云result到文件i.pcd std::stringstream ss;ss << i << ".pcd";pcl::io::savePCDFile (ss.str (), *result, true);} }

對于上述過程中的核心函數pairAlign()，我們重點介紹如下：

/**匹配一對點云數據集并且返還結果*參數 cloud_src 是源點云*參數 cloud_src 是目標點云*參數output輸出的配準結果的源點云*參數final_transform是在來源和目標之間的轉換*/ void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false) {//下采樣//為了一致性和高速的下采樣//注意：為了大數據集需要允許這項 PointCloud::Ptr src (new PointCloud); //存儲濾波后的源點云 PointCloud::Ptr tgt (new PointCloud); //存儲濾波后的目標點云 pcl::VoxelGrid<PointT> grid; //濾波處理對象 if (downsample){grid.setLeafSize (0.05, 0.05, 0.05); //設置濾波處理時采用的體素大小 grid.setInputCloud (cloud_src);grid.filter (*src);grid.setInputCloud (cloud_tgt);grid.filter (*tgt);}else {src = cloud_src;tgt = cloud_tgt;}//計算曲面法線和曲率 PointCloudWithNormals::Ptr points_with_normals_src (new PointCloudWithNormals);PointCloudWithNormals::Ptr points_with_normals_tgt (new PointCloudWithNormals);pcl::NormalEstimation<PointT, PointNormalT> norm_est; //點云法線估計對象 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());norm_est.setSearchMethod (tree); //設置估計對象采用的搜索對象 norm_est.setKSearch (30); //設置估計時進行搜索用的k數 norm_est.setInputCloud (src);norm_est.compute (*points_with_normals_src); //下面分別估計源和目標點云法線 pcl::copyPointCloud (*src, *points_with_normals_src);norm_est.setInputCloud (tgt);norm_est.compute (*points_with_normals_tgt);pcl::copyPointCloud (*tgt, *points_with_normals_tgt);//一切準備好之后，可以開始配準了，創建ICP對象，設置它的參數//以需要匹配的兩個點云作為輸入，使用時，參數的設置需要根據自己的數據集進行調整。////舉例說明我們自定義點的表示（以上定義） MyPointRepresentation point_representation;//調整'curvature'尺寸權重以便使它和x, y, z平衡 float alpha[4] = {1.0, 1.0, 1.0, 1.0};point_representation.setRescaleValues (alpha);//// 配準 pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg; //配準對象 reg.setTransformationEpsilon (1e-6); //設置收斂判斷條件，越小精度越大，收斂也越慢//將兩個對應關系之間的(src<->tgt)最大距離設置為10厘米//注意：根據你的數據集大小來調整 reg.setMaxCorrespondenceDistance (0.1);//設置點表示 reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));reg.setInputCloud (points_with_normals_src); //設置源點云 reg.setInputTarget (points_with_normals_tgt); //設置目標點云////在一個循環中運行相同的最優化并且使結果可視化 Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;PointCloudWithNormals::Ptr reg_result = points_with_normals_src;//由于這是一個demo，因而希望顯示匹配過程的迭代過程，為達到該目的，ICP在內部進行計算時，限制其最大的迭代次數為2，即每迭代兩次就認為收斂，停止內部迭代 reg.setMaximumIterations (2); //設置最大迭代次數//手動迭代，此處設置為30次，每手動迭代一次，在配準結果視口對迭代的最新的結果進行刷新顯示 for (int i = 0; i < 30; ++i){PCL_INFO ("Iteration Nr. %d.n", i);//為了可視化的目的保存點云 points_with_normals_src = reg_result;reg.setInputCloud (points_with_normals_src);reg.align (*reg_result);//在每一個迭代之間累積轉換 Ti = reg.getFinalTransformation () * Ti;//如果這次轉換和之前轉換之間的差異小于閾值//則通過減小最大對應距離來改善程序//也就是說，如果迭代N次找到的變換和迭代N-1次中找到的變換之間的差異小于傳給ICP的變換收斂閾值，我們選擇源和目標之間更靠近的對應點距離閾值來改善配準過程if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);prev = reg.getLastIncrementalTransformation ();//可視化當前狀態 showCloudsRight(points_with_normals_tgt, points_with_normals_src);}////一旦找到最優的變換，ICP返回的變換是從源點云到目標點云的變換矩陣，我們求逆變換得到從目標點云到源點云的變換矩陣，并應用到目標點云，變換后的目標點云然后添加到源點云中。// 得到目標點云到源點云的變換 targetToSource = Ti.inverse();////把目標點云轉換回源框架 pcl::transformPointCloud (*cloud_tgt, *output, targetToSource);p->removePointCloud ("source");p->removePointCloud ("target");PointCloudColorHandlerCustom<PointT> cloud_tgt_h (output, 0, 255, 0);PointCloudColorHandlerCustom<PointT> cloud_src_h (cloud_src, 255, 0, 0);p->addPointCloud (output, cloud_tgt_h, "target", vp_2);p->addPointCloud (cloud_src, cloud_src_h, "source", vp_2);PCL_INFO ("Press q to continue the registration.n");p->spin ();p->removePointCloud ("source");p->removePointCloud ("target");//添加源點云到轉換目標 *output += *cloud_src;final_transform = targetToSource;} 效果圖如下，此為動態圖，請耐心等待幾秒鐘，注意迭代過程。

思考：

對于小型或者中型數量的點云數據（點個數<100，000），我們選擇ICP來進行迭代配準，可是利用對應點的特征計算和匹配較為耗時，如果對于兩個大型點云（都超過100000）之間的剛體變換的確定，有什么好辦法可以解決呢？

主要參考:郭浩主編的<點云庫PCL從入門到精通>

上述內容，如有侵犯版權，請聯系作者，會自行刪文。

星球成員，可免費提問，并邀進討論群

總結

以上是生活随笔為你收集整理的保存点云数据_3Ｄ点云配准（二多幅点云配准）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。