源代源码链接：https://github.com/PointCloudLibrary/pcl/blob/master/apps/src/feature_matching.cpp
#include <vector>
#include <string>
#include <sstream>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/keypoints/sift_keypoint.h>
#include <pcl/keypoints/harris_3d.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/features/pfh.h>
#include <pcl/features/pfhrgb.h>
#include <pcl/features/3dsc.h>
#include <pcl/features/shot_omp.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/common/transforms.h>
#include <pcl/surface/grid_projection.h>
#include <pcl/surface/gp3.h>
#include <pcl/surface/marching_cubes_hoppe.h>
template<typename FeatureType>
class ICCVTutorial
{
public:
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,
pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source,
pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target);
/**
* @brief starts the event loop for the visualizer
*/
void run ();
protected:
/**
* @brief remove plane and select largest cluster as input object
* @param input the input point cloud
* @param segmented the resulting segmented point cloud containing only points of the largest cluster
*/
void segmentation (const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& input, const typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr& segmented) const;
/**
* @brief Detects key points in the input point cloud
* @param input the input point cloud
* @param keypoints the resulting key points. Note that they are not necessarily a subset of the input cloud
*/
void detectKeypoints (const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& input, const pcl::PointCloud<pcl::PointXYZI>::Ptr& keypoints) const;
/**
* @brief extract descriptors for given key points
* @param input point cloud to be used for descriptor extraction
* @param keypoints locations where descriptors are to be extracted
* @param features resulting descriptors
*/
void extractDescriptors (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, const typename pcl::PointCloud<pcl::PointXYZI>::Ptr& keypoints, typename pcl::PointCloud<FeatureType>::Ptr features);
/**
* @brief find corresponding features based on some metric
* @param source source feature descriptors
* @param target target feature descriptors
* @param correspondences indices out of the target descriptors that correspond (nearest neighbor) to the source descriptors
*/
void findCorrespondences (typename pcl::PointCloud<FeatureType>::Ptr source, typename pcl::PointCloud<FeatureType>::Ptr target, std::vector<int>& correspondences) const;
/**
* @brief remove non-consistent correspondences
*/
void filterCorrespondences ();
/**
* @brief calculate the initial rigid transformation from filtered corresponding keypoints
*/
void determineInitialTransformation ();
/**
* @brief calculate the final rigid transformation using ICP over all points
*/
void determineFinalTransformation ();
/**
* @brief reconstructs the surface from merged point clouds
*/
void reconstructSurface ();
/**
* @brief callback to handle keyboard events
* @param event object containing information about the event. e.g. type (press, release) etc.
* @param cookie user defined data passed during registration of the callback
*/
void keyboard_callback (const pcl::visualization::KeyboardEvent& event, void* cookie);
private:
pcl::visualization::PCLVisualizer visualizer_;
pcl::PointCloud<pcl::PointXYZI>::Ptr source_keypoints_;
pcl::PointCloud<pcl::PointXYZI>::Ptr target_keypoints_;
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_;
typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_segmented_;
typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_segmented_;
typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_transformed_;
typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_registered_;
typename pcl::PolygonMesh surface_;
typename pcl::PointCloud<FeatureType>::Ptr source_features_;
typename pcl::PointCloud<FeatureType>::Ptr target_features_;
std::vector<int> source2target_;
std::vector<int> target2source_;
pcl::CorrespondencesPtr correspondences_;
Eigen::Matrix4f initial_transformation_matrix_;
Eigen::Matrix4f transformation_matrix_;
bool show_source2target_;
bool show_target2source_;
bool show_correspondences;
};
template<typename FeatureType>
ICCVTutorial<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,
pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source,
pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target)
: source_keypoints_ (new pcl::PointCloud<pcl::PointXYZI> ())
, target_keypoints_ (new pcl::PointCloud<pcl::PointXYZI> ())
, keypoint_detector_ (std::move(keypoint_detector))
, feature_extractor_ (feature_extractor)
, surface_reconstructor_ (std::move(surface_reconstructor))
, source_ (std::move(source))
, target_ (std::move(target))
, source_segmented_ (new pcl::PointCloud<pcl::PointXYZRGB>)
, target_segmented_ (new pcl::PointCloud<pcl::PointXYZRGB>)
, source_transformed_ (new pcl::PointCloud<pcl::PointXYZRGB>)
, source_registered_ (new pcl::PointCloud<pcl::PointXYZRGB>)
, source_features_ (new pcl::PointCloud<FeatureType>)
, target_features_ (new pcl::PointCloud<FeatureType>)
, correspondences_ (new pcl::Correspondences)
, show_source2target_ (false)
, show_target2source_ (false)
, show_correspondences (false)
{
visualizer_.registerKeyboardCallback(&ICCVTutorial::keyboard_callback, *this, nullptr);
segmentation (source_, source_segmented_);
segmentation (target_, target_segmented_);
detectKeypoints (source_segmented_, source_keypoints_);
detectKeypoints (target_segmented_, target_keypoints_);
extractDescriptors (source_segmented_, source_keypoints_, source_features_);
extractDescriptors (target_segmented_, target_keypoints_, target_features_);
findCorrespondences (source_features_, target_features_, source2target_);
findCorrespondences (target_features_, source_features_, target2source_);
filterCorrespondences ();
determineInitialTransformation ();
determineFinalTransformation ();
reconstructSurface ();
}
template<typename FeatureType>
void ICCVTutorial<FeatureType>::segmentation (const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& source, const typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr& segmented) const
{
std::cout << "segmentation..." << std::flush;
// fit plane and keep points above that plane
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.02);
seg.setInputCloud (source);
seg.segment (*inliers, *coefficients);
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud (source);
extract.setIndices (inliers);
extract.setNegative (true);
extract.filter (*segmented);
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*segmented, *segmented, indices);
std::cout << "OK" << std::endl;
std::cout << "clustering..." << std::flush;
// euclidean clustering
typename pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (segmented);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> clustering;
clustering.setClusterTolerance (0.02); // 2cm
clustering.setMinClusterSize (1000);
clustering.setMaxClusterSize (250000);
clustering.setSearchMethod (tree);
clustering.setInputCloud(segmented);
clustering.extract (cluster_indices);
if (!cluster_indices.empty ())//use largest cluster
{
std::cout << cluster_indices.size() << " clusters found";
if (cluster_indices.size() > 1)
std::cout <<" Using largest one...";
std::cout << std::endl;
typename pcl::IndicesPtr indices (new std::vector<int>);
*indices = cluster_indices[0].indices;
extract.setInputCloud (segmented);
extract.setIndices (indices);
extract.setNegative (false);
extract.filter (*segmented);
}
}
template<typename FeatureType>
void ICCVTutorial<FeatureType>::detectKeypoints (const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& input, const pcl::PointCloud<pcl::PointXYZI>::Ptr& keypoints) const
{
std::cout << "keypoint detection..." << std::flush;
keypoint_detector_->setInputCloud(input);
keypoint_detector_->compute(*keypoints);
std::cout << "OK. keypoints found: " << keypoints->points.size() << std::endl;
}
template<typename FeatureType>
void ICCVTutorial<FeatureType>::extractDescriptors (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, const typename pcl::PointCloud<pcl::PointXYZI>::Ptr& keypoints, typename pcl::PointCloud<FeatureType>::Ptr features)
{
typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr kpts(new pcl::PointCloud<pcl::PointXYZRGB>);
kpts->points.resize(keypoints->points.size());
pcl::copyPointCloud(*keypoints, *kpts);
typename pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType>::Ptr feature_from_normals = boost::dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType> > (feature_extractor_);
feature_extractor_->setSearchSurface(input);
feature_extractor_->setInputCloud(kpts);
if (feature_from_normals)
//if (boost::dynamic_pointer_cast<typename pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType> > (feature_extractor_))
{
std::cout << "normal estimation..." << std::flush;
typename pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> normal_estimation;
normal_estimation.setSearchMethod (pcl::search::Search<pcl::PointXYZRGB>::Ptr (new pcl::search::KdTree<pcl::PointXYZRGB>));
normal_estimation.setRadiusSearch (0.01);
normal_estimation.setInputCloud (input);
normal_estimation.compute (*normals);
feature_from_normals->setInputNormals(normals);
std::cout << "OK" << std::endl;
}
std::cout << "descriptor extraction..." << std::flush;
feature_extractor_->compute (*features);
std::cout << "OK" << std::endl;
}
template<typename FeatureType>
void ICCVTutorial<FeatureType>::findCorrespondences (typename pcl::PointCloud<FeatureType>::Ptr source, typename pcl::PointCloud<FeatureType>::Ptr target, std::vector<int>& correspondences) const
{
std::cout << "correspondence assignment..." << std::flush;
correspondences.resize (source->size());
// Use a KdTree to search for the nearest matches in feature space
pcl::KdTreeFLANN<FeatureType> descriptor_kdtree;
descriptor_kdtree.setInputCloud (target);
// Find the index of the best match for each keypoint, and store it in "correspondences_out"
const int k = 1;
std::vector<int> k_indices (k);
std::vector<float> k_squared_distances (k);
for (int i = 0; i < static_cast<int> (source->size ()); ++i)
{
descriptor_kdtree.nearestKSearch (*source, i, k, k_indices, k_squared_distances);
correspondences[i] = k_indices[0];
}
std::cout << "OK" << std::endl;
}
template<typename FeatureType>
void ICCVTutorial<FeatureType>::filterCorrespondences ()
{
std::cout << "correspondence rejection..." << std::flush;
std::vector<std::pair<unsigned, unsigned> > correspondences;
for (std::size_t cIdx = 0; cIdx < source2target_.size (); ++cIdx)
if (target2source_[source2target_[cIdx]] == static_cast<int> (cIdx))
correspondences.push_back(std::make_pair(cIdx, source2target_[cIdx]));
correspondences_->resize (correspondences.size());
for (std::size_t cIdx = 0; cIdx < correspondences.size(); ++cIdx)
{
(*correspondences_)[cIdx].index_query = correspondences[cIdx].first;
(*correspondences_)[cIdx].index_match = correspondences[cIdx].second;
}
pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZI> rejector;
rejector.setInputSource (source_keypoints_);
rejector.setInputTarget (target_keypoints_);
rejector.setInputCorrespondences(correspondences_);
rejector.getCorrespondences(*correspondences_);
std::cout << "OK" << std::endl;
}
template<typename FeatureType>
void ICCVTutorial<FeatureType>::determineInitialTransformation ()
{
std::cout << "initial alignment..." << std::flush;
pcl::registration::TransformationEstimation<pcl::PointXYZI, pcl::PointXYZI>::Ptr transformation_estimation (new pcl::registration::TransformationEstimationSVD<pcl::PointXYZI, pcl::PointXYZI>);
transformation_estimation->estimateRigidTransformation (*source_keypoints_, *target_keypoints_, *correspondences_, initial_transformation_matrix_);
pcl::transformPointCloud(*source_segmented_, *source_transformed_, initial_transformation_matrix_);
std::cout << "OK" << std::endl;
}
template<typename FeatureType>
void ICCVTutorial<FeatureType>::determineFinalTransformation ()
{
std::cout << "final registration..." << std::flush;
pcl::Registration<pcl::PointXYZRGB, pcl::PointXYZRGB>::Ptr registration (new pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB>);
registration->setInputSource (source_transformed_);
//registration->setInputSource (source_segmented_);
registration->setInputTarget (target_segmented_);
registration->setMaxCorrespondenceDistance(0.05);
registration->setRANSACOutlierRejectionThreshold (0.05);
registration->setTransformationEpsilon (0.000001);
registration->setMaximumIterations (1000);
registration->align(*source_registered_);
transformation_matrix_ = registration->getFinalTransformation();
std::cout << "OK" << std::endl;
}
template<typename FeatureType>
void ICCVTutorial<FeatureType>::reconstructSurface ()
{
std::cout << "surface reconstruction..." << std::flush;
// merge the transformed and the target point cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr merged (new pcl::PointCloud<pcl::PointXYZRGB>);
*merged = *source_registered_;
*merged += *target_segmented_;
// apply grid filtering to reduce amount of points as well as to make them uniform distributed
pcl::VoxelGrid<pcl::PointXYZRGB> voxel_grid;
voxel_grid.setInputCloud(merged);
voxel_grid.setLeafSize (0.002f, 0.002f, 0.002f);
voxel_grid.setDownsampleAllData(true);
voxel_grid.filter(*merged);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr vertices (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::copyPointCloud(*merged, *vertices);
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> normal_estimation;
normal_estimation.setSearchMethod (pcl::search::Search<pcl::PointXYZRGB>::Ptr (new pcl::search::KdTree<pcl::PointXYZRGB>));
normal_estimation.setRadiusSearch (0.01);
normal_estimation.setInputCloud (merged);
normal_estimation.compute (*vertices);
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
tree->setInputCloud (vertices);
surface_reconstructor_->setSearchMethod(tree);
surface_reconstructor_->setInputCloud(vertices);
surface_reconstructor_->reconstruct(surface_);
std::cout << "OK" << std::endl;
}
template<typename FeatureType>
void ICCVTutorial<FeatureType>::run()
{
visualizer_.spin ();
}
template<typename FeatureType>
void ICCVTutorial<FeatureType>::keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
{
if (event.keyUp())
{
switch (event.getKeyCode())
{
case '1':
if (!visualizer_.removePointCloud("source_points"))
{
visualizer_.addPointCloud(source_, "source_points");
}
break;
case '2':
if (!visualizer_.removePointCloud("target_points"))
{
visualizer_.addPointCloud(target_, "target_points");
}
break;
case '3':
if (!visualizer_.removePointCloud("source_segmented"))
{
visualizer_.addPointCloud(source_segmented_, "source_segmented");
}
break;
case '4':
if (!visualizer_.removePointCloud("target_segmented"))
{
visualizer_.addPointCloud(target_segmented_, "target_segmented");
}
break;
case '5':
if (!visualizer_.removePointCloud("source_keypoints"))
{
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> keypoint_color (source_keypoints_, 0, 0, 255);
//pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> keypoint_color (source_keypoints_, "intensity");
visualizer_.addPointCloud(source_keypoints_, keypoint_color, "source_keypoints");
}
break;
case '6':
if (!visualizer_.removePointCloud("target_keypoints"))
{
//pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> keypoint_color (target_keypoints_, "intensity");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> keypoint_color (target_keypoints_, 255, 0, 0);
visualizer_.addPointCloud(target_keypoints_, keypoint_color, "target_keypoints");
}
break;
case '7':
if (!show_source2target_)
visualizer_.addCorrespondences<pcl::PointXYZI>(source_keypoints_, target_keypoints_, source2target_, "source2target");
else
visualizer_.removeCorrespondences("source2target");
show_source2target_ = !show_source2target_;
break;
case '8':
if (!show_target2source_)
visualizer_.addCorrespondences<pcl::PointXYZI>(target_keypoints_, source_keypoints_, target2source_, "target2source");
else
visualizer_.removeCorrespondences("target2source");
show_target2source_ = !show_target2source_;
break;
case '9':
if (!show_correspondences)
visualizer_.addCorrespondences<pcl::PointXYZI>(source_keypoints_, target_keypoints_, *correspondences_, "correspondences");
else
visualizer_.removeCorrespondences("correspondences");
show_correspondences = !show_correspondences;
break;
case 'i':
case 'I':
if (!visualizer_.removePointCloud("transformed"))
visualizer_.addPointCloud(source_transformed_, "transformed");
break;
case 'r':
case 'R':
if (!visualizer_.removePointCloud("registered"))
visualizer_.addPointCloud(source_registered_, "registered");
break;
case 't':
case 'T':
visualizer_.addPolygonMesh(surface_, "surface");
break;
}
}
}
int
main (int argc, char ** argv)
{
if (argc < 6)
{
pcl::console::print_info ("Syntax is: %s <source-pcd-file> <target-pcd-file> <keypoint-method> <descriptor-type> <surface-reconstruction-method>\n", argv[0]);
pcl::console::print_info ("available <keypoint-methods>: 1 = Sift3D\n");
pcl::console::print_info (" 2 = Harris3D\n");
pcl::console::print_info (" 3 = Tomasi3D\n");
pcl::console::print_info (" 4 = Noble3D\n");
pcl::console::print_info (" 5 = Lowe3D\n");
pcl::console::print_info (" 6 = Curvature3D\n\n");
pcl::console::print_info ("available <descriptor-types>: 1 = FPFH\n");
pcl::console::print_info (" 2 = SHOTRGB\n");
pcl::console::print_info (" 3 = PFH\n");
pcl::console::print_info (" 4 = PFHRGB\n\n");
pcl::console::print_info ("available <surface-methods>: 1 = Greedy Projection\n");
pcl::console::print_info (" 2 = Marching Cubes\n");
return (1);
}
pcl::console::print_info ("== MENU ==\n");
pcl::console::print_info ("1 - show/hide source point cloud\n");
pcl::console::print_info ("2 - show/hide target point cloud\n");
pcl::console::print_info ("3 - show/hide segmented source point cloud\n");
pcl::console::print_info ("4 - show/hide segmented target point cloud\n");
pcl::console::print_info ("5 - show/hide source key points\n");
pcl::console::print_info ("6 - show/hide target key points\n");
pcl::console::print_info ("7 - show/hide source2target correspondences\n");
pcl::console::print_info ("8 - show/hide target2source correspondences\n");
pcl::console::print_info ("9 - show/hide consistent correspondences\n");
pcl::console::print_info ("i - show/hide initial alignment\n");
pcl::console::print_info ("r - show/hide final registration\n");
pcl::console::print_info ("t - show/hide triangulation (surface reconstruction)\n");
pcl::console::print_info ("h - show visualizer options\n");
pcl::console::print_info ("q - quit\n");
pcl::PointCloud<pcl::PointXYZRGB>::Ptr source (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::io::loadPCDFile (argv[1], *source);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr target (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::io::loadPCDFile (argv[2], *target);
int keypoint_type = atoi (argv[3]);
int descriptor_type = atoi (argv[4]);
int surface_type = atoi (argv[5]);
pcl::Keypoint<pcl::PointXYZRGB, pcl::PointXYZI>::Ptr keypoint_detector;
if (keypoint_type == 1)
{
pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointXYZI>* sift3D = new pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointXYZI>;
sift3D->setScales (0.01f, 3, 2);
sift3D->setMinimumContrast (0.0);
keypoint_detector.reset (sift3D);
}
else
{
pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>* harris3D = new pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI> (pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::HARRIS);
harris3D->setNonMaxSupression (true);
harris3D->setRadius (0.01f);
harris3D->setRadiusSearch (0.01f);
keypoint_detector.reset (harris3D);
switch (keypoint_type)
{
case 2:
harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::HARRIS);
break;
case 3:
harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::TOMASI);
break;
case 4:
harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::NOBLE);
break;
case 5:
harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::LOWE);
break;
case 6:
harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::CURVATURE);
break;
default:
pcl::console::print_error("unknown key point detection method %d\n expecting values between 1 and 6", keypoint_type);
exit (1);
break;
}
}
pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal>::Ptr surface_reconstruction;
if (surface_type == 1)
{
pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal>* gp3 = new pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal>;
// Set the maximum distance between connected points (maximum edge length)
gp3->setSearchRadius (0.025);
// Set typical values for the parameters
gp3->setMu (2.5);
gp3->setMaximumNearestNeighbors (100);
gp3->setMaximumSurfaceAngle(M_PI/4); // 45 degrees
gp3->setMinimumAngle(M_PI/18); // 10 degrees
gp3->setMaximumAngle(2*M_PI/3); // 120 degrees
gp3->setNormalConsistency(false);
surface_reconstruction.reset(gp3);
}
else if (surface_type == 2)
{
pcl::MarchingCubes<pcl::PointXYZRGBNormal>* mc = new pcl::MarchingCubesHoppe<pcl::PointXYZRGBNormal>;
mc->setIsoLevel (0.001f);
mc->setGridResolution (50, 50, 50);
surface_reconstruction.reset(mc);
}
else
{
pcl::console::print_error("unknown surface reconstruction method %d\n expecting values between 1 and 2", surface_type);
exit (1);
}
switch (descriptor_type)
{
case 1:
{
pcl::Feature<pcl::PointXYZRGB, pcl::FPFHSignature33>::Ptr feature_extractor (new pcl::FPFHEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33>);
feature_extractor->setSearchMethod (pcl::search::Search<pcl::PointXYZRGB>::Ptr (new pcl::search::KdTree<pcl::PointXYZRGB>));
feature_extractor->setRadiusSearch (0.05);
ICCVTutorial<pcl::FPFHSignature33> tutorial (keypoint_detector, feature_extractor, surface_reconstruction, source, target);
tutorial.run ();
}
break;
case 2:
{
pcl::SHOTColorEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::SHOT1344>* shot = new pcl::SHOTColorEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::SHOT1344>;
shot->setRadiusSearch (0.04);
pcl::Feature<pcl::PointXYZRGB, pcl::SHOT1344>::Ptr feature_extractor (shot);
ICCVTutorial<pcl::SHOT1344> tutorial (keypoint_detector, feature_extractor, surface_reconstruction, source, target);
tutorial.run ();
}
break;
case 3:
{
pcl::Feature<pcl::PointXYZRGB, pcl::PFHSignature125>::Ptr feature_extractor (new pcl::PFHEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::PFHSignature125>);
feature_extractor->setKSearch(50);
ICCVTutorial<pcl::PFHSignature125> tutorial (keypoint_detector, feature_extractor, surface_reconstruction, source, target);
tutorial.run ();
}
break;
case 4:
{
pcl::Feature<pcl::PointXYZRGB, pcl::PFHRGBSignature250>::Ptr feature_extractor (new pcl::PFHRGBEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::PFHRGBSignature250>);
feature_extractor->setKSearch(50);
ICCVTutorial<pcl::PFHRGBSignature250> tutorial (keypoint_detector, feature_extractor, surface_reconstruction, source, target);
tutorial.run ();
}
break;
default:
pcl::console::print_error("unknown descriptor type %d\n expecting values between 1 and 4", descriptor_type);
exit (1);
break;
}
return (0);
}