Point Cloud Library (PCL)  1.3.1
incremental_registration.h
Go to the documentation of this file.
00001 #ifndef PCL_REGISTRATION_INCREMENTAL_REGISTRATION_H_
00002 #define PCL_REGISTRATION_INCREMENTAL_REGISTRATION_H_
00003 
00004 #include <pcl/pcl_base.h>
00005 #include <pcl/filters/voxel_grid.h>
00006 #include <pcl/registration/stanford_gicp.h>
00007 
00008 #include <pcl/registration/correspondence_estimation.h>
00009 
00010 #include <pcl/registration/correspondence_rejection_distance.h>
00011 #include <pcl/registration/correspondence_rejection_trimmed.h>
00012 #include <pcl/registration/correspondence_rejection_one_to_one.h>
00013 #include <pcl/registration/correspondence_rejection_reciprocal.h>
00014 #include <pcl/registration/correspondence_rejection_sample_consensus.h>
00015 
00016 #include <pcl/registration/transformation_estimation_svd.h>
00017 
00018 namespace pcl
00019 {
00020   namespace registration
00021   {
00022 
00023     template <typename PointT>
00024     class IncrementalRegistration : public PCLBase<PointT>
00025     {
00026       using PCLBase<PointT>::initCompute;
00027       using PCLBase<PointT>::deinitCompute;
00028 
00029     public:
00030 
00031       using PCLBase<PointT>::indices_;
00032       using PCLBase<PointT>::input_;
00033 
00034       typedef pcl::PointCloud<PointT> PointCloud;
00035       typedef typename PointCloud::Ptr PointCloudPtr;
00036       typedef typename PointCloud::ConstPtr PointCloudConstPtr;
00037 
00038       IncrementalRegistration()
00039       {
00040         transform_incremental_ = Eigen::Matrix4f::Identity();
00041 
00042         downsampling_leaf_size_input_ = 0.05;
00043         downsampling_leaf_size_model_ = 0.05;
00044         registration_distance_threshold_ = 1.0;
00045 
00046         downsample_input_cloud = true;
00047         downsample_model_cloud = true;
00048 
00049         number_clouds_processed_ = 0;
00050       };
00051 
00052       virtual ~IncrementalRegistration(){};
00053 
00054       inline void
00055       align(PointCloud &output, bool use_vanilla_ICP = false)
00056       {
00057 
00059         if ( !initCompute() )
00060           return;
00061 
00062 
00064         if ( downsample_input_cloud )
00065         {
00066           sor_.setInputCloud(input_);
00067           sor_.setIndices(indices_);
00068           sor_.setLeafSize(downsampling_leaf_size_input_, downsampling_leaf_size_input_, downsampling_leaf_size_input_);
00069           sor_.filter(cloud_input_);
00070           cloud_input_ptr_ = cloud_input_.makeShared();
00071         }
00072         else
00073         {
00074           cloud_input_ = *input_;
00075           cloud_input_ptr_ = cloud_input_.makeShared();
00076 //          cloud_input_ptr_ = input_;
00077         }
00078 
00080         if (number_clouds_processed_ == 0)
00081         {
00082           // no registration needed
00083           output = cloud_input_;
00084           cloud_model_ = cloud_input_;
00085           cloud_model_ptr_ = cloud_model_.makeShared();
00086           transform_incremental_.setIdentity();
00087         }
00088         else
00089         {
00090           pcl::transformPointCloud(cloud_input_, cloud_input_, transform_incremental_);
00091 
00092           Eigen::Matrix4f transform_current;
00093           transform_current.setIdentity();
00094 
00095           bool registration_successful = true;
00096 
00097           if ( !use_vanilla_ICP )
00098           {
00100             reg_.setInputCloud(cloud_input_ptr_);
00101             reg_.setInputTarget(cloud_model_ptr_);
00102             reg_.setMaxCorrespondenceDistance(registration_distance_threshold_);
00103             reg_.setMaximumIterations(25);
00104             reg_.align(output);
00105 
00106             transform_current = reg_.getFinalTransformation();
00107 
00108             registration_successful = true;
00110           }
00111           else
00112           {
00114             output = cloud_input_;
00115             unsigned int max_iterations = 30, n_iter = 0, n_iter_linear = max_iterations - 5;
00116 
00117             registration_successful = true;
00118             while ( ( n_iter++ < max_iterations ) && registration_successful )
00119             {
00120               float max_dist = registration_distance_threshold_;
00121 
00122               bool use_linear_distance_threshold = true;
00123               if ( use_linear_distance_threshold )
00124               {
00125                 float dist_start = registration_distance_threshold_;
00126                 float dist_stop = 1.5f * downsampling_leaf_size_model_;
00127                 if ( n_iter < n_iter_linear )
00128                   max_dist = dist_start - n_iter * (dist_start - dist_stop) / (float)(max_iterations);
00129                 else
00130                   max_dist = dist_stop;
00131               }
00132 
00133               // determine correspondences
00134               PointCloudConstPtr cloud_output_ptr = output.makeShared();
00135 
00136               Correspondences correspondences;
00137               corr_est_.setInputTarget(cloud_model_ptr_);
00138               corr_est_.setInputCloud(cloud_output_ptr);
00139               corr_est_.determineCorrespondences(correspondences, max_dist);
00140 
00141               // remove one-to-n correspondences
00142               Correspondences correspondeces_one_to_one;
00143               cor_rej_one_to_one_.getCorrespondences(correspondences, correspondeces_one_to_one);
00144 
00145               // SAC-based correspondence rejection
00146               double sac_threshold = max_dist;
00147               int sac_max_iterations = 100;
00148               pcl::Correspondences correspondences_sac;
00149               cor_rej_sac_.setInputCloud(cloud_output_ptr);
00150               cor_rej_sac_.setTargetCloud(cloud_model_ptr_);
00151               cor_rej_sac_.setInlierThreshold(sac_threshold);
00152               cor_rej_sac_.setMaxIterations(sac_max_iterations);
00153               cor_rej_sac_.getCorrespondences(correspondeces_one_to_one, correspondences_sac);
00154 
00155               unsigned int nr_min_correspondences = 10;
00156               if ( correspondences_sac.size() < nr_min_correspondences )
00157               {
00158                 registration_successful = false;
00159                 break;
00160               }
00161 
00162               Eigen::Matrix4f transform_svd;
00163               trans_est_.estimateRigidTransformation(output, cloud_model_, correspondences_sac, transform_svd);
00164 
00165               pcl::transformPointCloud(output, output, transform_svd);
00166 
00167               transform_current = transform_svd * transform_current;
00168             }
00169 
00170           }
00171 
00172           if ( registration_successful )
00173           {
00174             transform_incremental_ = transform_current * transform_incremental_;
00175 
00176             // setting up new model
00177             addCloudToModel(output);
00178             if ( downsample_model_cloud )
00179               subsampleModel();
00180           }
00181 
00182         }
00183 
00184         ++number_clouds_processed_;
00185 
00187         deinitCompute ();
00188       }
00189 
00191       inline PointCloud* getModel() { return &cloud_model_; };
00192 
00194       inline void reset() { number_clouds_processed_ = 0; };
00195 
00197       inline Eigen::Matrix4f getTransformation() { return transform_incremental_; };
00198       
00199 
00200 
00202       inline void setDownsamplingLeafSizeInput(double leaf_size) { downsampling_leaf_size_input_ = leaf_size; };
00204       inline double getDownsamplingLeafSizeInput() { return downsampling_leaf_size_input_; };
00205 
00207       inline void setDownsamplingLeafSizeModel(double leaf_size) { downsampling_leaf_size_model_ = leaf_size; };
00209       inline void getDownsamplingLeafSizeModel() { return downsampling_leaf_size_model_; };
00210 
00212       inline void setRegistrationDistanceThreshold(double threshold) { registration_distance_threshold_ = threshold; };
00214       inline void getRegistrationDistanceThreshold() { return registration_distance_threshold_; };
00215 
00217       inline void downsampleInputCloud(bool enable) { downsample_input_cloud = enable; };
00219       inline void downsampleModelCloud(bool enable) { downsample_model_cloud = enable; };
00220 
00221     protected:
00222 
00223       pcl::GeneralizedIterativeClosestPoint<PointT, PointT> reg_;
00224       pcl::VoxelGrid<PointT> sor_;
00225       PointCloud cloud_input_, cloud_model_;
00226       PointCloudConstPtr cloud_input_ptr_, cloud_model_ptr_;
00227 
00228       Eigen::Matrix4f transform_incremental_;
00229 
00230       inline void addCloudToModel(PointCloud& cloud)
00231       {
00232         cloud_model_ += cloud;
00233         cloud_model_ptr_ = cloud_model_.makeShared();
00234       }
00235 
00236       inline void subsampleModel()
00237       {
00238         PointCloud cloud_temp;
00239         sor_.setInputCloud(cloud_model_ptr_);
00240         sor_.setLeafSize(downsampling_leaf_size_model_, downsampling_leaf_size_model_, downsampling_leaf_size_model_);
00241         sor_.filter(cloud_temp);
00242         cloud_model_ = cloud_temp;
00243         cloud_model_ptr_ = cloud_model_.makeShared();
00244       }
00245 
00246       bool downsample_input_cloud;
00247       bool downsample_model_cloud;
00248       double downsampling_leaf_size_input_;
00249       double downsampling_leaf_size_model_;
00250       double registration_distance_threshold_;
00251 
00252       unsigned int number_clouds_processed_;
00253 
00254       pcl::registration::CorrespondenceEstimation<PointT, PointT> corr_est_;
00255       pcl::registration::CorrespondenceRejectorOneToOne cor_rej_one_to_one_;
00256       pcl::registration::CorrespondenceRejectorSampleConsensus<PointT> cor_rej_sac_;
00257       pcl::registration::TransformationEstimationSVD<PointT, PointT> trans_est_;
00258     };
00259   }
00260 }
00261 
00262 #endif /* PCL_REGISTRATION_INCREMENTAL_REGISTRATION_H_ */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines