#如何递增配准成对点云
本文档演示了使用Iterative Closest Point算法为了递增地逐个配准一系列点云。
这个想法是转换第一个云框架中的所有云。
这是通过在每个连续云之间找到最佳转换来完成的，并在整个云集上累积这些转换。
您的数据集应包含已经在公共框架（例如在机器人的测距或地图框架中）中大致预先对齐的云并且彼此重叠的云。
我们在
github.com/PointCloudLibrary/data/tree/master/tutorials/pairwise/
提供一组云。
#代码
/*
* 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 (clou