接下來介紹如何在PCL中實現多幅點雲的配准。在程序中加入當前代碼：
/* \author Radu Bogdan Rusu * adaptation Raphael Favier*/ #include <boost/make_shared.hpp> #include <pcl/point_types.h> #include <pcl/point_cloud.h> #include <pcl/point_representation.h> #include <pcl/io/pcd_io.h> #include <pcl/filters/voxel_grid.h> #include <pcl/filters/filter.h> #include <pcl/features/normal_3d.h> #include <pcl/registration/icp.h> #include <pcl/registration/icp_nl.h> #include <pcl/registration/transforms.h> #include <pcl/visualization/pcl_visualizer.h> using pcl::visualization::PointCloudColorHandlerGenericField; using pcl::visualization::PointCloudColorHandlerCustom; //簡單類型定義 typedef pcl::PointXYZ PointT; typedef pcl::PointCloud<PointT> PointCloud; typedef pcl::PointNormal PointNormalT; typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals; //這是一個輔助教程，因此我們可以負擔全局變量 //創建可視化工具 pcl::visualization::PCLVisualizer *p; //定義左右視點 int vp_1, vp_2; //處理點雲的方便的結構定義 struct PCD { PointCloud::Ptr cloud; std::string f_name; PCD() : cloud (new PointCloud) {}; }; struct PCDComparator { bool operator () (const PCD& p1, const PCD& p2) { return (p1.f_name < p2.f_name); } }; //以< x, y, z, curvature >形式定義一個新的點 class MyPointRepresentation : public pcl::PointRepresentation <PointNormalT> { using pcl::PointRepresentation<PointNormalT>::nr_dimensions_; public: MyPointRepresentation () { //定義尺寸值 nr_dimensions_ = 4; } //覆蓋copyToFloatArray方法來定義我們的特征矢量 virtual void copyToFloatArray (const PointNormalT &p, float * out) const { // < x, y, z, curvature > out[0] = p.x; out[1] = p.y; out[2] = p.z; out[3] = p.curvature; } }; //////////////////////////////////////////////////////////////////////////////// /** 在可視化窗口的第一視點顯示源點雲和目標點雲 * */ void showCloudsLeft(const PointCloud::Ptr cloud_target, const PointCloud::Ptr cloud_source) { p->removePointCloud ("vp1_target"); p->removePointCloud ("vp1_source"); PointCloudColorHandlerCustom<PointT> tgt_h (cloud_target, 0, 255, 0); PointCloudColorHandlerCustom<PointT> src_h (cloud_source, 255, 0, 0); p->addPointCloud (cloud_target, tgt_h, "vp1_target", vp_1); p->addPointCloud (cloud_source, src_h, "vp1_source", vp_1); PCL_INFO ("Press q to begin the registration.\n"); p-> spin(); } //////////////////////////////////////////////////////////////////////////////// /**在可視化窗口的第二視點顯示源點雲和目標點雲 * */ void showCloudsRight(const PointCloudWithNormals::Ptr cloud_target, const PointCloudWithNormals::Ptr cloud_source) { p->removePointCloud ("source"); p->removePointCloud ("target"); PointCloudColorHandlerGenericField<PointNormalT> tgt_color_handler (cloud_target, "curvature"); if (!tgt_color_handler.isCapable ()) PCL_WARN ("Cannot create curvature color handler!"); PointCloudColorHandlerGenericField<PointNormalT> src_color_handler (cloud_source, "curvature"); if (!src_color_handler.isCapable ()) PCL_WARN ("Cannot create curvature color handler!"); p->addPointCloud (cloud_target, tgt_color_handler, "target", vp_2); p->addPointCloud (cloud_source, src_color_handler, "source", vp_2); p->spinOnce(); } //////////////////////////////////////////////////////////////////////////////// /**加載一組我們想要匹配在一起的PCD文件 * 參數argc是參數的數量 (pass from main ()) *參數 argv 實際的命令行參數 (pass from main ()) *參數models點雲數據集的合成矢量 */ void loadData (int argc, char **argv, std::vector<PCD, Eigen::aligned_allocator<PCD> > &models) { std::string extension (".pcd"); //假定第一個參數是實際測試模型 for (int i = 1; i < argc; i++) { std::string fname = std::string (argv[i]); // 至少需要5個字符長（因為.plot就有 5個字符） if (fname.size () <= extension.size ()) continue; std::transform (fname.begin (), fname.end (), fname.begin (), (int(*)(int))tolower); //檢查參數是一個pcd文件 if (fname.compare (fname.size () - extension.size (), extension.size (), extension) == 0) { //加載點雲並保存在總體的模型列表中 PCD m; m.f_name = argv[i]; pcl::io::loadPCDFile (argv[i], *m.cloud); //從點雲中移除NAN點 std::vector<int> indices; pcl::removeNaNFromPointCloud(*m.cloud,*m.cloud, indices); models.push_back (m); } } } //////////////////////////////////////////////////////////////////////////////// /**匹配一對點雲數據集並且返還結果 *參數 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); 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); // //舉例說明我們自定義點的表示（以上定義） 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; reg.setMaximumIterations (2); 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; //如果這次轉換和之前轉換之間的差異小於閾值 //則通過減小最大對應距離來改善程序 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); } // // 得到目標點雲到源點雲的變換 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; } /* ---[ */ int main (int argc, char** argv) { // 加載數據 std::vector<PCD, Eigen::aligned_allocator<PCD> > data; loadData (argc, argv, 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"); p->createViewPort (0.0, 0, 0.5, 1.0, vp_1); p->createViewPort (0.5, 0, 1.0, 1.0, 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 ()); pairAlign (source, target, temp, pairTransform, true); //把當前的兩兩配對轉換到全局變換 pcl::transformPointCloud (*temp, *result, GlobalTransform); //update the global transform更新全局變換 GlobalTransform = pairTransform * GlobalTransform; //保存配准對，轉換到第一個點雲框架中 std::stringstream ss; ss << i << ".pcd"; pcl::io::savePCDFile (ss.str (), *result, true); } return 0; } /* ]--- */
編譯該程序，在vs中找到編譯文件.exe,在此例子中，在Debug中找到text.exe。
在文件夾下，按住shift+右鍵，選擇在當前目錄下打開CMD。輸入如下指令，xxx.pcd是指待匹配的對象。匹配完成后程序將匹配結果存入一個名為1.pcd的文件。
text.exe room0001.pcd room0002.pcd