PCL点云获取+拼接(2)

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

使用方法：

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

2.ICP算法，精确配准

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

正视图

俯视图

代码：

#include <iostream>



#include <vector>



#include <Eigen/Core>



#include <pcl/registration/icp.h>



#include <pcl/point_types.h>



#include <pcl/point_cloud.h>



#include <pcl/io/pcd_io.h>



#include <pcl/filters/statistical_outlier_removal.h>



#include <pcl/kdtree/kdtree_flann.h>



#include <pcl/filters/voxel_grid.h>



#include <pcl/features/normal_3d.h>



#include <pcl/features/fpfh.h>



#include <pcl/registration/ia_ransac.h>



#include <pcl/visualization/cloud_viewer.h>



 




using namespace std;




using namespace pcl;



 



class FeatureCloud



{



  public:



    // A bit of shorthand




    typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;



    typedef pcl::PointCloud<pcl::Normal> SurfaceNormals;



    typedef pcl::PointCloud<pcl::FPFHSignature33> LocalFeatures;



    typedef pcl::search::KdTree<pcl::PointXYZ> 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<pcl::PointXYZ> vox_grid;



       vox_grid.setInputCloud (xyz_);



      vox_grid.setLeafSize (voxel_grid_size, voxel_grid_size, voxel_grid_size);



       vox_grid.filter (*gridsample);



 



     pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;



        sor.setInputCloud(gridsample);



      sor.setMeanK(50);



      sor.setStddevMulThresh(1.0);



       sor.filter(*tempCloud);



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



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



   }



 



    // Compute the surface normals




    void computeSurfaceNormals ()




    {



      normals_ = SurfaceNormals::Ptr (new SurfaceNormals);



 



      pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> 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<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> 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<pcl::PointXYZ> 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<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> 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<PointXYZ>::ConstPtr source_cloud)




    {



     icp.setInputCloud(source_cloud);



    }



 



    void setTargetCloud (PointCloud<PointXYZ>::ConstPtr target_cloud)




    {



       icp.setInputTarget(target_cloud);



    }



    



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




    void align (PointCloud<PointXYZ> &temp)




    {



      



      pcl::PointCloud<pcl::PointXYZ> 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<PointXYZ, PointXYZ> icp;



    Eigen::Matrix4f final_transformation;



  float fitness_score;



};



 



int main (int argc, char **argv)



{



   int begin = 0;



  int end = 2;



    std::vector<FeatureCloud> object_templates;



    std::stringstream ss;



    TemplateAlignment my_alignment;



    MyICP my_icp;



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



 PointCloud<PointXYZRGB>::Ptr result (new PointCloud<PointXYZRGB>);



   PointCloud<PointXYZRGB>::Ptr my_cloud (new PointCloud<PointXYZRGB>);



 PointCloud<PointXYZRGB>::Ptr Color_in (new PointCloud<PointXYZRGB>);



    PointCloud<PointXYZRGB>::Ptr Color_out (new PointCloud<PointXYZRGB>);



 PointCloud<PointXYZRGB> Final_Color;



 PointCloud<PointXYZ>::Ptr temp (new PointCloud<PointXYZ>);



   PointCloud<PointXYZ> temp2;



  



   ss.str(“”);



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



   if(io::loadPCDFile<PointXYZRGB>(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<<i<<endl;



       //cout<<”first size:”<<object_templates[i-1].getPointCloud()->size()<<”, second size:”<<object_templates[i].getPointCloud()->size()<<endl;




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



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



        my_alignment.align();



      cout<<“sac_ia fitness score:”<<my_alignment.getScore()<<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:”<<my_icp.getScore()<<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_”<<i<<“.pcd”;



       if(pcl::io::loadPCDFile<pcl::PointXYZRGB>(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|

            </div>