test_recognition_cg.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: $
00037  *
00038  */
00039 #include <gtest/gtest.h>
00040 #include <pcl/io/pcd_io.h>
00041 #include <pcl/point_cloud.h>
00042 #include <pcl/common/transforms.h>
00043 #include <pcl/correspondence.h>
00044 #include <pcl/features/normal_3d_omp.h>
00045 #include <pcl/features/shot_omp.h>
00046 #include <pcl/features/board.h>
00047 #include <pcl/keypoints/uniform_sampling.h>
00048 #include <pcl/recognition/cg/hough_3d.h>
00049 #include <pcl/recognition/cg/geometric_consistency.h>
00050 #include <pcl/kdtree/kdtree_flann.h>
00051 #include <pcl/kdtree/impl/kdtree_flann.hpp>
00052 #include <pcl/common/eigen.h>
00053 
00054 using namespace std;
00055 using namespace pcl;
00056 using namespace pcl::io;
00057 
00058 typedef PointXYZ PointType;
00059 typedef Normal NormalType;
00060 typedef ReferenceFrame RFType;
00061 typedef SHOT352 DescriptorType;
00062 
00063 PointCloud<PointType>::Ptr model_ (new PointCloud<PointType> ());
00064 PointCloud<PointType>::Ptr model_downsampled_ (new PointCloud<PointType> ());
00065 PointCloud<PointType>::Ptr scene_ (new PointCloud<PointType> ());
00066 PointCloud<PointType>::Ptr scene_downsampled_ (new PointCloud<PointType> ());
00067 PointCloud<NormalType>::Ptr model_normals_ (new PointCloud<NormalType> ());
00068 PointCloud<NormalType>::Ptr scene_normals_ (new PointCloud<NormalType> ());
00069 PointCloud<DescriptorType>::Ptr model_descriptors_ (new PointCloud<DescriptorType> ());
00070 PointCloud<DescriptorType>::Ptr scene_descriptors_ (new PointCloud<DescriptorType> ());
00071 CorrespondencesPtr model_scene_corrs_ (new Correspondences ());
00072 
00073 double
00074 computeRmsE (const PointCloud<PointType>::ConstPtr &model, const PointCloud<PointType>::ConstPtr &scene, const Eigen::Matrix4f &rototranslation)
00075 {
00076   PointCloud<PointType> transformed_model;
00077   transformPointCloud (*model, transformed_model, rototranslation);
00078 
00079   KdTreeFLANN<PointType> tree;
00080   tree.setInputCloud (scene);
00081 
00082   double sqr_norm_sum = 0;
00083   int found_points = 0;
00084 
00085   vector<int> neigh_indices (1);
00086   vector<float> neigh_sqr_dists (1);
00087   for (size_t i = 0; i < transformed_model.size (); ++i)
00088   {
00089 
00090     int found_neighs = tree.nearestKSearch (transformed_model.at (i), 1, neigh_indices, neigh_sqr_dists);
00091     if(found_neighs == 1)
00092     {
00093       ++found_points;
00094       sqr_norm_sum += static_cast<double> (neigh_sqr_dists[0]);
00095     }
00096   }
00097 
00098   if (found_points > 0)
00099     return sqrt (sqr_norm_sum / double (transformed_model.size ()));
00100 
00101   return numeric_limits<double>::max ();
00102 }
00103 
00105 TEST (PCL, Hough3DGrouping)
00106 {
00107   PointCloud<RFType>::Ptr model_rf (new PointCloud<RFType> ());
00108   PointCloud<RFType>::Ptr scene_rf (new PointCloud<RFType> ());
00109 
00110   //RFs
00111   BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
00112   rf_est.setRadiusSearch (0.015);
00113   rf_est.setInputCloud (model_downsampled_);
00114   rf_est.setInputNormals (model_normals_);
00115   rf_est.setSearchSurface (model_);
00116   rf_est.compute (*model_rf);
00117 
00118   rf_est.setInputCloud (scene_downsampled_);
00119   rf_est.setInputNormals (scene_normals_);
00120   rf_est.setSearchSurface (scene_);
00121   rf_est.compute (*scene_rf);
00122 
00123   vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
00124 
00125   //Actual CG
00126   Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
00127   clusterer.setInputCloud (model_downsampled_);
00128   clusterer.setInputRf (model_rf);
00129   clusterer.setSceneCloud (scene_downsampled_);
00130   clusterer.setSceneRf (scene_rf);
00131   clusterer.setModelSceneCorrespondences (model_scene_corrs_);
00132   clusterer.setHoughBinSize (0.03);
00133   clusterer.setHoughThreshold (25);
00134   EXPECT_TRUE (clusterer.recognize (rototranslations));
00135 
00136   //Assertions
00137   EXPECT_EQ (rototranslations.size (), 1);
00138   EXPECT_LT (computeRmsE (model_, scene_, rototranslations[0]), 1E-4);
00139 }
00140 
00142 TEST (PCL, GeometricConsistencyGrouping)
00143 {
00144   vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
00145 
00146   GeometricConsistencyGrouping<PointType, PointType> clusterer;
00147   clusterer.setInputCloud (model_downsampled_);
00148   clusterer.setSceneCloud (scene_downsampled_);
00149   clusterer.setModelSceneCorrespondences (model_scene_corrs_);
00150   clusterer.setGCSize (0.015);
00151   clusterer.setGCThreshold (25);
00152   EXPECT_TRUE (clusterer.recognize (rototranslations));
00153 
00154   //Assertions
00155   EXPECT_EQ (rototranslations.size (), 1);
00156   EXPECT_LT (computeRmsE (model_, scene_, rototranslations[0]), 1E-4);
00157 }
00158 
00159 
00160 /* ---[ */
00161 int
00162 main (int argc, char** argv)
00163 {
00164   if (argc < 3)
00165   {
00166     cerr << "No test file given. Please download `milk.pcd` and `milk_cartoon_all_small_clorox.pcd` and pass their paths to the test." << endl;
00167     return (-1);
00168   }
00169 
00170   if (loadPCDFile (argv[1], *model_) < 0)
00171   {
00172     cerr << "Failed to read test file. Please download `milk.pcd` and pass its path to the test." << endl;
00173     return (-1);
00174   }
00175 
00176   if (loadPCDFile (argv[2], *scene_) < 0)
00177   {
00178     cerr << "Failed to read test file. Please download `milk_cartoon_all_small_clorox.pcd` and pass its path to the test." << endl;
00179     return (-1);
00180   }
00181 
00182   //Normals
00183   NormalEstimationOMP<PointType, NormalType> norm_est;
00184   norm_est.setKSearch (10);
00185   norm_est.setInputCloud (model_);
00186   norm_est.compute (*model_normals_);
00187 
00188   norm_est.setInputCloud (scene_);
00189   norm_est.compute (*scene_normals_);
00190 
00191   //Downsampling
00192   PointCloud<int> sampled_indices;
00193   UniformSampling<PointType> uniform_sampling;
00194   uniform_sampling.setInputCloud (model_);
00195   uniform_sampling.setRadiusSearch (0.005);
00196   uniform_sampling.compute (sampled_indices);
00197   copyPointCloud (*model_, sampled_indices.points, *model_downsampled_);
00198 
00199   uniform_sampling.setInputCloud (scene_);
00200   uniform_sampling.setRadiusSearch (0.02);
00201   uniform_sampling.compute (sampled_indices);
00202   copyPointCloud (*scene_, sampled_indices.points, *scene_downsampled_);
00203 
00204   //Descriptor
00205   SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;
00206   descr_est.setRadiusSearch (0.015);
00207   descr_est.setInputCloud (model_downsampled_);
00208   descr_est.setInputNormals (model_normals_);
00209   descr_est.setSearchSurface (model_);
00210   descr_est.compute (*model_descriptors_);
00211 
00212   descr_est.setInputCloud (scene_downsampled_);
00213   descr_est.setInputNormals (scene_normals_);
00214   descr_est.setSearchSurface (scene_);
00215   descr_est.compute (*scene_descriptors_);
00216 
00217   //Correspondences with KdTree
00218   KdTreeFLANN<DescriptorType> match_search;
00219   match_search.setInputCloud (model_descriptors_);
00220 
00221   for (size_t i = 0; i < scene_descriptors_->size (); ++i)
00222   {
00223     if ( pcl_isfinite( scene_descriptors_->at (i).descriptor[0] ) )
00224     {
00225       vector<int> neigh_indices (1);
00226       vector<float> neigh_sqr_dists (1);
00227       int found_neighs = match_search.nearestKSearch (scene_descriptors_->at (i), 1, neigh_indices, neigh_sqr_dists);
00228       if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f)
00229       {
00230         Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
00231         model_scene_corrs_->push_back (corr);
00232       }
00233     }
00234   }
00235 
00236   testing::InitGoogleTest (&argc, argv);
00237   return (RUN_ALL_TESTS ());
00238 }
00239 /* ]--- */


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:35:37