PCLCorespondenceGrouping⽅法进⾏3D模板匹配/*
* 此例程使⽤pcl_recognition 组件讲解 3D⽬标的识别。特别指出，例程主要讲解Corespondence Grouping算法
* 通过点对点的⽐对相似性进⾏点阵的聚类来识别场景中的物体。每⼀个输出点阵，都是识别的物体的实例，另外
* 算法还输出⼀个6DOF的转换矩阵来表⽰⽬标在场景中的位姿信息
*
*
*/
#include<pcl/io/pcd_io.h>
#include<pcl/point_cloud.h>
#include<pcl/correspondence.h>
#include<pcl/features/normal_3d_omp.h>
#include<pcl/features/shot_omp.h>
#include<pcl/features/board.h>
#include<pcl/filters/uniform_sampling.h>
#include<pcl/recognition/cg/hough_3d.h>
#include<pcl/recognition/cg/geometric_consistency.h>
#include<pcl/visualization/pcl_visualizer.h>
#include<pcl/kdtree/kdtree_flann.h>
#include<pcl/kdtree/impl/kdtree_flann.hpp>
#include<pcl/common/transforms.h>
#include<pcl/console/par.h>
typedef pcl::PointXYZRGBA PointType;
typedef pcl::Normal NormalType;
typedef pcl::ReferenceFrame RFType;
typedef pcl::SHOT352 DescriptorType;
std::string model_filename_;
std::string scene_filename_;
//Algorithm params
bool show_keypoints_ (fal);
bool show_correspondences_(fal);
bool u_cloud_resolution_(fal);
bool u_hough_ (true);gat
float model_ss_ (0.01f);
float scene_ss_ (0.03f);
float rf_rad_ (0.015f);
float descr_rad_ (0.02f);
float cg_size_ (0.01f);
float cg_thresh_ (5.0f);
void
showHelp(char* filename)
{
std::cout << std::endl;
std::cout <<"***************************************************************************"<< std::endl;
std::cout <<"* *"<< std::endl;
std::cout <<"* Correspondence Grouping Tutorial - Usage Guide *"<< std::endl;
std::cout <<"* *"<< std::endl;
std::cout <<"***************************************************************************"<< std::endl << std::endl;
std::cout <<"Usage: "<< filename <<" model_filename.pcd scene_filename.pcd [Options]"<< std::endl << std::endl;
std::cout <<"Options:"<< std::endl;
std::cout <<" -h: Show this help."<< std::endl;
std::cout <<" -k: Show ud keypoints."<< std::endl;
std::cout <<" -c: Show ud correspondences."<< std::endl;
std::cout <<" -r: Compute the model cloud resolution and multiply"<< std::endl;
std::cout <<" each radius given by that value."<< std::endl;
std::cout <<" --algorithm (Hough|GC): Clustering algorithm ud (default Hough)."<< std::endl;
std::cout <<" --model_ss val: Model uniform sampling radius (default 0.01)"<< std::endl;
std::cout <<" --scene_ss val: Scene uniform sampling radius (default 0.03)"<< std::endl;
std::cout <<" --rf_rad val: Reference frame radius (default 0.015)"<< std::endl;
std::cout <<" --rf_rad val: Reference frame radius (default 0.015)"<< std::endl;
std::cout <<" --descr_rad val: Descriptor radius (default 0.02)"<< std::endl;
std::cout <<" --cg_size val: Cluster size (default 0.01)"<< std::endl;
std::cout <<" --cg_thresh val: Clustering threshold (default 5)"<< std::endl << std::endl; }
void
parCommandLine(int argc,char* argv[])
{
//Show help
if(pcl::console::find_switch(argc, argv,"-h"))
{
免费翻译网showHelp(argv[0]);
exit(0);
}
//Model & scene filenames
std::vector<int> filenames;
filenames = pcl::console::par_file_extension_argument(argc, argv,".pcd");
if(filenames.size()!=2)
{
std::cout <<"Filenames missing.\n";
showHelp(argv[0]);
exit(-1);
}
model_filename_ = argv[filenames[0]];
scene_filename_ = argv[filenames[1]];
//Program behavior
if(pcl::console::find_switch(argc, argv,"-k"))
{
show_keypoints_ =true;
}
if(pcl::console::find_switch(argc, argv,"-c"))
{
show_correspondences_ =true;
}
if(pcl::console::find_switch(argc, argv,"-r"))
{
u_cloud_resolution_ =true;
}
std::string ud_algorithm;
if(pcl::console::par_argument(argc, argv,"--algorithm", ud_algorithm)!=-1)
{
if(pare("Hough")==0)
{
u_hough_ =true;
}
el if(pare("GC")==0)
{
u_hough_ =fal;
}
el
{
std::cout <<"Wrong algorithm name.\n";
showHelp(argv[0]);
exit(-1);
}
}
//General parameters
pcl::console::par_argument(argc, argv,"--model_ss", model_ss_);
christmas song
pcl::console::par_argument(argc, argv,"--scene_ss", scene_ss_);
pcl::console::par_argument(argc, argv,"--rf_rad", rf_rad_);
pcl::console::par_argument(argc, argv,"--rf_rad", rf_rad_);
pcl::console::par_argument(argc, argv,"--descr_rad", descr_rad_);
pcl::console::par_argument(argc, argv,"--cg_size", cg_size_);one way or another
pcl::console::par_argument(argc, argv,"--cg_thresh", cg_thresh_);
}
/// <summary>
/// 计算点云的空间平均分辨率
/// </summary>
/// <param name="cloud"></param>
/// <returns></returns>
double
computeCloudResolution(const pcl::PointCloud<PointType>::ConstPtr& cloud)
{
double res =0.0;
int n_points =0;
int nres;
std::vector<int>indices(2);
std::vector<float>sqr_distances(2);
pcl::arch::KdTree<PointType> tree;
tree.tInputCloud(cloud);
for(std::size_t i =0; i < cloud->size();++i)
{
if(!std::isfinite((*cloud)[i].x))
incontrast
{
continue;
}
//Considering the cond neighbor since the first is the point itlf.
nres = arestKSearch(i,2, indices, sqr_distances);
if(nres ==2)
{
res +=sqrt(sqr_distances[1]);
++n_points;
}
}
if(n_points !=0)
{
res /= n_points;
}
return res;
}
int
main(int argc,char* argv[])
{
parCommandLine(argc, argv);
pcl::PointCloud<PointType>::Ptr model(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr model_keypoints(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr scene(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr scene_keypoints(new pcl::PointCloud<PointType>());
pcl::PointCloud<NormalType>::Ptr model_normals(new pcl::PointCloud<NormalType>());
pcl::PointCloud<NormalType>::Ptr scene_normals(new pcl::PointCloud<NormalType>());
pcl::PointCloud<DescriptorType>::Ptr model_descriptors(new pcl::PointCloud<DescriptorType>()); pcl::PointCloud<DescriptorType>::Ptr scene_descriptors(new pcl::PointCloud<DescriptorType>());
//
// Load clouds
//
if(pcl::io::loadPCDFile(model_filename_,*model)<0)
{
std::cout <<"Error loading model cloud."<< std::endl;
showHelp(argv[0]);
return(-1);
return(-1);
}
if(pcl::io::loadPCDFile(scene_filename_,*scene)<0)
{
std::cout <<"Error loading scene cloud."<< std::endl;
showHelp(argv[0]);
return(-1);
}
//
/
/ Set up resolution invariance
//
if(u_cloud_resolution_)
{
float resolution =static_cast<float>(computeCloudResolution(model));
if(resolution !=0.0f)
{
model_ss_ *= resolution;
scene_ss_ *= resolution;
rf_rad_ *= resolution;
descr_rad_ *= resolution;
cg_size_ *= resolution;
}
std::cout <<"Model resolution: "<< resolution << std::endl;
std::cout <<"Model sampling size: "<< model_ss_ << std::endl;
std::cout <<"Scene sampling size: "<< scene_ss_ << std::endl;
std::cout <<"LRF support radius: "<< rf_rad_ << std::endl;
std::cout <<"SHOT descriptor radius: "<< descr_rad_ << std::endl;
std::cout <<"Clustering bin size: "<< cg_size_ << std::endl << std::endl;
}
//
// 计算每个点的法向量 normal法向量
/
/
pcl::NormalEstimationOMP<PointType, NormalType> norm_est;
norm_est.tKSearch (10);//使⽤临近的10个点进⾏计算
norm_est.tInputCloud(model);
pute (*model_normals);
norm_est.tInputCloud(scene);
pute (*scene_normals);
//
// Downsample Clouds to Extract keypoints
//
出国留学申请流程
pcl::UniformSampling<PointType> uniform_sampling;
uniform_sampling.tInputCloud (model);
uniform_sampling.tRadiusSearch(model_ss_);
uniform_sampling.filter (*model_keypoints);
std::cout <<"Model total points: "<< model->size()<<"; Selected Keypoints: "<< model_keypoints->size()<< std::endl;
uniform_sampling.tInputCloud (scene);
uniform_sampling.tRadiusSearch(scene_ss_);
uniform_sampling.filter (*scene_keypoints);
std::cout <<"Scene total points: "<< scene->size()<<"; Selected Keypoints: "<< scene_keypoints->size()<< std::endl;
//
// Compute Descriptor for keypoints
//
pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;//SHOT算法关键点描述⼦
descr_est.tRadiusSearch (descr_rad_);
descr_est.tInputCloud (model_keypoints);
descr_est.tInputCloud (model_keypoints);
descr_est.tInputNormals (model_normals);
descr_est.tSearchSurface(model);
pute (*model_descriptors);
descr_est.tInputCloud (scene_keypoints);
descr_est.tInputNormals (scene_normals);
descr_est.tSearchSurface(scene);
pute (*scene_descriptors);
//
// Find Model-Scene Correspondences with KdTree
//
pcl::CorrespondencesPtr model_scene_corrs(new pcl::Correspondences());
pcl::KdTreeFLANN<DescriptorType> match_arch;
match_arch.tInputCloud(model_descriptors);//
// For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector.
hungerfor(std::size_t i =0; i < scene_descriptors->size();++i)
{
std::vector<int>neigh_indices(1);
std::vector<float>neigh_sqr_dists(1);
if(!std::isfinite(scene_descriptors->at(i).descriptor[0]))//skipping NaNs
{
continue;
}
//计算每⼀点相对于model中点的欧式⼏何距离，根据SHOT算法给定0.25阈值以下的为匹配到的点，并将点进⾏保存
int found_neighs = arestKSearch(scene_descriptors->at(i),1, neigh_indices, neigh_sqr_dists);
if(found_neighs ==1&& neigh_sqr_dists[0]<0.25f)// add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
{
pcl::Correspondence corr(neigh_indices[0],static_cast<int>(i), neigh_sqr_dists[0]);
model_scene_corrs->push_back(corr);
}
}商品 英语
std::cout <<"Correspondences found: "<< model_scene_corrs->size()<< std::endl;
//
ogallala
// Actual Clustering
//
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> rototranslations;
std::vector<pcl::Correspondences> clustered_corrs;
// Using Hough3D
if(u_hough_)
{
//圣诞老人 英文
// Compute (Keypoints) Reference Frames only for Hough
//
pcl::PointCloud<RFType>::Ptr model_rf(new pcl::PointCloud<RFType>());
pcl::PointCloud<RFType>::Ptr scene_rf(new pcl::PointCloud<RFType>());
pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
rf_est.tFindHoles (true);
rf_est.tRadiusSearch (rf_rad_);
rf_est.tInputCloud (model_keypoints);
rf_est.tInputNormals (model_normals);
rf_est.tSearchSurface (model);
pute (*model_rf);
rf_est.tInputCloud (scene_keypoints);
rf_est.tInputNormals (scene_normals);
rf_est.tSearchSurface (scene);
