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

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

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

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

/* ---[ */ int main (int argc, char** argv) {// 加載數(shù)據(jù) 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 ());//創(chuàng)建一個PCL可視化對象 p = new pcl::visualization::PCLVisualizer (argc, argv, "Pairwise Incremental Registration example"); //創(chuàng)建一個PCL可視化對象 p->createViewPort (0.0, 0, 0.5, 1.0, vp_1); //用左半窗口創(chuàng)建視口vp_1 p->createViewPort (0.5, 0, 1.0, 1.0, vp_2); //用右半窗口創(chuàng)建視口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) //循環(huán)處理所有點云 {source = data[i-1].cloud; //連續(xù)配準 target = data[i].cloud; //相鄰兩組點云//添加可視化數(shù)據(jù) 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 ());//調(diào)用子函數(shù)完成一組點云的配準，temp返回配準后兩組點云在第一組點云坐標下的點云，pairTransform返回從目標點云target到源點云source的變換矩陣。//現(xiàn)在我們開始進行實際的匹配，由子函數(shù)pairAlign具體實現(xiàn)，//其中參數(shù)有輸入一組需要配準的點云，以及是否進行下采樣的設(shè)置項，其他參數(shù)輸出配準后的點云及變換矩陣。 pairAlign (source, target, temp, pairTransform, true);//把當前的兩兩配對轉(zhuǎn)換到全局變換//把當前的兩兩配對后的點云temp轉(zhuǎn)換到全局坐標系下（第一個輸入點云的坐標系）返回result pcl::transformPointCloud (*temp, *result, GlobalTransform);//update the global transform更新全局變換//用當前的兩組點云之間的變換更新全局變換 GlobalTransform = pairTransform * GlobalTransform;//保存轉(zhuǎn)換到第一個點云坐標下的當前配準后的兩組點云result到文件i.pcd std::stringstream ss;ss << i << ".pcd";pcl::io::savePCDFile (ss.str (), *result, true);} }

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

/**匹配一對點云數(shù)據(jù)集并且返還結(jié)果*參數(shù) cloud_src 是源點云*參數(shù) cloud_src 是目標點云*參數(shù)output輸出的配準結(jié)果的源點云*參數(shù)final_transform是在來源和目標之間的轉(zhuǎn)換*/ void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false) {//下采樣//為了一致性和高速的下采樣//注意：為了大數(shù)據(jù)集需要允許這項 PointCloud::Ptr src (new PointCloud); //存儲濾波后的源點云 PointCloud::Ptr tgt (new PointCloud); //存儲濾波后的目標點云 pcl::VoxelGrid<PointT> grid; //濾波處理對象 if (downsample){grid.setLeafSize (0.05, 0.05, 0.05); //設(shè)置濾波處理時采用的體素大小 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); //設(shè)置估計對象采用的搜索對象 norm_est.setKSearch (30); //設(shè)置估計時進行搜索用的k數(shù) 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);//一切準備好之后，可以開始配準了，創(chuàng)建ICP對象，設(shè)置它的參數(shù)//以需要匹配的兩個點云作為輸入，使用時，參數(shù)的設(shè)置需要根據(jù)自己的數(shù)據(jù)集進行調(diào)整。////舉例說明我們自定義點的表示（以上定義） MyPointRepresentation point_representation;//調(diào)整'curvature'尺寸權(quán)重以便使它和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); //設(shè)置收斂判斷條件，越小精度越大，收斂也越慢//將兩個對應(yīng)關(guān)系之間的(src<->tgt)最大距離設(shè)置為10厘米//注意：根據(jù)你的數(shù)據(jù)集大小來調(diào)整 reg.setMaxCorrespondenceDistance (0.1);//設(shè)置點表示 reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));reg.setInputCloud (points_with_normals_src); //設(shè)置源點云 reg.setInputTarget (points_with_normals_tgt); //設(shè)置目標點云////在一個循環(huán)中運行相同的最優(yōu)化并且使結(jié)果可視化 Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;PointCloudWithNormals::Ptr reg_result = points_with_normals_src;//由于這是一個demo，因而希望顯示匹配過程的迭代過程，為達到該目的，ICP在內(nèi)部進行計算時，限制其最大的迭代次數(shù)為2，即每迭代兩次就認為收斂，停止內(nèi)部迭代 reg.setMaximumIterations (2); //設(shè)置最大迭代次數(shù)//手動迭代，此處設(shè)置為30次，每手動迭代一次，在配準結(jié)果視口對迭代的最新的結(jié)果進行刷新顯示 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);//在每一個迭代之間累積轉(zhuǎn)換 Ti = reg.getFinalTransformation () * Ti;//如果這次轉(zhuǎn)換和之前轉(zhuǎn)換之間的差異小于閾值//則通過減小最大對應(yīng)距離來改善程序//也就是說，如果迭代N次找到的變換和迭代N-1次中找到的變換之間的差異小于傳給ICP的變換收斂閾值，我們選擇源和目標之間更靠近的對應(yīng)點距離閾值來改善配準過程if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);prev = reg.getLastIncrementalTransformation ();//可視化當前狀態(tài) showCloudsRight(points_with_normals_tgt, points_with_normals_src);}////一旦找到最優(yōu)的變換，ICP返回的變換是從源點云到目標點云的變換矩陣，我們求逆變換得到從目標點云到源點云的變換矩陣，并應(yīng)用到目標點云，變換后的目標點云然后添加到源點云中。// 得到目標點云到源點云的變換 targetToSource = Ti.inverse();////把目標點云轉(zhuǎn)換回源框架 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");//添加源點云到轉(zhuǎn)換目標 *output += *cloud_src;final_transform = targetToSource;} 效果圖如下，此為動態(tài)圖，請耐心等待幾秒鐘，注意迭代過程。

思考：

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

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

上述內(nèi)容，如有侵犯版權(quán)，請聯(lián)系作者，會自行刪文。

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

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。