這次我們將使用Iterative Closest Point algorithm來遞增的注冊一系列的點云。

這個主意來自于把所有的點云轉換成第一個點云的框架，通過找到每個連續點云間最好的裝換，并且計算整個點云的轉換。

你的數據集應該由重新排列的，在一個相同的框架里面重疊的點云構成。

/* * Software License Agreement (BSD License) * * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * $Id$ * *//* \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;//convenient typedefs typedef pcl::PointXYZ PointT; typedef pcl::PointCloud<PointT> PointCloud; typedef pcl::PointNormal PointNormalT; typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;// This is a tutorial so we can afford having global variables //our visualizerpcl::visualization::PCLVisualizer *p;//its left and right viewportsint vp_1, vp_2;//convenient structure to handle our pointclouds 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);} };// Define a new point representation for < x, y, z, curvature > class MyPointRepresentation : public pcl::PointRepresentation <PointNormalT> {using pcl::PointRepresentation<PointNormalT>::nr_dimensions_; public:MyPointRepresentation (){// Define the number of dimensionsnr_dimensions_ = 4;}// Override the copyToFloatArray method to define our feature vectorvirtual 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;} }; /** \brief Display source and target on the first viewport of the visualizer * */ 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(); } /** \brief Display source and target on the second viewport of the visualizer * */ 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");// Suppose the first argument is the actual test modelfor (int i = 1; i < argc; i++){std::string fname = std::string (argv[i]);// Needs to be at least 5: .plotif (fname.size () <= extension.size ())continue;std::transform (fname.begin (), fname.end (), fname.begin (), (int(*)(int))tolower);//check that the argument is a pcd fileif (fname.compare (fname.size () - extension.size (), extension.size (), extension) == 0){// Load the cloud and saves it into the global list of modelsPCD m;m.f_name = argv[i];pcl::io::loadPCDFile (argv[i], *m.cloud);//remove NAN points from the cloudstd::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 datasetsPointCloud::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;}// Compute surface normals and curvaturePointCloudWithNormals::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 zfloat alpha[4] = {1.0, 1.0, 1.0, 1.0};point_representation.setRescaleValues (alpha);//// Alignpcl::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 datasetsreg.setMaxCorrespondenceDistance (0.1); // Set the point representationreg.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 resultsEigen::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);// save cloud for visualization purposepoints_with_normals_src = reg_result;// Estimatereg.setInputSource (points_with_normals_src);reg.align (*reg_result);//accumulate transformation between each IterationTi = 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 distanceif (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);prev = reg.getLastIncrementalTransformation ();// visualize current stateshowCloudsRight(points_with_normals_tgt, points_with_normals_src);}//// Get the transformation from target to sourcetargetToSource = Ti.inverse();//// Transform target back in source framepcl::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) {// Load datastd::vector<PCD, Eigen::aligned_allocator<PCD> > data;loadData (argc, argv, data);// Check user inputif (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 ());// Create a PCLVisualizer objectp = 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;// Add visualization datashowCloudsLeft(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);//transform current pair into the global transformpcl::transformPointCloud (*temp, *result, GlobalTransform);//update the global transformGlobalTransform = GlobalTransform * pairTransform;//save aligned pair, transformed into the first cloud's framestd::stringstream ss;ss << i << ".pcd";pcl::io::savePCDFile (ss.str (), *result, true);} } /* ]--- */

我們這次直接看看核心函數的功能

主函數檢查用戶輸入，加載數據通過矢量的形式并且開啟了一個匹配注冊的過程，在找到一對點云的轉換后，這一對點云將轉換到第一個點云的框架。全局的轉換就會被更新。

int main (int argc, char** argv) {// Load datastd::vector<PCD, Eigen::aligned_allocator<PCD> > data;loadData (argc, argv, data);// Check user inputif (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 ());// Create a PCLVisualizer objectp = 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;// Add visualization datashowCloudsLeft(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);//transform current pair into the global transformpcl::transformPointCloud (*temp, *result, GlobalTransform);//update the global transformGlobalTransform = GlobalTransform * pairTransform;//save aligned pair, transformed into the first cloud's framestd::stringstream ss;ss << i << ".pcd";pcl::io::savePCDFile (ss.str (), *result, true);} } /* ]--- */

加載數據是很簡單的。我們迭代了每個程序的參數。

對于每一個參數，我們檢測是否它鏈接到一個pcd文件。如果是，我們就創造一個PCD對象來添加點云向量。

void loadData (int argc, char **argv, std::vector<PCD, Eigen::aligned_allocator<PCD> > &models) {std::string extension (".pcd");// Suppose the first argument is the actual test modelfor (int i = 1; i < argc; i++){std::string fname = std::string (argv[i]);// Needs to be at least 5: .plotif (fname.size () <= extension.size ())continue;std::transform (fname.begin (), fname.end (), fname.begin (), (int(*)(int))tolower);//check that the argument is a pcd fileif (fname.compare (fname.size () - extension.size (), extension.size (), extension) == 0){// Load the cloud and saves it into the global list of modelsPCD m;m.f_name = argv[i];pcl::io::loadPCDFile (argv[i], *m.cloud);//remove NAN points from the cloudstd::vector<int> indices;pcl::removeNaNFromPointCloud(*m.cloud,*m.cloud, indices);models.push_back (m);}} }

我們現在實際已經到了點的注冊

void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false)

接下去我們進行對點云的降低采樣，然后創建ICP這個對象。

一旦最好的轉換找到了，我們將把它進行反轉并把它放到目標點云里面。

// // Get the transformation from target to source targetToSource = Ti.inverse();// // Transform target back in source frame pcl::transformPointCloud (*cloud_tgt, *output, targetToSource); [...] *output += *cloud_tgt; final_transform = targetToSource;

?

運行

/pairwise_incremental_registration capture000[1-5].pcd

?

結果

運行最終結果

pcl_viewer 1.pcd 2.pcd 3.pcd 4.pcd

總結

以上是生活随笔為你收集整理的怎么样递增的注册成对的点云的全部內容，希望文章能夠幫你解決所遇到的問題。

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