1 #include <vector>
2 #include <string>
3 #include <sstream>
4
5 #include <pcl/io/pcd_io.h>
6 #include <pcl/registration/transforms.h>
7 #include <pcl/visualization/pcl_visualizer.h>
8 #include <pcl/keypoints/sift_keypoint.h>
9
10 #include <pcl/pcl_config.h>
11 #if PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 7
12 # include <pcl/keypoints/harris_3d.h>
13 #else
14 # include <pcl/keypoints/harris_3d.h>
15 #endif
16
17 #include <pcl/memory.h> // for pcl::dynamic_pointer_cast
18 #include <pcl/ModelCoefficients.h>
19 #include <pcl/sample_consensus/method_types.h>
20 #include <pcl/sample_consensus/model_types.h>
21 #include <pcl/segmentation/sac_segmentation.h>
22 #include <pcl/search/kdtree.h>
23 #include <pcl/segmentation/extract_clusters.h>
24 #include <pcl/features/fpfh_omp.h>
25 #include <pcl/features/pfh.h>
26 #include <pcl/features/pfhrgb.h>
27 #include <pcl/features/3dsc.h>
28 #include <pcl/features/shot_omp.h>
29 #include <pcl/filters/extract_indices.h> // for ExtractIndices
30 #include <pcl/kdtree/kdtree_flann.h>
31 #include <pcl/kdtree/impl/kdtree_flann.hpp>
32 #include <pcl/registration/transformation_estimation_svd.h>
33 #include <pcl/registration/icp.h>
34 #include <pcl/registration/correspondence_rejection_sample_consensus.h>
35 #include <pcl/common/transforms.h>
36 #include <pcl/surface/grid_projection.h>
37 #include <pcl/surface/gp3.h>
38 #include <pcl/surface/marching_cubes_hoppe.h>
39
40 template<typename FeatureType>
41 class ICCVTutorial
42 {
43 public:
44 ICCVTutorial (pcl::Keypoint<pcl::PointXYZRGB, pcl::PointXYZI>::Ptr keypoint_detector,
45 typename pcl::Feature<pcl::PointXYZRGB, FeatureType>::Ptr feature_extractor,
46 pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal>::Ptr surface_reconstructor,
47 typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source,
48 typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target);
49
50 /**
51 * @brief starts the event loop for the visualizer
52 */
53 void run ();
54 protected:
55 /**
56 * @brief remove plane and select largest cluster as input object
57 * @param input the input point cloud
58 * @param segmented the resulting segmented point cloud containing only points of the largest cluster
59 */
60 void segmentation (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented) const;
61
62 /**
63 * @brief Detects key points in the input point cloud
64 * @param input the input point cloud
65 * @param keypoints the resulting key points. Note that they are not necessarily a subset of the input cloud
66 */
67 void detectKeypoints (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints) const;
68
69 /**
70 * @brief extract descriptors for given key points
71 * @param input point cloud to be used for descriptor extraction
72 * @param keypoints locations where descriptors are to be extracted
73 * @param features resulting descriptors
74 */
75 void extractDescriptors (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, typename pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints, typename pcl::PointCloud<FeatureType>::Ptr features);
76
77 /**
78 * @brief find corresponding features based on some metric
79 * @param source source feature descriptors
80 * @param target target feature descriptors
81 * @param correspondences indices out of the target descriptors that correspond (nearest neighbor) to the source descriptors
82 */
83 void findCorrespondences (typename pcl::PointCloud<FeatureType>::Ptr source, typename pcl::PointCloud<FeatureType>::Ptr target, std::vector<int>& correspondences) const;
84
85 /**
86 * @brief remove non-consistent correspondences
87 */
88 void filterCorrespondences ();
89
90 /**
91 * @brief calculate the initial rigid transformation from filtered corresponding keypoints
92 */
93 void determineInitialTransformation ();
94
95 /**
96 * @brief calculate the final rigid transformation using ICP over all points
97 */
98 void determineFinalTransformation ();
99
100 /**
101 * @brief reconstructs the surface from merged point clouds
102 */
103 void reconstructSurface ();
104
105 /**
106 * @brief callback to handle keyboard events
107 * @param event object containing information about the event. e.g. type (press, release) etc.
108 * @param cookie user defined data passed during registration of the callback
109 */
110 void keyboard_callback (const pcl::visualization::KeyboardEvent& event, void* cookie);
111
112 private:
113 pcl::visualization::PCLVisualizer visualizer_;
114 pcl::PointCloud<pcl::PointXYZI>::Ptr source_keypoints_;
115 pcl::PointCloud<pcl::PointXYZI>::Ptr target_keypoints_;
116 pcl::Keypoint<pcl::PointXYZRGB, pcl::PointXYZI>::Ptr keypoint_detector_;
117 typename pcl::Feature<pcl::PointXYZRGB, FeatureType>::Ptr feature_extractor_;
118 pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal>::Ptr surface_reconstructor_;
119 typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source_;
120 typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target_;
121 typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_segmented_;
122 typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_segmented_;
123 typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_transformed_;
124 typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_registered_;
125 typename pcl::PolygonMesh surface_;
126 typename pcl::PointCloud<FeatureType>::Ptr source_features_;
127 typename pcl::PointCloud<FeatureType>::Ptr target_features_;
128 std::vector<int> source2target_;
129 std::vector<int> target2source_;
130 pcl::CorrespondencesPtr correspondences_;
131 Eigen::Matrix4f initial_transformation_matrix_;
132 Eigen::Matrix4f transformation_matrix_;
133 bool show_source2target_;
134 bool show_target2source_;
135 bool show_correspondences;
136 };
137
138 template<typename FeatureType>
ICCVTutorial(pcl::Keypoint<pcl::PointXYZRGB,pcl::PointXYZI>::Ptr keypoint_detector,typename pcl::Feature<pcl::PointXYZRGB,FeatureType>::Ptr feature_extractor,pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal>::Ptr surface_reconstructor,typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source,typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target)139 ICCVTutorial<FeatureType>::ICCVTutorial(pcl::Keypoint<pcl::PointXYZRGB, pcl::PointXYZI>::Ptr keypoint_detector,
140 typename pcl::Feature<pcl::PointXYZRGB, FeatureType>::Ptr feature_extractor,
141 pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal>::Ptr surface_reconstructor,
142 typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source,
143 typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target)
144 : source_keypoints_ (new pcl::PointCloud<pcl::PointXYZI> ())
145 , target_keypoints_ (new pcl::PointCloud<pcl::PointXYZI> ())
146 , keypoint_detector_ (keypoint_detector)
147 , feature_extractor_ (feature_extractor)
148 , surface_reconstructor_ (surface_reconstructor)
149 , source_ (source)
150 , target_ (target)
151 , source_segmented_ (new pcl::PointCloud<pcl::PointXYZRGB>)
152 , target_segmented_ (new pcl::PointCloud<pcl::PointXYZRGB>)
153 , source_transformed_ (new pcl::PointCloud<pcl::PointXYZRGB>)
154 , source_registered_ (new pcl::PointCloud<pcl::PointXYZRGB>)
155 , source_features_ (new pcl::PointCloud<FeatureType>)
156 , target_features_ (new pcl::PointCloud<FeatureType>)
157 , correspondences_ (new pcl::Correspondences)
158 , show_source2target_ (false)
159 , show_target2source_ (false)
160 , show_correspondences (false)
161 {
162 visualizer_.registerKeyboardCallback(&ICCVTutorial::keyboard_callback, *this, 0);
163
164 segmentation (source_, source_segmented_);
165 segmentation (target_, target_segmented_);
166
167 detectKeypoints (source_segmented_, source_keypoints_);
168 detectKeypoints (target_segmented_, target_keypoints_);
169
170 extractDescriptors (source_segmented_, source_keypoints_, source_features_);
171 extractDescriptors (target_segmented_, target_keypoints_, target_features_);
172
173 findCorrespondences (source_features_, target_features_, source2target_);
174 findCorrespondences (target_features_, source_features_, target2source_);
175
176 filterCorrespondences ();
177
178 determineInitialTransformation ();
179 determineFinalTransformation ();
180
181 reconstructSurface ();
182 }
183
184 template<typename FeatureType>
segmentation(typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source,typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented) const185 void ICCVTutorial<FeatureType>::segmentation (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source, typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented) const
186 {
187 std::cout << "segmentation..." << std::flush;
188 // fit plane and keep points above that plane
189 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
190 pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
191 // Create the segmentation object
192 pcl::SACSegmentation<pcl::PointXYZRGB> seg;
193 // Optional
194 seg.setOptimizeCoefficients (true);
195 // Mandatory
196 seg.setModelType (pcl::SACMODEL_PLANE);
197 seg.setMethodType (pcl::SAC_RANSAC);
198 seg.setDistanceThreshold (0.02);
199
200 seg.setInputCloud (source);
201 seg.segment (*inliers, *coefficients);
202
203 pcl::ExtractIndices<pcl::PointXYZRGB> extract;
204 extract.setInputCloud (source);
205 extract.setIndices (inliers);
206 extract.setNegative (true);
207
208 extract.filter (*segmented);
209 std::vector<int> indices;
210 pcl::removeNaNFromPointCloud(*segmented, *segmented, indices);
211 std::cout << "OK" << std::endl;
212
213 std::cout << "clustering..." << std::flush;
214 // euclidean clustering
215 typename pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
216 tree->setInputCloud (segmented);
217
218 std::vector<pcl::PointIndices> cluster_indices;
219 pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> clustering;
220 clustering.setClusterTolerance (0.02); // 2cm
221 clustering.setMinClusterSize (1000);
222 clustering.setMaxClusterSize (250000);
223 clustering.setSearchMethod (tree);
224 clustering.setInputCloud(segmented);
225 clustering.extract (cluster_indices);
226
227 if (cluster_indices.size() > 0)//use largest cluster
228 {
229 std::cout << cluster_indices.size() << " clusters found";
230 if (cluster_indices.size() > 1)
231 std::cout <<" Using largest one...";
232 std::cout << std::endl;
233 typename pcl::IndicesPtr indices (new std::vector<int>);
234 *indices = cluster_indices[0].indices;
235 extract.setInputCloud (segmented);
236 extract.setIndices (indices);
237 extract.setNegative (false);
238
239 extract.filter (*segmented);
240 }
241 }
242
243 template<typename FeatureType>
detectKeypoints(typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input,pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints) const244 void ICCVTutorial<FeatureType>::detectKeypoints (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints) const
245 {
246 std::cout << "keypoint detection..." << std::flush;
247 keypoint_detector_->setInputCloud(input);
248 keypoint_detector_->setSearchSurface(input);
249 keypoint_detector_->compute(*keypoints);
250 std::cout << "OK. keypoints found: " << keypoints->size() << std::endl;
251 }
252
253 template<typename FeatureType>
extractDescriptors(typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input,typename pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints,typename pcl::PointCloud<FeatureType>::Ptr features)254 void ICCVTutorial<FeatureType>::extractDescriptors (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, typename pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints, typename pcl::PointCloud<FeatureType>::Ptr features)
255 {
256 typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr kpts(new pcl::PointCloud<pcl::PointXYZRGB>);
257 kpts->points.resize(keypoints->size());
258
259 pcl::copyPointCloud(*keypoints, *kpts);
260
261 typename pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType>::Ptr feature_from_normals = pcl::dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType> > (feature_extractor_);
262
263 feature_extractor_->setSearchSurface(input);
264 feature_extractor_->setInputCloud(kpts);
265
266 if (feature_from_normals)
267 //if (pcl::dynamic_pointer_cast<typename pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType> > (feature_extractor_))
268 {
269 std::cout << "normal estimation..." << std::flush;
270 typename pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
271 pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> normal_estimation;
272 normal_estimation.setSearchMethod (pcl::search::Search<pcl::PointXYZRGB>::Ptr (new pcl::search::KdTree<pcl::PointXYZRGB>));
273 normal_estimation.setRadiusSearch (0.01);
274 normal_estimation.setInputCloud (input);
275 normal_estimation.compute (*normals);
276 feature_from_normals->setInputNormals(normals);
277 std::cout << "OK" << std::endl;
278 }
279
280 std::cout << "descriptor extraction..." << std::flush;
281 feature_extractor_->compute (*features);
282 std::cout << "OK" << std::endl;
283 }
284
285 template<typename FeatureType>
findCorrespondences(typename pcl::PointCloud<FeatureType>::Ptr source,typename pcl::PointCloud<FeatureType>::Ptr target,std::vector<int> & correspondences) const286 void ICCVTutorial<FeatureType>::findCorrespondences (typename pcl::PointCloud<FeatureType>::Ptr source, typename pcl::PointCloud<FeatureType>::Ptr target, std::vector<int>& correspondences) const
287 {
288 std::cout << "correspondence assignment..." << std::flush;
289 correspondences.resize (source->size());
290
291 // Use a KdTree to search for the nearest matches in feature space
292 pcl::KdTreeFLANN<FeatureType> descriptor_kdtree;
293 descriptor_kdtree.setInputCloud (target);
294
295 // Find the index of the best match for each keypoint, and store it in "correspondences_out"
296 const int k = 1;
297 std::vector<int> k_indices (k);
298 std::vector<float> k_squared_distances (k);
299 for (std::size_t i = 0; i < source->size (); ++i)
300 {
301 descriptor_kdtree.nearestKSearch (*source, i, k, k_indices, k_squared_distances);
302 correspondences[i] = k_indices[0];
303 }
304 std::cout << "OK" << std::endl;
305 }
306
307 template<typename FeatureType>
filterCorrespondences()308 void ICCVTutorial<FeatureType>::filterCorrespondences ()
309 {
310 std::cout << "correspondence rejection..." << std::flush;
311 std::vector<std::pair<unsigned, unsigned> > correspondences;
312 for (unsigned cIdx = 0; cIdx < source2target_.size (); ++cIdx)
313 if (static_cast<unsigned int>(target2source_[source2target_[cIdx]]) == cIdx)
314 correspondences.push_back(std::make_pair(cIdx, source2target_[cIdx]));
315
316 correspondences_->resize (correspondences.size());
317 for (unsigned cIdx = 0; cIdx < correspondences.size(); ++cIdx)
318 {
319 (*correspondences_)[cIdx].index_query = correspondences[cIdx].first;
320 (*correspondences_)[cIdx].index_match = correspondences[cIdx].second;
321 }
322
323 pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZI> rejector;
324 rejector.setInputSource(source_keypoints_);
325 rejector.setInputTarget(target_keypoints_);
326 rejector.setInputCorrespondences(correspondences_);
327 rejector.getCorrespondences(*correspondences_);
328 std::cout << "OK" << std::endl;
329 }
330
331 template<typename FeatureType>
determineInitialTransformation()332 void ICCVTutorial<FeatureType>::determineInitialTransformation ()
333 {
334 std::cout << "initial alignment..." << std::flush;
335 pcl::registration::TransformationEstimation<pcl::PointXYZI, pcl::PointXYZI>::Ptr transformation_estimation (new pcl::registration::TransformationEstimationSVD<pcl::PointXYZI, pcl::PointXYZI>);
336
337 transformation_estimation->estimateRigidTransformation (*source_keypoints_, *target_keypoints_, *correspondences_, initial_transformation_matrix_);
338
339 pcl::transformPointCloud(*source_segmented_, *source_transformed_, initial_transformation_matrix_);
340 std::cout << "OK" << std::endl;
341 }
342
343 template<typename FeatureType>
determineFinalTransformation()344 void ICCVTutorial<FeatureType>::determineFinalTransformation ()
345 {
346 std::cout << "final registration..." << std::flush;
347 pcl::Registration<pcl::PointXYZRGB, pcl::PointXYZRGB>::Ptr registration (new pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB>);
348 registration->setInputSource(source_transformed_);
349 //registration->setInputCloud(source_segmented_);
350 registration->setInputTarget (target_segmented_);
351 registration->setMaxCorrespondenceDistance(0.05);
352 registration->setRANSACOutlierRejectionThreshold (0.05);
353 registration->setTransformationEpsilon (0.000001);
354 registration->setMaximumIterations (1000);
355 registration->align(*source_registered_);
356 transformation_matrix_ = registration->getFinalTransformation();
357 std::cout << "OK" << std::endl;
358 }
359
360 template<typename FeatureType>
reconstructSurface()361 void ICCVTutorial<FeatureType>::reconstructSurface ()
362 {
363 std::cout << "surface reconstruction..." << std::flush;
364 // merge the transformed and the target point cloud
365 pcl::PointCloud<pcl::PointXYZRGB>::Ptr merged (new pcl::PointCloud<pcl::PointXYZRGB>);
366 *merged = *source_transformed_;
367 *merged += *target_segmented_;
368
369 // apply grid filtering to reduce amount of points as well as to make them uniform distributed
370 pcl::VoxelGrid<pcl::PointXYZRGB> voxel_grid;
371 voxel_grid.setInputCloud(merged);
372 voxel_grid.setLeafSize(0.002, 0.002, 0.002);
373 voxel_grid.setDownsampleAllData(true);
374 voxel_grid.filter(*merged);
375
376 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr vertices (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
377 pcl::copyPointCloud(*merged, *vertices);
378
379 pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> normal_estimation;
380 normal_estimation.setSearchMethod (pcl::search::Search<pcl::PointXYZRGB>::Ptr (new pcl::search::KdTree<pcl::PointXYZRGB>));
381 normal_estimation.setRadiusSearch (0.01);
382 normal_estimation.setInputCloud (merged);
383 normal_estimation.compute (*vertices);
384
385 pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
386 tree->setInputCloud (vertices);
387
388 surface_reconstructor_->setSearchMethod(tree);
389 surface_reconstructor_->setInputCloud(vertices);
390 surface_reconstructor_->reconstruct(surface_);
391 std::cout << "OK" << std::endl;
392 }
393
394 template<typename FeatureType>
run()395 void ICCVTutorial<FeatureType>::run()
396 {
397 visualizer_.spin ();
398 }
399
400 template<typename FeatureType>
keyboard_callback(const pcl::visualization::KeyboardEvent & event,void *)401 void ICCVTutorial<FeatureType>::keyboard_callback (const pcl::visualization::KeyboardEvent& event, void* /*cookie*/)
402 {
403 if (event.keyUp())
404 {
405 switch (event.getKeyCode())
406 {
407 case '1':
408 if (!visualizer_.removePointCloud("source_points"))
409 {
410 visualizer_.addPointCloud(source_, "source_points");
411 }
412 break;
413
414 case '2':
415 if (!visualizer_.removePointCloud("target_points"))
416 {
417 visualizer_.addPointCloud(target_, "target_points");
418 }
419 break;
420
421 case '3':
422 if (!visualizer_.removePointCloud("source_segmented"))
423 {
424 visualizer_.addPointCloud(source_segmented_, "source_segmented");
425 }
426 break;
427
428 case '4':
429 if (!visualizer_.removePointCloud("target_segmented"))
430 {
431 visualizer_.addPointCloud(target_segmented_, "target_segmented");
432 }
433 break;
434
435 case '5':
436 if (!visualizer_.removePointCloud("source_keypoints"))
437 {
438 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> keypoint_color (source_keypoints_, 0, 0, 255);
439 //pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> keypoint_color (source_keypoints_, "intensity");
440 visualizer_.addPointCloud(source_keypoints_, keypoint_color, "source_keypoints");
441 }
442 break;
443
444 case '6':
445 if (!visualizer_.removePointCloud("target_keypoints"))
446 {
447 //pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> keypoint_color (target_keypoints_, "intensity");
448 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> keypoint_color (target_keypoints_, 255, 0, 0);
449 visualizer_.addPointCloud(target_keypoints_, keypoint_color, "target_keypoints");
450 }
451 break;
452
453 case '7':
454 if (!show_source2target_)
455 visualizer_.addCorrespondences<pcl::PointXYZI>(source_keypoints_, target_keypoints_, source2target_, "source2target");
456 else
457 visualizer_.removeCorrespondences("source2target");
458
459 show_source2target_ = !show_source2target_;
460 break;
461
462 case '8':
463 if (!show_target2source_)
464 visualizer_.addCorrespondences<pcl::PointXYZI>(target_keypoints_, source_keypoints_, target2source_, "target2source");
465 else
466 visualizer_.removeCorrespondences("target2source");
467
468 show_target2source_ = !show_target2source_;
469 break;
470
471 case '9':
472 if (!show_correspondences)
473 visualizer_.addCorrespondences<pcl::PointXYZI>(source_keypoints_, target_keypoints_, *correspondences_, "correspondences");
474 else
475 visualizer_.removeCorrespondences("correspondences");
476 show_correspondences = !show_correspondences;
477 break;
478
479 case 'i':
480 case 'I':
481 if (!visualizer_.removePointCloud("transformed"))
482 visualizer_.addPointCloud(source_transformed_, "transformed");
483 break;
484
485 case 'r':
486 case 'R':
487 if (!visualizer_.removePointCloud("registered"))
488 visualizer_.addPointCloud(source_registered_, "registered");
489 break;
490
491 case 't':
492 case 'T':
493 visualizer_.addPolygonMesh(surface_, "surface");
494 break;
495 }
496 }
497 }
498
499 int
main(int argc,char ** argv)500 main (int argc, char ** argv)
501 {
502 if (argc < 6)
503 {
504 pcl::console::print_info ("Syntax is: %s <source-pcd-file> <target-pcd-file> <keypoint-method> <descriptor-type> <surface-reconstruction-method>\n", argv[0]);
505 pcl::console::print_info ("available <keypoint-methods>: 1 = Sift3D\n");
506 pcl::console::print_info (" 2 = Harris3D\n");
507 pcl::console::print_info (" 3 = Tomasi3D\n");
508 pcl::console::print_info (" 4 = Noble3D\n");
509 pcl::console::print_info (" 5 = Lowe3D\n");
510 pcl::console::print_info (" 6 = Curvature3D\n\n");
511 pcl::console::print_info ("available <descriptor-types>: 1 = FPFH\n");
512 pcl::console::print_info (" 2 = SHOTRGB\n");
513 pcl::console::print_info (" 3 = PFH\n");
514 pcl::console::print_info (" 4 = PFHRGB\n\n");
515 pcl::console::print_info ("available <surface-methods>: 1 = Greedy Projection\n");
516 pcl::console::print_info (" 2 = Marching Cubes\n");
517
518 return (1);
519 }
520 pcl::console::print_info ("== MENU ==\n");
521 pcl::console::print_info ("1 - show/hide source point cloud\n");
522 pcl::console::print_info ("2 - show/hide target point cloud\n");
523 pcl::console::print_info ("3 - show/hide segmented source point cloud\n");
524 pcl::console::print_info ("4 - show/hide segmented target point cloud\n");
525 pcl::console::print_info ("5 - show/hide source key points\n");
526 pcl::console::print_info ("6 - show/hide target key points\n");
527 pcl::console::print_info ("7 - show/hide source2target correspondences\n");
528 pcl::console::print_info ("8 - show/hide target2source correspondences\n");
529 pcl::console::print_info ("9 - show/hide consistent correspondences\n");
530 pcl::console::print_info ("i - show/hide initial alignment\n");
531 pcl::console::print_info ("r - show/hide final registration\n");
532 pcl::console::print_info ("t - show/hide triangulation (surface reconstruction)\n");
533 pcl::console::print_info ("h - show visualizer options\n");
534 pcl::console::print_info ("q - quit\n");
535
536 pcl::PointCloud<pcl::PointXYZRGB>::Ptr source (new pcl::PointCloud<pcl::PointXYZRGB>);
537 pcl::io::loadPCDFile (argv[1], *source);
538
539 pcl::PointCloud<pcl::PointXYZRGB>::Ptr target (new pcl::PointCloud<pcl::PointXYZRGB>);
540 pcl::io::loadPCDFile (argv[2], *target);
541
542 int keypoint_type = atoi (argv[3]);
543 int descriptor_type = atoi (argv[4]);
544 int surface_type = atoi (argv[5]);
545
546 pcl::Keypoint<pcl::PointXYZRGB, pcl::PointXYZI>::Ptr keypoint_detector;
547
548 if (keypoint_type == 1)
549 {
550 pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointXYZI>* sift3D = new pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointXYZI>;
551 sift3D->setScales(0.01, 3, 2);
552 sift3D->setMinimumContrast(0.0);
553 keypoint_detector.reset(sift3D);
554 }
555 else
556 {
557 pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>* harris3D = new pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI> (pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::HARRIS);
558 harris3D->setNonMaxSupression(true);
559 harris3D->setRadius (0.01);
560 harris3D->setRadiusSearch (0.01);
561 keypoint_detector.reset(harris3D);
562 switch (keypoint_type)
563 {
564 case 2:
565 harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::HARRIS);
566 break;
567
568 case 3:
569 harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::TOMASI);
570 break;
571
572 case 4:
573 harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::NOBLE);
574 break;
575
576 case 5:
577 harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::LOWE);
578 break;
579
580 case 6:
581 harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::CURVATURE);
582 break;
583 default:
584 pcl::console::print_error("unknown key point detection method %d\n expecting values between 1 and 6", keypoint_type);
585 exit (1);
586 break;
587 }
588 }
589
590 pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal>::Ptr surface_reconstruction;
591
592 if (surface_type == 1)
593 {
594 pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal>* gp3 = new pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal>;
595
596 // Set the maximum distance between connected points (maximum edge length)
597 gp3->setSearchRadius (0.025);
598
599 // Set typical values for the parameters
600 gp3->setMu (2.5);
601 gp3->setMaximumNearestNeighbors (100);
602 gp3->setMaximumSurfaceAngle(M_PI/4); // 45 degrees
603 gp3->setMinimumAngle(M_PI/18); // 10 degrees
604 gp3->setMaximumAngle(2*M_PI/3); // 120 degrees
605 gp3->setNormalConsistency(false);
606 surface_reconstruction.reset(gp3);
607 }
608 else if (surface_type == 2)
609 {
610 pcl::MarchingCubes<pcl::PointXYZRGBNormal>* mc = new pcl::MarchingCubesHoppe<pcl::PointXYZRGBNormal>;
611 mc->setIsoLevel(0.001);
612 mc->setGridResolution(50, 50, 50);
613 surface_reconstruction.reset(mc);
614 }
615 else
616 {
617 pcl::console::print_error("unknown surface reconstruction method %d\n expecting values between 1 and 2", surface_type);
618 exit (1);
619 }
620
621 switch (descriptor_type)
622 {
623 case 1:
624 {
625 pcl::Feature<pcl::PointXYZRGB, pcl::FPFHSignature33>::Ptr feature_extractor (new pcl::FPFHEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33>);
626 feature_extractor->setSearchMethod (pcl::search::Search<pcl::PointXYZRGB>::Ptr (new pcl::search::KdTree<pcl::PointXYZRGB>));
627 feature_extractor->setRadiusSearch (0.05);
628 ICCVTutorial<pcl::FPFHSignature33> tutorial (keypoint_detector, feature_extractor, surface_reconstruction, source, target);
629 tutorial.run ();
630 }
631 break;
632
633 case 2:
634 {
635 pcl::SHOTColorEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::SHOT1344>* shot = new pcl::SHOTColorEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::SHOT1344>;
636 shot->setRadiusSearch (0.04);
637 pcl::Feature<pcl::PointXYZRGB, pcl::SHOT1344>::Ptr feature_extractor (shot);
638 ICCVTutorial<pcl::SHOT1344> tutorial (keypoint_detector, feature_extractor, surface_reconstruction, source, target);
639 tutorial.run ();
640 }
641 break;
642
643 case 3:
644 {
645 pcl::Feature<pcl::PointXYZRGB, pcl::PFHSignature125>::Ptr feature_extractor (new pcl::PFHEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::PFHSignature125>);
646 feature_extractor->setKSearch(50);
647 ICCVTutorial<pcl::PFHSignature125> tutorial (keypoint_detector, feature_extractor, surface_reconstruction, source, target);
648 tutorial.run ();
649 }
650 break;
651
652 case 4:
653 {
654 pcl::Feature<pcl::PointXYZRGB, pcl::PFHRGBSignature250>::Ptr feature_extractor (new pcl::PFHRGBEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::PFHRGBSignature250>);
655 feature_extractor->setKSearch(50);
656 ICCVTutorial<pcl::PFHRGBSignature250> tutorial (keypoint_detector, feature_extractor, surface_reconstruction, source, target);
657 tutorial.run ();
658 }
659 break;
660
661 default:
662 pcl::console::print_error("unknown descriptor type %d\n expecting values between 1 and 4", descriptor_type);
663 exit (1);
664 break;
665 }
666
667 return (0);
668 }
669