PCL点云获取+拼接(2)

本次博文介绍固定点拼接点云，kinect拍摄两幅画面，二者之间旋转10度，运行环境vs2012 pcl1.7.2

使用方法：

1.采样一致性初始配准算法SAC-IA，粗略估计初始变换矩阵

2.ICP算法，精确配准

原始点云（拼接前，隔着10度）

正视图

俯视图

代码：

     
     
     
     

      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         #include 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         #include 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         #include 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         #include 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         #include 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         #include 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         #include 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         #include 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         #include 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         #include 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         #include 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         #include 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         #include 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         #include 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         using 
         
         
         
         namespace 
         
         
         
         std;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         using 
         
         
         
         namespace pcl;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         class FeatureCloud
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
  
         
         
         
         public:
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         // A bit of shorthand
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         typedef pcl::PointCloud PointCloud;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         typedef pcl::PointCloud SurfaceNormals;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         typedef pcl::PointCloud LocalFeatures;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         typedef pcl::search::KdTree SearchMethod;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             FeatureCloud () :
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               search_method_xyz_ (
         
         
         
         new SearchMethod),
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               normal_radius_ (
         
         
         
         0.5f),
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               feature_radius_ (
         
         
         
         0.5f),
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               voxel_grid_size (
         
         
         
         0.07f)
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
        
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             ~FeatureCloud () {}
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         // Process the given cloud
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         void setInputCloud (PointCloud::Ptr xyz)
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               xyz_ = xyz;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               processInput ();
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         // Load and process the cloud in the given PCD file
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         void loadInputCloud (const std::string &pcd_file)
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               xyz_ = PointCloud::Ptr (
         
         
         
         new PointCloud);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               pcl::io::loadPCDFile (pcd_file, *xyz_);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               processInput ();
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         // Get a pointer to the cloud 3D points
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             PointCloud::
         
         
         
         Ptr getPointCloud () const
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         return (tempCloud);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         // Get a pointer to the cloud of 3D surface normals
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             SurfaceNormals::
         
         
         
         Ptr getSurfaceNormals () const
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         return (normals_);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         // Get a pointer to the cloud of feature descriptors
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             LocalFeatures::
         
         
         
         Ptr getLocalFeatures () const
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         return (features_);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
  
         
         
         
         protected:
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         // Compute the surface normals and local features
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         void processInput ()
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               mysample();
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               computeSurfaceNormals ();
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               computeLocalFeatures ();
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         void mysample()
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
            {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                gridsample = PointCloud::Ptr (
         
         
         
         new PointCloud);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                 tempCloud = PointCloud::Ptr (
         
         
         
         new PointCloud);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
              pcl::VoxelGrid vox_grid;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                vox_grid.setInputCloud (xyz_);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               vox_grid.setLeafSize (voxel_grid_size, voxel_grid_size, voxel_grid_size);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                vox_grid.filter (*gridsample);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
              pcl::StatisticalOutlierRemoval sor;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                 sor.setInputCloud(gridsample);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               sor.setMeanK(
         
         
         
         50);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               sor.setStddevMulThresh(
         
         
         
         1.0);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                sor.filter(*tempCloud);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
     
         
         
         
         cout<<
         
         
         
         “cloud size before filtering:”<size()<<
         
         
         
         endl;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
       
         
         
         
         cout<<
         
         
         
         “cloud size after filtering:”<size()<<
         
         
         
         endl;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
            }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         // Compute the surface normals
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         void computeSurfaceNormals ()
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               normals_ = SurfaceNormals::Ptr (
         
         
         
         new SurfaceNormals);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               pcl::NormalEstimation norm_est;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               norm_est.setInputCloud (tempCloud);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               norm_est.setSearchMethod (search_method_xyz_);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               norm_est.setRadiusSearch (normal_radius_);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               norm_est.compute (*normals_);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         // Compute the local feature descriptors
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         void computeLocalFeatures ()
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               features_ = LocalFeatures::Ptr (
         
         
         
         new LocalFeatures);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               pcl::FPFHEstimation fpfh_est;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               fpfh_est.setInputCloud (tempCloud);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               fpfh_est.setInputNormals (normals_);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               fpfh_est.setSearchMethod (search_method_xyz_);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               fpfh_est.setRadiusSearch (feature_radius_);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               fpfh_est.compute (*features_);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
  
         
         
         
         private:
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         // Point cloud data
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             PointCloud::Ptr xyz_;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             PointCloud::Ptr gridsample;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             PointCloud::Ptr tempCloud;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             SurfaceNormals::Ptr normals_;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             LocalFeatures::Ptr features_;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             SearchMethod::Ptr search_method_xyz_;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         // Parameters
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         float normal_radius_;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         float feature_radius_;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         float voxel_grid_size;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         };
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         class TemplateAlignment
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
  
         
         
         
         public:
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             TemplateAlignment () :
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               min_sample_distance_ (
         
         
         
         0.01f),
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               max_correspondence_distance_ (
         
         
         
         0.01f*
         
         
         
         0.01f),
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               nr_iterations_ (
         
         
         
         300)
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         // Intialize the parameters in the Sample Consensus Intial Alignment (SAC-IA) algorithm
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               sac_ia_.setMinSampleDistance (min_sample_distance_);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               sac_ia_.setMaximumIterations (nr_iterations_);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             ~TemplateAlignment () {}
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
         
         
         
         void setSourceCloud(FeatureCloud &source_cloud)
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                 sac_ia_.setInputCloud (source_cloud.getPointCloud ());
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                 sac_ia_.setSourceFeatures (source_cloud.getLocalFeatures ());
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         void setTargetCloud (FeatureCloud &target_cloud)
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               sac_ia_.setInputTarget (target_cloud.getPointCloud ());
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ());
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         // Align the given template cloud to the target specified by setTargetCloud ()
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         void align ()
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               pcl::PointCloud registration_output;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               sac_ia_.align (registration_output);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               fitness_score = (
         
         
         
         float) sac_ia_.getFitnessScore (max_correspondence_distance_);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               final_transformation = sac_ia_.getFinalTransformation ();
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
          Eigen::
         
         
         
         Matrix4f getMatrix()
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         return final_transformation;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
          }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         float getScore()
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
          {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         return fitness_score;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
  
         
         
         
         private:
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         // The Sample Consensus Initial Alignment (SAC-IA) registration routine and its parameters
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             pcl::SampleConsensusInitialAlignment sac_ia_;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         float min_sample_distance_;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         float fitness_score;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         float max_correspondence_distance_;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
            Eigen::Matrix4f final_transformation;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         int nr_iterations_;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         };
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         class MyICP
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
   
         
         
         
         public:
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             MyICP ()
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
       
         
         
         
         // Intialize the parameters in the ICP algorithm
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                icp.setMaxCorrespondenceDistance(
         
         
         
         0.01);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                icp.setTransformationEpsilon(
         
         
         
         1e-7);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                icp.setEuclideanFitnessEpsilon(
         
         
         
         1);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                icp.setMaximumIterations(
         
         
         
         100);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             ~MyICP () {}
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
         
         
         
         void setSourceCloud(PointCloud::ConstPtr source_cloud)
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
              icp.setInputCloud(source_cloud);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         void setTargetCloud (PointCloud::ConstPtr target_cloud)
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                icp.setInputTarget(target_cloud);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
   
         
         
         
         // Align the given template cloud to the target specified by setTargetCloud ()
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         void align (PointCloud &temp)
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               pcl::PointCloud registration_output;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               icp.align (temp);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               fitness_score =  icp.getFitnessScore();
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             final_transformation = icp.getFinalTransformation ();
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         float getScore()
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
          {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         return fitness_score;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             Eigen::
         
         
         
         Matrix4f getMatrix()
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         return final_transformation;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
          }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
  
         
         
         
         private:
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             IterativeClosestPoint icp;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             Eigen::Matrix4f final_transformation;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
  
         
         
         
         float fitness_score;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         };
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         int main (int argc, char **argv)
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
   
         
         
         
         int begin = 
         
         
         
         0;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
  
         
         
         
         int end = 
         
         
         
         2;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         std::
         
         
         
         vector object_templates;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         std::
         
         
         
         stringstream ss;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             TemplateAlignment my_alignment;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             MyICP my_icp;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
           Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform, pairTransform2;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
          PointCloud::
         
         
         
         Ptr result (new PointCloud);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
            PointCloud::
         
         
         
         Ptr my_cloud (new PointCloud);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
          PointCloud::
         
         
         
         Ptr Color_in (new PointCloud);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             PointCloud::
         
         
         
         Ptr Color_out (new PointCloud);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
          PointCloud Final_Color;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
          PointCloud::
         
         
         
         Ptr temp (new PointCloud);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
            PointCloud temp2;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
  
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
            ss.str(
         
         
         
         “”);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
            ss<<
         
         
         
         “color_”<“.pcd”;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
   
         
         
         
         if(io::loadPCDFile(ss.str(),*Color_in)==
         
         
         
         -1)
         
         
         
         //*打开点云文件
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             PCL_ERROR(
         
         
         
         “Couldn’t read file test_pcd.pcd\n”);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
       
         
         
         
         return(
         
         
         
         -1);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         //load data
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         for(
         
         
         
         int j = begin;j < end;j++)
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
           {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         std::
         
         
         
         stringstream ss;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                 ss << j << 
         
         
         
         “.pcd”;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                 FeatureCloud template_cloud;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                template_cloud.loadInputCloud (ss.str());
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               object_templates.push_back (template_cloud);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
            }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             Final_Color = *Color_in;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
   
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
   
         
         
         
         for (
         
         
         
         size_t i = begin + 
         
         
         
         1; i < begin + object_templates.size (); ++i)
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         cout<endl;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
       
         
         
         
         //cout<<”first size:”<size()<<”, second size:”<size()<
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                my_alignment.setTargetCloud(object_templates[i
         
         
         
         -1-begin]);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
              my_alignment.setSourceCloud(object_templates[i-begin]);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                 my_alignment.align();
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         cout<<
         
         
         
         “sac_ia fitness score:”<endl;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
     
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
       
         
         
         
         //update the global transform
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                 pairTransform = my_alignment.getMatrix();
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         //print matrix
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
       
         
         
         
         printf (
         
         
         
         “\n”);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
     
         
         
         
         printf (
         
         
         
         ”    | %6.3f %6.3f %6.3f %6.3f| \n”, pairTransform (
         
         
         
         0,
         
         
         
         0), pairTransform (
         
         
         
         0,
         
         
         
         1), pairTransform (
         
         
         
         0,
         
         
         
         2), pairTransform (
         
         
         
         0,
         
         
         
         3));
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         printf (
         
         
         
         “R = | %6.3f %6.3f %6.3f %6.3f| \n”, pairTransform (
         
         
         
         1,
         
         
         
         0), pairTransform (
         
         
         
         1,
         
         
         
         1), pairTransform (
         
         
         
         1,
         
         
         
         2), pairTransform (
         
         
         
         1,
         
         
         
         3));
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         printf (
         
         
         
         ”    | %6.3f %6.3f %6.3f %6.3f| \n”, pairTransform (
         
         
         
         2,
         
         
         
         0), pairTransform (
         
         
         
         2,
         
         
         
         1), pairTransform (
         
         
         
         2,
         
         
         
         2), pairTransform (
         
         
         
         2,
         
         
         
         3));
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         printf (
         
         
         
         ”    | %6.3f %6.3f %6.3f %6.3f| \n”, pairTransform (
         
         
         
         3,
         
         
         
         0), pairTransform (
         
         
         
         3,
         
         
         
         1), pairTransform (
         
         
         
         3,
         
         
         
         2), pairTransform (
         
         
         
         3,
         
         
         
         3));
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               GlobalTransform = GlobalTransform * pairTransform;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
     
         
         
         
         //GlobalTransform =  pairTransform;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
       
         
         
         
         //transform current pair into the global transform
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                pcl::transformPointCloud (*object_templates[i-begin].getPointCloud(), *temp, GlobalTransform);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                my_icp.setSourceCloud(temp);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                my_icp.setTargetCloud(object_templates[i
         
         
         
         -1-begin].getPointCloud());
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                my_icp.align(temp2);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
       
         
         
         
         cout<<
         
         
         
         “icp fitness score:”<endl;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               pairTransform2 = my_icp.getMatrix();
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
       
         
         
         
         printf (
         
         
         
         “\n”);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
     
         
         
         
         printf (
         
         
         
         ”    | %6.3f %6.3f %6.3f %6.3f| \n”, pairTransform2 (
         
         
         
         0,
         
         
         
         0), pairTransform2 (
         
         
         
         0,
         
         
         
         1), pairTransform2 (
         
         
         
         0,
         
         
         
         2), pairTransform2 (
         
         
         
         0,
         
         
         
         3));
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         printf (
         
         
         
         “R = | %6.3f %6.3f %6.3f %6.3f| \n”, pairTransform2 (
         
         
         
         1,
         
         
         
         0), pairTransform2 (
         
         
         
         1,
         
         
         
         1), pairTransform2 (
         
         
         
         1,
         
         
         
         2), pairTransform2 (
         
         
         
         1,
         
         
         
         3));
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         printf (
         
         
         
         ”    | %6.3f %6.3f %6.3f %6.3f| \n”, pairTransform2 (
         
         
         
         2,
         
         
         
         0), pairTransform2 (
         
         
         
         2,
         
         
         
         1), pairTransform2 (
         
         
         
         2,
         
         
         
         2), pairTransform2 (
         
         
         
         2,
         
         
         
         3));
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         printf (
         
         
         
         ”    | %6.3f %6.3f %6.3f %6.3f| \n”, pairTransform2 (
         
         
         
         3,
         
         
         
         0), pairTransform2 (
         
         
         
         3,
         
         
         
         1), pairTransform2 (
         
         
         
         3,
         
         
         
         2), pairTransform2 (
         
         
         
         3,
         
         
         
         3));
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               GlobalTransform = GlobalTransform * pairTransform2;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               ss.str(
         
         
         
         “”);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                ss<<
         
         
         
         “color_”<“.pcd”;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
       
         
         
         
         if(pcl::io::loadPCDFile(ss.str(),*Color_out)==
         
         
         
         -1)
         
         
         
         //*打开点云彩色文件
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                  PCL_ERROR(
         
         
         
         “Couldn’t read file test_pcd.pcd\n”);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
           
         
         
         
         return(
         
         
         
         -1);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
      
         
         
         
         //transform current pair into the global transform
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                pcl::transformPointCloud (*Color_out, *result, GlobalTransform);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                Final_Color = Final_Color + *result;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
   
         
         
         
         //构造拼接临时的点云
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         for(
         
         
         
         int i=
         
         
         
         0;i< Final_Color.points.size();i++)
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               pcl::PointXYZRGB basic_point;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
               basic_point.x = Final_Color.points[i].x;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                basic_point.y = Final_Color.points[i].y;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                basic_point.z = Final_Color.points[i].z;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                basic_point.r = Final_Color.points[i].r;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                basic_point.g = Final_Color.points[i].g;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                basic_point.b = Final_Color.points[i].b;
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
                my_cloud->points.push_back(basic_point);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             pcl::visualization::
         
         
         
         CloudViewer viewer(”My Cloud Viewer”);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             viewer.showCloud(my_cloud);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         while(!viewer.wasStopped())
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             {
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             }
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
    
         
         
         
         return (
         
         
         
         0);
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         }
        
        
        
        
       
       
       
       
     
     
     
     
     
     
     
     

     
     
     
     

结果如下，角度不见了~~

别高兴太早，这套算法如果这么牛逼，我也不用这么蛋疼了。如果用他拼接360度，必定失败，如果有用这个方法能搞定连续多幅图片拼接的朋友，请私信我。

下面是我用NDT方法，连续拼接90度的结果，只能这样了。。。

     
     
     
     

      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         Filtered cloud contains 
         
         
         
         540
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         ndt fitness score:
         
         
         
         0.0227071
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.985  
         
         
         
         0.007 
         
         
         
         -0.174  
         
         
         
         0.003|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         R = | 
         
         
         
         -0.007  
         
         
         
         1.000  
         
         
         
         0.002  
         
         
         
         0.000|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.174 
         
         
         
         -0.000  
         
         
         
         0.985 
         
         
         
         -0.006|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         1.000|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         Filtered cloud contains 
         
         
         
         420
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         ndt fitness score:
         
         
         
         0.0343324
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.989  
         
         
         
         0.040 
         
         
         
         -0.146  
         
         
         
         0.005|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         R = | 
         
         
         
         -0.037  
         
         
         
         0.999  
         
         
         
         0.021 
         
         
         
         -0.005|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.146 
         
         
         
         -0.015  
         
         
         
         0.989 
         
         
         
         -0.005|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         1.000|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         Filtered cloud contains 
         
         
         
         552
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         ndt fitness score:
         
         
         
         0.0802134
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.968 
         
         
         
         -0.016 
         
         
         
         -0.249  
         
         
         
         0.152|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         R = |  
         
         
         
         0.021  
         
         
         
         1.000  
         
         
         
         0.016 
         
         
         
         -0.014|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.248 
         
         
         
         -0.020  
         
         
         
         0.969 
         
         
         
         -0.012|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         1.000|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         Filtered cloud contains 
         
         
         
         926
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         ndt fitness score:
         
         
         
         0.0198928
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.978 
         
         
         
         -0.015 
         
         
         
         -0.210  
         
         
         
         0.148|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         R = |  
         
         
         
         0.019  
         
         
         
         1.000  
         
         
         
         0.017 
         
         
         
         -0.024|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.209 
         
         
         
         -0.020  
         
         
         
         0.978  
         
         
         
         0.016|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         1.000|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         Filtered cloud contains 
         
         
         
         575
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         ndt fitness score:
         
         
         
         0.0492542
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.962 
         
         
         
         -0.007 
         
         
         
         -0.273  
         
         
         
         0.085|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         R = |  
         
         
         
         0.006  
         
         
         
         1.000 
         
         
         
         -0.001 
         
         
         
         -0.002|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.273 
         
         
         
         -0.000  
         
         
         
         0.962 
         
         
         
         -0.009|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         1.000|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         Filtered cloud contains 
         
         
         
         412
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         ndt fitness score:
         
         
         
         0.00171811
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.992 
         
         
         
         -0.024 
         
         
         
         -0.127  
         
         
         
         0.001|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         R = |  
         
         
         
         0.023  
         
         
         
         1.000 
         
         
         
         -0.007 
         
         
         
         -0.000|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.127  
         
         
         
         0.004  
         
         
         
         0.992  
         
         
         
         0.003|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         1.000|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         Filtered cloud contains 
         
         
         
         295
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         ndt fitness score:
         
         
         
         0.00152303
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.983 
         
         
         
         -0.001 
         
         
         
         -0.182  
         
         
         
         0.086|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         R = |  
         
         
         
         0.003  
         
         
         
         1.000  
         
         
         
         0.010  
         
         
         
         0.038|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.182 
         
         
         
         -0.011  
         
         
         
         0.983  
         
         
         
         0.090|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         1.000|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         Filtered cloud contains 
         
         
         
         191
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         ndt fitness score:
         
         
         
         0.023204
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.975 
         
         
         
         -0.080 
         
         
         
         -0.208  
         
         
         
         0.121|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         R = |  
         
         
         
         0.092  
         
         
         
         0.995  
         
         
         
         0.047 
         
         
         
         -0.142|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.203 
         
         
         
         -0.065  
         
         
         
         0.977  
         
         
         
         0.103|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         1.000|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         Filtered cloud contains 
         
         
         
         133
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         ndt fitness score:
         
         
         
         0.00556793
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        
 
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.983  
         
         
         
         0.003 
         
         
         
         -0.184  
         
         
         
         0.008|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
         R = | 
         
         
         
         -0.004  
         
         
         
         1.000 
         
         
         
         -0.002  
         
         
         
         0.000|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.184  
         
         
         
         0.003  
         
         
         
         0.983  
         
         
         
         0.011|
        
        
        
        
       
       
       
       
      
      
      
      

       
       
       
       

        
        
        
        

       
       
       
       
       
       
       
       

        
        
        
        

         
         
         
             |  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         0.000  
         
         
         
         1.000|