點雲配準，然後將配準後的點雲保存爲pcd文件

兩個點雲進行配準，然後將配準後的點雲保存爲pcd文件，本文是在《點雲庫PCL從入門到精通》基礎上修改的。
pairwise_incremental_registration.cpp如下所示：

#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>
//定義PCL點雲的類型
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
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)
  {
    std::cout<<"3 "<<std::endl;
    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;
  }
  //輸出其大小，並計算點雲法線
  std::cout<<"src size "<<src->points.size()<<std::endl;
  std::cout<<"tgt size "<<tgt->points.size()<<std::endl;
  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);//設置估計時進行搜索用的k數；
  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);
  pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;//配準對象
  reg.setTransformationEpsilon (1e-6);//設置收斂判斷條件，越小精度越大，收斂也越慢；
  //將兩個點雲中對應點對之間的(src<->tgt)最大距離設置爲10釐米，大於此值的不考慮
  reg.setMaxCorrespondenceDistance (0.1);  
  //設置源點雲和目標點雲
  reg.setInputSource (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 (30);
    reg.align (*reg_result);
    std::cout<<"reg_result size "<<reg_result->points.size()<<std::endl;
    Ti = reg.getFinalTransformation () ;//* Ti;
  targetToSource = Ti.inverse();
  pcl::transformPointCloud (*cloud_tgt, *output, targetToSource);
  *output = *output + *cloud_src;
 }
int main (int argc, char** argv)
{
    PointCloud::Ptr result (new PointCloud), source (new PointCloud), target (new PointCloud);
    if(argc>3){
        std::cout<<"error argment "<<std::endl;
    } // 加載需要進行點雲配準的源點雲和目標點雲。
      if (pcl::io::loadPCDFile<pcl::PointXYZ> ("capture0001.pcd", *source) == -1)
      {
        PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
        return (-1);
      }
      if (pcl::io::loadPCDFile<pcl::PointXYZ> ("capture0002.pcd", *target) == -1)
      {
        PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
        return (-1);
      }
           //ŽÓµãÔÆÖÐÒƳýNANµã
        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*source,*source, indices);
        pcl::removeNaNFromPointCloud(*target,*target, indices);
        Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform;
        PointCloud::Ptr temp (new PointCloud);
//    PCL_INFO ("Aligning %s (%d) with %s (%d).\n", data[i-1].), source->points.size (), data[i].f_name.c_str (), target->points.size ());
    std::cout<<"save cloud 1"<<std::endl;
    //調用子函數完成一組點雲的配準，temp返回配準後兩組點雲在第//一組點雲座標系下的點雲，pairTranse返回從目標點雲到源點雲的變//換矩陣。
    pairAlign (source, target, temp, pairTransform, false);
    std::cout<<"13 "<<std::endl;
    //把當前兩兩配對後的點雲temp轉換到全局座標系下，返回result
    pcl::transformPointCloud (*temp, *result, GlobalTransform);
    //使用當前兩組點雲之間的變換更新全局變換
    GlobalTransform = pairTransform * GlobalTransform;
    std::cout<<"save cloud "<<std::endl;
    //逐個點打印，不建議使用
    /*for(size_t i=0;i<result->points.size();++i)
        std::cout<<"    "<<result->points[i].x<<" "<<result->points[i].y<<result->points[i].z<<std::endl;
    */pcl::io::savePCDFileASCII ("test_pcd.pcd", *result);
     std::cout<<"reg_result size "<<result->points.size()<<std::endl;//兩個點雲都轉換到第一個點雲座標系下的總的點雲的個數
    pcl::PCDWriter writer;
    writer.write<pcl::PointXYZ>("2.pcd", *result, false);//保存轉換到第一個點雲座標系下當前配準後的兩組點雲result到文件2.pcd中
}

CMakeLists.txt文件如下所示：

cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
project(tuto-pairwise)
find_package(PCL 1.4 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})		
add_definitions(${PCL_DEFINITIONS})
add_definitions(-Wno-deprecated -DEIGEN_DONT_VECTORIZE -DEIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT)
add_executable (pairwise_incremental_registration pairwise_incremental_registration.cpp)
target_link_libraries (pairwise_incremental_registration ${PCL_LIBRARIES})

系統Ubuntu16.04，基於ROS kinetic平臺下，編譯過程

mkdir build
cd build
cmake ..
make
./pairwise_incremental_registration

pcd測試文件如有需要，可在下面留下郵箱，我會發
如有什麼問題，還請各位多多指教