Point Cloud Library (PCL)
1.3.1
|
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_ */