在逆向工程，計算機視覺，文物數字化等領域中，由於點雲的不完整，旋轉錯位，平移錯位等，使得要得到的完整的點雲就需要對局部點雲進行配准，為了得到被測物體的完整數據模型，需要確定一個合適的坐標系，將從各個視角得到的點集合並到統一的坐標系下形成一個完整的點雲，然后就可以方便進行可視化的操作，這就是點雲數據的配准。點雲的配准有手動配准依賴儀器的配准，和自動配准，點雲的自動配准技術是通過一定的算法或者統計學規律利用計算機計算兩塊點雲之間錯位，從而達到兩塊點雲自動配准的效果，其實質就是把不同的坐標系中測得到的數據點雲進行坐標系的變換，以得到整體的數據模型，問題的關鍵是如何讓得到坐標變換的參數R（旋轉矩陣）和T（平移向量），使得兩視角下測得的三維數據經坐標變換后的距離最小，，目前配准算法按照過程可以分為整體配准和局部配准，。PCL中有單獨的配准模塊，實現了配准相關的基礎數據結構，和經典的配准算法如ICP。
PCL中實現配准算法以及相關的概念
兩兩配准的簡介：一對點雲數據集的配准問題是兩兩配准（pairwise registration 或 pair-wise registration）.通常通過應用一個估計得到的表示平移和選裝的4*4缸體變換矩陣來使得一個點雲的數據集精確的與另一個點雲數據集（目標數據集）進行完美的配准
具體的實現步驟：
（1）首先從兩個數據集中按照同樣的關鍵點選取的標准，提取關鍵點
（2）對選擇所有的關鍵點分別計算其特征描述子
（3）結合特征描述子在兩個數據集中的坐標位置，以兩者之間的特征和位置的相似度為基礎，來估算它們的對應關系，初步的估計對應點對。
（4）假設數據是有噪聲，出去對配准有影響的錯誤的對應點對
（5）利用剩余的正確的對應關系來估算剛體變換，完整配准。
對應估計（correspondences estimation）：假設我們已經得到由來給你此掃描的點雲數據獲得的兩組特征向量，在此基礎基礎上，我們必須找到，相似特征再確定數據的重疊部分，然后才能進行配准，根據特征的類型PCL使用不同的方法來搜索特征之間的對應關系
使用點匹配時，使用點的XYZ的坐標作為特征值，針對有序點雲和無序點雲數據的不同的處理策略：
（1）窮舉配准（brute force matching）
(2)kd——數最近鄰查詢（FLANN）
（3）在有序點雲數據的圖像空間中查找
（4）在無序點雲數據的索引空間中查找
（3）對應關系的去除（correspondence rejection）
由於噪聲的影響，通常並不是所有估計的對應關系都是正確的，由於錯誤的對應關系對於最終的剛體變換矩陣的估算會產生負面的影響，所以必須去除它們，可以采用隨機采樣一致性估計，或者其他方法剔除錯誤的對應關系，最終使用對應關系數量只使用一定比例的對應關系，這樣既能提高變換矩陣的估計京都也可以提高配准點的速度。
（4）變換矩陣的估算（transormation estimation）
估算對應矩陣的步驟如下
1. 在對應關系的基礎上評估一些錯誤的度量標准
2.在攝像機位姿（運動估算）和最小化錯誤度量標准下估算一個剛體變換
3.優化點的結構
4使用剛體變換把源旋轉/平移到與目標所在的同一坐標系下，用所有點，點的一個子集或者關鍵點運算一個內部的ICP循環
5，進行迭代，直到符合收斂性判斷標准為止。
（5）迭代最近點算法（Iterative CLosest Point簡稱ICP算法）
ICP算法對待拼接的2片點雲，首先根據一定的准則確立對應點集P與Q，其中對應點對的個數，然后通過最小乘法迭代計算最優的坐標變換，即旋轉矩陣R和平移矢量t，使得誤差函數最小，ICP處理流程分為四個主要的步驟：
1. 對原始點雲數據進行采樣
2.確定初始對應點集
3，去除錯誤對應點對
4.坐標變換的求解
PCL類的相關的介紹
| class | pcl::CorrespondenceGrouping< PointModelT, PointSceneT > |
| Abstract base class for Correspondence Grouping algorithms | |
| class | pcl::GeometricConsistencyGrouping< PointModelT, PointSceneT > |
| Class implementing a 3D correspondence grouping enforcing geometric consistency among feature correspondences. | |
| class | pcl::recognition::HoughSpace3D |
| HoughSpace3D is a 3D voting space. | |
| class | pcl::Hough3DGrouping< PointModelT, PointSceneT, PointModelRfT, PointSceneRfT > |
| Class implementing a 3D correspondence grouping algorithm that can deal with multiple instances of a model template found into a given scene. | |
| class | pcl::CRHAlignment< PointT, nbins_ > |
| CRHAlignment uses two Camera Roll Histograms (CRH) to find the roll rotation that aligns both views. | |
| class | pcl::recognition::ObjRecRANSAC::Output |
| This is an output item of the ObjRecRANSAC::recognize() method. More... | |
| class | pcl::recognition::ObjRecRANSAC::OrientedPointPair |
| class | pcl::recognition::ObjRecRANSAC::HypothesisCreator |
| class | pcl::recognition::ObjRecRANSAC |
| This is a RANSAC-based 3D object recognition method. | |
| class | pcl::recognition::ORROctree::Node::Data |
| class | pcl::recognition::ORROctree::Node |
| class | pcl::recognition::ORROctree |
| That's a very specialized and simple octree class. | |
| class | pcl::recognition::RotationSpace |
| This is a class for a discrete representation of the rotation space based on the axis-angle representation. |
實例分析：
（1）如何使用迭代最近點算法：在代碼中使用ICP迭代最近點算法，程序隨機生成一個點與作為源點雲，並將其沿x軸平移后作為目標點雲，然后利用ICP估計源到目標的剛體變換橘子，中間對所有信息都打印出來
新建文件
iterative_closest_point.cpp
#include <iostream> //標准輸入輸出頭文件 #include <pcl/io/pcd_io.h> //I/O操作頭文件 #include <pcl/point_types.h> //點類型定義頭文件 #include <pcl/registration/icp.h> //ICP配准類相關頭文件 int main (int argc, char** argv) { //創建兩個pcl::PointCloud<pcl::PointXYZ>共享指針，並初始化它們 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>); // 隨機填充點雲 cloud_in->width = 5; //設置點雲寬度 cloud_in->height = 1; //設置點雲為無序點 cloud_in->is_dense = false; cloud_in->points.resize (cloud_in->width * cloud_in->height); for (size_t i = 0; i < cloud_in->points.size (); ++i) { cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f); cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f); cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f); } std::cout << "Saved " << cloud_in->points.size () << " data points to input:"//打印處點雲總數 << std::endl; for (size_t i = 0; i < cloud_in->points.size (); ++i) std::cout << " " << //打印處實際坐標 cloud_in->points[i].x << " " << cloud_in->points[i].y << " " << cloud_in->points[i].z << std::endl; *cloud_out = *cloud_in; std::cout << "size:" << cloud_out->points.size() << std::endl; //實現一個簡單的點雲剛體變換，以構造目標點雲 for (size_t i = 0; i < cloud_in->points.size (); ++i) cloud_out->points[i].x = cloud_in->points[i].x + 0.7f; std::cout << "Transformed " << cloud_in->points.size () << " data points:" << std::endl; for (size_t i = 0; i < cloud_out->points.size (); ++i) //打印構造出來的目標點雲 std::cout << " " << cloud_out->points[i].x << " " << cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl; pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; //創建IterativeClosestPoint的對象 icp.setInputCloud(cloud_in); //cloud_in設置為點雲的源點 icp.setInputTarget(cloud_out); //cloud_out設置為與cloud_in對應的匹配目標 pcl::PointCloud<pcl::PointXYZ> Final; //存儲經過配准變換點雲后的點雲 icp.align(Final); //打印配准相關輸入信息 std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl; std::cout << icp.getFinalTransformation() << std::endl; return (0); }
編譯運行的結果
可以有試驗結果看得出變換后的點雲只是在x軸的值增加了固定的值0.7,然后由這目標點雲與源點雲計算出它的旋轉與平移，明顯可以看出最后一行的x值為0.7
同時，我們可以自己更改程序，來觀察不同的實驗結果。
對於兩幅圖像通過ICP求它的變換：
剛開始，如果直接通過通過kinect 得到數據運行會出現如下的錯誤，是因為該ICP 算法不能處理含有NaNs的點雲數據，所以需要通過移除這些點，才能作為ICP算法的輸入
錯誤的提示
所以我們必須通過之前所學的代碼把其中的無效的點雲去除，然后作為輸入，由於是我自己獲得的點雲數據，數據沒有預處理，其中輸入的兩個點雲后ICP后的結果及可視化為
(2)如何逐步匹配多幅點雲
本實例是使用迭代最近點算法，逐步實現地對一系列點雲進行兩兩匹配，他的思想是對所有的點雲進行變換，使得都與第一個點雲統一坐標系中，在每個連貫的有重疊的點雲之間找出最佳的變換，並積累這些變換到全部的點雲，能夠進行ICP算法的點雲需要粗略的預匹配(比如在一個機器人的量距內或者在地圖的框架內)，並且一個點雲與另一個點雲需要有重疊的部分。
新建文件pairwise_incremental_registration.cpp
#include <boost/make_shared.hpp> //boost指針相關頭文件 #include <pcl/point_types.h> //點類型定義頭文件 #include <pcl/point_cloud.h> //點雲類定義頭文件 #include <pcl/point_representation.h> //點表示相關的頭文件 #include <pcl/io/pcd_io.h> //PCD文件打開存儲類頭文件 #include <pcl/filters/voxel_grid.h> //用於體素網格化的濾波類頭文件 #include <pcl/filters/filter.h> //濾波相關頭文件 #include <pcl/features/normal_3d.h> //法線特征頭文件 #include <pcl/registration/icp.h> //ICP類相關頭文件 #include <pcl/registration/icp_nl.h> //非線性ICP 相關頭文件 #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; //申明pcl::PointXYZ數據 typedef pcl::PointNormal PointNormalT; typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals; // 申明一個全局可視化對象變量，定義左右視點分別顯示配准前和配准后的結果點雲 pcl::visualization::PCLVisualizer *p; //創建可視化對象 int vp_1, vp_2; //定義存儲 左 右視點的ID //申明一個結構體方便對點雲以文件名和點雲對象進行成對處理和管理點雲，處理過程中可以同時接受多個點雲文件的輸入 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(); } //////////////////////////////////////////////////////////////////////////////// /** \brief Load a set of PCD files that we want to register together * \param argc the number of arguments (pass from main ()) * \param argv the actual command line arguments (pass from main ()) * \param models the resultant vector of point cloud datasets */ 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]); // PCD文件名至少為5個字符大小字符串（因為后綴名.pcd就已經占了四個字符位置） 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); } } } //////////////////////////////////////////////////////////////////////////////// /** \brief Align a pair of PointCloud datasets and return the result * \param cloud_src the source PointCloud * \param cloud_tgt the target PointCloud * \param output the resultant aligned source PointCloud * \param final_transform the resultant transform between source and target *//
//實現匹配，其中參數有輸入一組需要配准的點雲，以及是否需要進行下采樣，其他參數輸出配准后的點雲以及變換矩陣
void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false) { // // Downsample for consistency and speed // \note enable this for large datasets 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); // // Instantiate our custom point representation (defined above) ... MyPointRepresentation point_representation; // ... and weight the 'curvature' dimension so that it is balanced against x, y, and 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); ///設置收斂判斷條件，越小精度越大，收斂也越慢 // Set the maximum distance between two correspondences (src<->tgt) to 10cm大於此值的點對不考慮 // Note: adjust this based on the size of your datasets reg.setMaxCorrespondenceDistance (0.1); // 設置點表示 reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation)); reg.setInputSource (points_with_normals_src); // 設置源點雲 reg.setInputTarget (points_with_normals_tgt); // 設置目標點雲 // // Run the same optimization in a loop and visualize the results 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; // Estimate reg.setInputSource (points_with_normals_src); reg.align (*reg_result); //accumulate transformation between each Iteration Ti = reg.getFinalTransformation () * Ti; //if the difference between this transformation and the previous one //is smaller than the threshold, refine the process by reducing //the maximal correspondence distance if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ()) reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001); prev = reg.getLastIncrementalTransformation (); // visualize current state showCloudsRight(points_with_normals_tgt, points_with_normals_src); } // // Get the transformation from target to source targetToSource = Ti.inverse();//deidao // // Transform target back in source frame 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"); //add the source to the transformed 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); // 加載所有點雲到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"); 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); //用左半窗口創建視口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); //可視化為配准的源和目標點雲 //調用子函數完成一組點雲的配准，temp返回配准后兩組點雲在第一組點雲坐標下的點雲 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 ()); // pairTransform返回從目標點雲target到source的變換矩陣 pairAlign (source, target, temp, pairTransform, true); //把當前兩兩配准后的點雲temp轉化到全局坐標系下返回result pcl::transformPointCloud (*temp, *result, GlobalTransform); //用當前的兩組點雲之間的變換更新全局變換 GlobalTransform = GlobalTransform * pairTransform; //保存轉換到第一個點雲坐標下的當前配准后的兩組點雲result到文件i.pcd std::stringstream ss; ss << i << ".pcd"; pcl::io::savePCDFile (ss.str (), *result, true); } } /* ]--- */
運行執行可執行文件
./pairwise_incremental_registration frame_00000.pcd capture0001.pcd capture0002.pcd capture0004.pcd capture0005.pcd
如果觀察不到結果，就按鍵R來重設攝像頭，調整角度可以觀察到有紅綠兩組點雲顯示在窗口的左邊，紅色為源點雲，將看到上面的類似結果，命令行提示需要執行配准按下Q，按下后可以發現左邊的窗口不斷的調整點雲，其實是配准過程中的迭代中間結果的輸出，在迭代次數小於設定的次數之前，右邊會不斷刷新最新的配准結果，直到收斂，迭代次數30次完成整個匹配的過程，再次按下Q后會看到存儲的1.pcd文件，此文件為第一個和第二個點雲配准后與第一個輸入點雲在同一個坐標系下的點雲。
微信公眾號號可掃描二維碼一起共同學習交流
未完待續************************************8