3D物体识别的假设检验

3D物体识别的假设验证

这次目的在于解释如何做3D物体识别通过验证模型假设在聚类里面。在描述器匹配后，这次我们将运行某个相关组算法在PCL里面为了聚类点对点相关性的集合，决定假设物体在场景里面的实例。在这个假定里面，全局假设验证算法将被用来减少错误的数量。

代码:

在开始之前，你应该从Correspondence Grouping里面下载文件。

下面是代码

/*
* Software License Agreement (BSD License)
*
*  Point Cloud Library (PCL) - www.pointclouds.org
*  Copyright (c) 2014-, Open Perception, Inc.
*
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the copyright holder(s) nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*/

#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/recognition/hv/hv_go.h>
#include <pcl/registration/icp.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/parse.h>

typedef pcl::PointXYZRGBA PointType;
typedef pcl::Normal NormalType;
typedef pcl::ReferenceFrame RFType;
typedef pcl::SHOT352 DescriptorType;

struct CloudStyle
{
double r;
double g;
double b;
double size;

CloudStyle (double r,
double g,
double b,
double size) :
r (r),
g (g),
b (b),
size (size)
{
}
};

CloudStyle style_white (255.0, 255.0, 255.0, 4.0);
CloudStyle style_red (255.0, 0.0, 0.0, 3.0);
CloudStyle style_green (0.0, 255.0, 0.0, 5.0);
CloudStyle style_cyan (93.0, 200.0, 217.0, 4.0);
CloudStyle style_violet (255.0, 0.0, 255.0, 8.0);

std::string model_filename_;
std::string scene_filename_;

//Algorithm params
bool show_keypoints_ (false);
bool use_hough_ (true);
float model_ss_ (0.02f);
float scene_ss_ (0.02f);
float rf_rad_ (0.015f);
float descr_rad_ (0.02f);
float cg_size_ (0.01f);
float cg_thresh_ (5.0f);
int icp_max_iter_ (5);
float icp_corr_distance_ (0.005f);
float hv_clutter_reg_ (5.0f);
float hv_inlier_th_ (0.005f);
float hv_occlusion_th_ (0.01f);
float hv_rad_clutter_ (0.03f);
float hv_regularizer_ (3.0f);
float hv_rad_normals_ (0.05);
bool hv_detect_clutter_ (true);

/**
* Prints out Help message
* @param filename Runnable App Name
*/
void
showHelp (char *filename)
{
std::cout << std::endl;
std::cout << "***************************************************************************" << std::endl;
std::cout << "*                                                                         *" << std::endl;
std::cout << "*          Global Hypothese Verification 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 keypoints." << std::endl;
std::cout << "     --algorithm (Hough|GC):      Clustering algorithm used (default Hough)." << std::endl;
std::cout << "     --model_ss val:              Model uniform sampling radius (default " << model_ss_ << ")" << std::endl;
std::cout << "     --scene_ss val:              Scene uniform sampling radius (default " << scene_ss_ << ")" << std::endl;
std::cout << "     --rf_rad val:                Reference frame radius (default " << rf_rad_ << ")" << std::endl;
std::cout << "     --descr_rad val:             Descriptor radius (default " << descr_rad_ << ")" << std::endl;
std::cout << "     --cg_size val:               Cluster size (default " << cg_size_ << ")" << std::endl;
std::cout << "     --cg_thresh val:             Clustering threshold (default " << cg_thresh_ << ")" << std::endl << std::endl;
std::cout << "     --icp_max_iter val:          ICP max iterations number (default " << icp_max_iter_ << ")" << std::endl;
std::cout << "     --icp_corr_distance val:     ICP correspondence distance (default " << icp_corr_distance_ << ")" << std::endl << std::endl;
std::cout << "     --hv_clutter_reg val:        Clutter Regularizer (default " << hv_clutter_reg_ << ")" << std::endl;
std::cout << "     --hv_inlier_th val:          Inlier threshold (default " << hv_inlier_th_ << ")" << std::endl;
std::cout << "     --hv_occlusion_th val:       Occlusion threshold (default " << hv_occlusion_th_ << ")" << std::endl;
std::cout << "     --hv_rad_clutter val:        Clutter radius (default " << hv_rad_clutter_ << ")" << std::endl;
std::cout << "     --hv_regularizer val:        Regularizer value (default " << hv_regularizer_ << ")" << std::endl;
std::cout << "     --hv_rad_normals val:        Normals radius (default " << hv_rad_normals_ << ")" << std::endl;
std::cout << "     --hv_detect_clutter val:     TRUE if clutter detect enabled (default " << hv_detect_clutter_ << ")" << std::endl << std::endl;
}

/**
* Parses Command Line Arguments (Argc,Argv)
* @param argc
* @param argv
*/
void
parseCommandLine (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::parse_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;
}

std::string used_algorithm;
if (pcl::console::parse_argument (argc, argv, "--algorithm", used_algorithm) != -1)
{
if (used_algorithm.compare ("Hough") == 0)
{
use_hough_ = true;
}
else if (used_algorithm.compare ("GC") == 0)
{
use_hough_ = false;
}
else
{
std::cout << "Wrong algorithm name.\n";
showHelp (argv[0]);
exit (-1);
}
}

//General parameters
pcl::console::parse_argument (argc, argv, "--model_ss", model_ss_);
pcl::console::parse_argument (argc, argv, "--scene_ss", scene_ss_);
pcl::console::parse_argument (argc, argv, "--rf_rad", rf_rad_);
pcl::console::parse_argument (argc, argv, "--descr_rad", descr_rad_);
pcl::console::parse_argument (argc, argv, "--cg_size", cg_size_);
pcl::console::parse_argument (argc, argv, "--cg_thresh", cg_thresh_);
pcl::console::parse_argument (argc, argv, "--icp_max_iter", icp_max_iter_);
pcl::console::parse_argument (argc, argv, "--icp_corr_distance", icp_corr_distance_);
pcl::console::parse_argument (argc, argv, "--hv_clutter_reg", hv_clutter_reg_);
pcl::console::parse_argument (argc, argv, "--hv_inlier_th", hv_inlier_th_);
pcl::console::parse_argument (argc, argv, "--hv_occlusion_th", hv_occlusion_th_);
pcl::console::parse_argument (argc, argv, "--hv_rad_clutter", hv_rad_clutter_);
pcl::console::parse_argument (argc, argv, "--hv_regularizer", hv_regularizer_);
pcl::console::parse_argument (argc, argv, "--hv_rad_normals", hv_rad_normals_);
pcl::console::parse_argument (argc, argv, "--hv_detect_clutter", hv_detect_clutter_);
}

int
main (int argc,
char *argv[])
{
parseCommandLine (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);
}
if (pcl::io::loadPCDFile (scene_filename_, *scene) < 0)
{
std::cout << "Error loading scene cloud." << std::endl;
showHelp (argv[0]);
return (-1);
}

/**
* Compute Normals
*/
pcl::NormalEstimationOMP<PointType, NormalType> norm_est;
norm_est.setKSearch (10);
norm_est.setInputCloud (model);
norm_est.compute (*model_normals);

norm_est.setInputCloud (scene);
norm_est.compute (*scene_normals);

/**
*  Downsample Clouds to Extract keypoints
*/
pcl::UniformSampling<PointType> uniform_sampling;
uniform_sampling.setInputCloud (model);
uniform_sampling.setRadiusSearch (model_ss_);
uniform_sampling.filter (*model_keypoints);
std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl;

uniform_sampling.setInputCloud (scene);
uniform_sampling.setRadiusSearch (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;
descr_est.setRadiusSearch (descr_rad_);

descr_est.setInputCloud (model_keypoints);
descr_est.setInputNormals (model_normals);
descr_est.setSearchSurface (model);
descr_est.compute (*model_descriptors);

descr_est.setInputCloud (scene_keypoints);
descr_est.setInputNormals (scene_normals);
descr_est.setSearchSurface (scene);
descr_est.compute (*scene_descriptors);

/**
*  Find Model-Scene Correspondences with KdTree
*/
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<DescriptorType> match_search;
match_search.setInputCloud (model_descriptors);
std::vector<int> model_good_keypoints_indices;
std::vector<int> scene_good_keypoints_indices;

for (size_t i = 0; i < scene_descriptors->size (); ++i)
{
std::vector<int> neigh_indices (1);
std::vector<float> neigh_sqr_dists (1);
if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0]))  //skipping NaNs
{
continue;
}
int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25f)
{
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
model_scene_corrs->push_back (corr);
model_good_keypoints_indices.push_back (corr.index_query);
scene_good_keypoints_indices.push_back (corr.index_match);
}
}
pcl::PointCloud<PointType>::Ptr model_good_kp (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr scene_good_kp (new pcl::PointCloud<PointType> ());
pcl::copyPointCloud (*model_keypoints, model_good_keypoints_indices, *model_good_kp);
pcl::copyPointCloud (*scene_keypoints, scene_good_keypoints_indices, *scene_good_kp);

std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;

/**
*  Clustering
*/
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
std::vector < pcl::Correspondences > clustered_corrs;

if (use_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.setFindHoles (true);
rf_est.setRadiusSearch (rf_rad_);

rf_est.setInputCloud (model_keypoints);
rf_est.setInputNormals (model_normals);
rf_est.setSearchSurface (model);
rf_est.compute (*model_rf);

rf_est.setInputCloud (scene_keypoints);
rf_est.setInputNormals (scene_normals);
rf_est.setSearchSurface (scene);
rf_est.compute (*scene_rf);

//  Clustering
pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
clusterer.setHoughBinSize (cg_size_);
clusterer.setHoughThreshold (cg_thresh_);
clusterer.setUseInterpolation (true);
clusterer.setUseDistanceWeight (false);

clusterer.setInputCloud (model_keypoints);
clusterer.setInputRf (model_rf);
clusterer.setSceneCloud (scene_keypoints);
clusterer.setSceneRf (scene_rf);
clusterer.setModelSceneCorrespondences (model_scene_corrs);

clusterer.recognize (rototranslations, clustered_corrs);
}
else
{
pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
gc_clusterer.setGCSize (cg_size_);
gc_clusterer.setGCThreshold (cg_thresh_);

gc_clusterer.setInputCloud (model_keypoints);
gc_clusterer.setSceneCloud (scene_keypoints);
gc_clusterer.setModelSceneCorrespondences (model_scene_corrs);

gc_clusterer.recognize (rototranslations, clustered_corrs);
}

/**
* Stop if no instances
*/
if (rototranslations.size () <= 0)
{
cout << "*** No instances found! ***" << endl;
return (0);
}
else
{
cout << "Recognized Instances: " << rototranslations.size () << endl << endl;
}

/**
* Generates clouds for each instances found
*/
std::vector<pcl::PointCloud<PointType>::ConstPtr> instances;

for (size_t i = 0; i < rototranslations.size (); ++i)
{
pcl::PointCloud<PointType>::Ptr rotated_model (new pcl::PointCloud<PointType> ());
pcl::transformPointCloud (*model, *rotated_model, rototranslations[i]);
instances.push_back (rotated_model);
}

/**
* ICP
*/
std::vector<pcl::PointCloud<PointType>::ConstPtr> registered_instances;
if (true)
{
cout << "--- ICP ---------" << endl;

for (size_t i = 0; i < rototranslations.size (); ++i)
{
pcl::IterativeClosestPoint<PointType, PointType> icp;
icp.setMaximumIterations (icp_max_iter_);
icp.setMaxCorrespondenceDistance (icp_corr_distance_);
icp.setInputTarget (scene);
icp.setInputSource (instances[i]);
pcl::PointCloud<PointType>::Ptr registered (new pcl::PointCloud<PointType>);
icp.align (*registered);
registered_instances.push_back (registered);
cout << "Instance " << i << " ";
if (icp.hasConverged ())
{
cout << "Aligned!" << endl;
}
else
{
cout << "Not Aligned!" << endl;
}
}

cout << "-----------------" << endl << endl;
}

/**
* Hypothesis Verification
*/
cout << "--- Hypotheses Verification ---" << endl;
std::vector<bool> hypotheses_mask;  // Mask Vector to identify positive hypotheses

pcl::GlobalHypothesesVerification<PointType, PointType> GoHv;

GoHv.setSceneCloud (scene);  // Scene Cloud
GoHv.addModels (registered_instances, true);  //Models to verify

GoHv.setInlierThreshold (hv_inlier_th_);
GoHv.setOcclusionThreshold (hv_occlusion_th_);
GoHv.setRegularizer (hv_regularizer_);
GoHv.setRadiusClutter (hv_rad_clutter_);
GoHv.setClutterRegularizer (hv_clutter_reg_);
GoHv.setDetectClutter (hv_detect_clutter_);
GoHv.setRadiusNormals (hv_rad_normals_);

GoHv.verify ();
GoHv.getMask (hypotheses_mask);  // i-element TRUE if hvModels[i] verifies hypotheses

for (int i = 0; i < hypotheses_mask.size (); i++)
{
if (hypotheses_mask[i])
{
cout << "Instance " << i << " is GOOD! <---" << endl;
}
else
{
cout << "Instance " << i << " is bad!" << endl;
}
}
cout << "-------------------------------" << endl;

/**
*  Visualization
*/
pcl::visualization::PCLVisualizer viewer ("Hypotheses Verification");
viewer.addPointCloud (scene, "scene_cloud");

pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints (new pcl::PointCloud<PointType> ());

pcl::PointCloud<PointType>::Ptr off_model_good_kp (new pcl::PointCloud<PointType> ());
pcl::transformPointCloud (*model, *off_scene_model, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));
pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));
pcl::transformPointCloud (*model_good_kp, *off_model_good_kp, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));

if (show_keypoints_)
{
CloudStyle modelStyle = style_white;
pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, modelStyle.r, modelStyle.g, modelStyle.b);
viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, modelStyle.size, "off_scene_model");
}

if (show_keypoints_)
{
CloudStyle goodKeypointStyle = style_violet;
pcl::visualization::PointCloudColorHandlerCustom<PointType> model_good_keypoints_color_handler (off_model_good_kp, goodKeypointStyle.r, goodKeypointStyle.g,
goodKeypointStyle.b);
viewer.addPointCloud (off_model_good_kp, model_good_keypoints_color_handler, "model_good_keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, goodKeypointStyle.size, "model_good_keypoints");

pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_good_keypoints_color_handler (scene_good_kp, goodKeypointStyle.r, goodKeypointStyle.g,
goodKeypointStyle.b);
viewer.addPointCloud (scene_good_kp, scene_good_keypoints_color_handler, "scene_good_keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, goodKeypointStyle.size, "scene_good_keypoints");
}

for (size_t i = 0; i < instances.size (); ++i)
{
std::stringstream ss_instance;
ss_instance << "instance_" << i;

CloudStyle clusterStyle = style_red;
pcl::visualization::PointCloudColorHandlerCustom<PointType> instance_color_handler (instances[i], clusterStyle.r, clusterStyle.g, clusterStyle.b);
viewer.addPointCloud (instances[i], instance_color_handler, ss_instance.str ());
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, clusterStyle.size, ss_instance.str ());

CloudStyle registeredStyles = hypotheses_mask[i] ? style_green : style_cyan;
ss_instance << "_registered" << endl;
pcl::visualization::PointCloudColorHandlerCustom<PointType> registered_instance_color_handler (registered_instances[i], registeredStyles.r,
registeredStyles.g, registeredStyles.b);
viewer.addPointCloud (registered_instances[i], registered_instance_color_handler, ss_instance.str ());
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, registeredStyles.size, ss_instance.str ());
}

while (!viewer.wasStopped ())
{
viewer.spinOnce ();
}

return (0);
}

聚类

下面的代码将执行一个完整的聚类管道，输入的管道是一对点云，输出是

std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;

rototraslations表示在场景里面一列粗糙的转换模型。

/**
* Compute Normals
*/
pcl::NormalEstimationOMP<PointType, NormalType> norm_est;
norm_est.setKSearch (10);
norm_est.setInputCloud (model);
norm_est.compute (*model_normals);

norm_est.setInputCloud (scene);
norm_est.compute (*scene_normals);

/**
*  Downsample Clouds to Extract keypoints
*/
pcl::UniformSampling<PointType> uniform_sampling;
uniform_sampling.setInputCloud (model);
uniform_sampling.setRadiusSearch (model_ss_);
uniform_sampling.filter (*model_keypoints);
std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl;

uniform_sampling.setInputCloud (scene);
uniform_sampling.setRadiusSearch (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;
descr_est.setRadiusSearch (descr_rad_);

descr_est.setInputCloud (model_keypoints);
descr_est.setInputNormals (model_normals);
descr_est.setSearchSurface (model);
descr_est.compute (*model_descriptors);

descr_est.setInputCloud (scene_keypoints);
descr_est.setInputNormals (scene_normals);
descr_est.setSearchSurface (scene);
descr_est.compute (*scene_descriptors);

/**
*  Find Model-Scene Correspondences with KdTree
*/
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<DescriptorType> match_search;
match_search.setInputCloud (model_descriptors);
std::vector<int> model_good_keypoints_indices;
std::vector<int> scene_good_keypoints_indices;

for (size_t i = 0; i < scene_descriptors->size (); ++i)
{
std::vector<int> neigh_indices (1);
std::vector<float> neigh_sqr_dists (1);
if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0]))  //skipping NaNs
{
continue;
}
int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25f)
{
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
model_scene_corrs->push_back (corr);
model_good_keypoints_indices.push_back (corr.index_query);
scene_good_keypoints_indices.push_back (corr.index_match);
}
}
pcl::PointCloud<PointType>::Ptr model_good_kp (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr scene_good_kp (new pcl::PointCloud<PointType> ());
pcl::copyPointCloud (*model_keypoints, model_good_keypoints_indices, *model_good_kp);
pcl::copyPointCloud (*scene_keypoints, scene_good_keypoints_indices, *scene_good_kp);

std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;

/**
*  Clustering
*/
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
std::vector < pcl::Correspondences > clustered_corrs;

if (use_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.setFindHoles (true);
rf_est.setRadiusSearch (rf_rad_);

rf_est.setInputCloud (model_keypoints);
rf_est.setInputNormals (model_normals);
rf_est.setSearchSurface (model);
rf_est.compute (*model_rf);

rf_est.setInputCloud (scene_keypoints);
rf_est.setInputNormals (scene_normals);
rf_est.setSearchSurface (scene);
rf_est.compute (*scene_rf);

//  Clustering
pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
clusterer.setHoughBinSize (cg_size_);
clusterer.setHoughThreshold (cg_thresh_);
clusterer.setUseInterpolation (true);
clusterer.setUseDistanceWeight (false);

clusterer.setInputCloud (model_keypoints);
clusterer.setInputRf (model_rf);
clusterer.setSceneCloud (scene_keypoints);
clusterer.setSceneRf (scene_rf);
clusterer.setModelSceneCorrespondences (model_scene_corrs);

clusterer.recognize (rototranslations, clustered_corrs);
}
else
{
pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
gc_clusterer.setGCSize (cg_size_);
gc_clusterer.setGCThreshold (cg_thresh_);

gc_clusterer.setInputCloud (model_keypoints);
gc_clusterer.setSceneCloud (scene_keypoints);
gc_clusterer.setModelSceneCorrespondences (model_scene_corrs);

gc_clusterer.recognize (rototranslations, clustered_corrs);
}

/**
* Stop if no instances
*/

模型场景投影

为了提高与每个对象假设相联系的粗糙的转换，我们创造了一个instances列去存储"粗糙"的转换

for (size_t i = 0; i < rototranslations.size (); ++i)
{
pcl::PointCloud<PointType>::Ptr rotated_model (new pcl::PointCloud<PointType> ());
pcl::transformPointCloud (*model, *rotated_model, rototranslations[i]);
instances.push_back (rotated_model);
}

/**
* ICP
*/

接下去，我们运行ICP在wrt的实例上。

if (true)
{
cout << "--- ICP ---------" << endl;

for (size_t i = 0; i < rototranslations.size (); ++i)
{
pcl::IterativeClosestPoint<PointType, PointType> icp;
icp.setMaximumIterations (icp_max_iter_);
icp.setMaxCorrespondenceDistance (icp_corr_distance_);
icp.setInputTarget (scene);
icp.setInputSource (instances[i]);
pcl::PointCloud<PointType>::Ptr registered (new pcl::PointCloud<PointType>);
icp.align (*registered);
registered_instances.push_back (registered);
cout << "Instance " << i << " ";
if (icp.hasConverged ())
{
cout << "Aligned!" << endl;
}
else
{
cout << "Not Aligned!" << endl;
}
}

cout << "-----------------" << endl << endl;
}

/**
* Hypothesis Verification
*/

假设验证

std::vector<bool> hypotheses_mask;  // Mask Vector to identify positive hypotheses

pcl::GlobalHypothesesVerification<PointType, PointType> GoHv;

GoHv.setSceneCloud (scene);  // Scene Cloud
GoHv.addModels (registered_instances, true);  //Models to verify

GoHv.setInlierThreshold (hv_inlier_th_);
GoHv.setOcclusionThreshold (hv_occlusion_th_);
GoHv.setRegularizer (hv_regularizer_);
GoHv.setRadiusClutter (hv_rad_clutter_);
GoHv.setClutterRegularizer (hv_clutter_reg_);
GoHv.setDetectClutter (hv_detect_clutter_);
GoHv.setRadiusNormals (hv_rad_normals_);

GoHv.verify ();
GoHv.getMask (hypotheses_mask);  // i-element TRUE if hvModels[i] verifies hypotheses

for (int i = 0; i < hypotheses_mask.size (); i++)
{
if (hypotheses_mask[i])
{
cout << "Instance " << i << " is GOOD! <---" << endl;
}
else
{
cout << "Instance " << i << " is bad!" << endl;
}
}
cout << "-------------------------------" << endl;

/**
*  Visualization
*/

GlobalHypothesesVerification作为resgistered_instances的一列输入和一个场景，所以我们可以验证他们来得到一个hypotheses_mask这是一个bool类型的数组，如果registered_instances[i]是一个正确的假设那么hypotheses_mask[i]是TRUE。

可视化

viewer.addPointCloud (scene, "scene_cloud");

pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints (new pcl::PointCloud<PointType> ());

pcl::PointCloud<PointType>::Ptr off_model_good_kp (new pcl::PointCloud<PointType> ());
pcl::transformPointCloud (*model, *off_scene_model, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));
pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));
pcl::transformPointCloud (*model_good_kp, *off_model_good_kp, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));

if (show_keypoints_)
{
CloudStyle modelStyle = style_white;
pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, modelStyle.r, modelStyle.g, modelStyle.b);
viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, modelStyle.size, "off_scene_model");
}

if (show_keypoints_)
{
CloudStyle goodKeypointStyle = style_violet;
pcl::visualization::PointCloudColorHandlerCustom<PointType> model_good_keypoints_color_handler (off_model_good_kp, goodKeypointStyle.r, goodKeypointStyle.g,
goodKeypointStyle.b);
viewer.addPointCloud (off_model_good_kp, model_good_keypoints_color_handler, "model_good_keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, goodKeypointStyle.size, "model_good_keypoints");

pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_good_keypoints_color_handler (scene_good_kp, goodKeypointStyle.r, goodKeypointStyle.g,
goodKeypointStyle.b);
viewer.addPointCloud (scene_good_kp, scene_good_keypoints_color_handler, "scene_good_keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, goodKeypointStyle.size, "scene_good_keypoints");
}

for (size_t i = 0; i < instances.size (); ++i)
{
std::stringstream ss_instance;
ss_instance << "instance_" << i;

CloudStyle clusterStyle = style_red;
pcl::visualization::PointCloudColorHandlerCustom<PointType> instance_color_handler (instances[i], clusterStyle.r, clusterStyle.g, clusterStyle.b);
viewer.addPointCloud (instances[i], instance_color_handler, ss_instance.str ());
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, clusterStyle.size, ss_instance.str ());

CloudStyle registeredStyles = hypotheses_mask[i] ? style_green : style_cyan;
ss_instance << "_registered" << endl;
pcl::visualization::PointCloudColorHandlerCustom<PointType> registered_instance_color_handler (registered_instances[i], registeredStyles.r,
registeredStyles.g, registeredStyles.b);
viewer.addPointCloud (registered_instances[i], registered_instance_color_handler, ss_instance.str ());
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, registeredStyles.size, ss_instance.str ());
}

while (!viewer.wasStopped ())
{
viewer.spinOnce ();
}

return (0);
}

运行

./global_hypothesis_verification milk.pcd milk_cartoon_all_small_clorox.pcd

有效的假设是图中绿色的部分

你可以模拟更多错误通过使用一个尺寸参数对于hough相关组决策算法。

./global_hypothesis_verification milk.pcd milk_cartoon_all_small_clorox.pcd --cg_size 0.035