test_pfh_estimation.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 Willow Garage, Inc. 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 
00040 #include <gtest/gtest.h>
00041 #include <pcl/point_cloud.h>
00042 #include <pcl/features/normal_3d.h>
00043 #include <pcl/features/pfh.h>
00044 #include <pcl/features/fpfh.h>
00045 #include <pcl/features/fpfh_omp.h>
00046 #include <pcl/features/vfh.h>
00047 #include <pcl/io/pcd_io.h>
00048 
00049 using namespace pcl;
00050 using namespace pcl::io;
00051 using namespace std;
00052 
00053 typedef search::KdTree<PointXYZ>::Ptr KdTreePtr;
00054 
00055 PointCloud<PointXYZ> cloud;
00056 vector<int> indices;
00057 KdTreePtr tree;
00058 
00060 template <typename FeatureEstimation, typename PointT, typename NormalT, typename OutputT> void
00061 testIndicesAndSearchSurface (const typename PointCloud<PointT>::Ptr & points,
00062                              const typename PointCloud<NormalT>::Ptr & normals,
00063                              const boost::shared_ptr<vector<int> > & indices, int ndims)
00064 {
00065   //
00066   // Test setIndices and setSearchSurface
00067   //
00068   PointCloud<OutputT> full_output, output0, output1, output2;
00069 
00070   // Compute for all points and then subsample the results
00071   FeatureEstimation est0;
00072   est0.setSearchMethod (typename search::KdTree<PointT>::Ptr (new search::KdTree<PointT>));
00073   est0.setKSearch (10);
00074   est0.setInputCloud (points);
00075   est0.setInputNormals (normals);
00076   est0.compute (full_output);
00077   copyPointCloud (full_output, *indices, output0);
00078 
00079   // Compute with all points as "search surface" and the specified sub-cloud as "input"
00080   typename PointCloud<PointT>::Ptr subpoints (new PointCloud<PointT>);
00081   copyPointCloud (*points, *indices, *subpoints);
00082   FeatureEstimation est1;
00083   est1.setSearchMethod (typename search::KdTree<PointT>::Ptr (new search::KdTree<PointT>));
00084   est1.setKSearch (10);
00085   est1.setInputCloud (subpoints);
00086   est1.setSearchSurface (points);
00087   est1.setInputNormals (normals);
00088   est1.compute (output1);
00089 
00090   // Compute with all points as "input" and the specified indices
00091   FeatureEstimation est2;
00092   est2.setSearchMethod (typename search::KdTree<PointT>::Ptr (new search::KdTree<PointT>));
00093   est2.setKSearch (10);
00094   est2.setInputCloud (points);
00095   est2.setInputNormals (normals);
00096   est2.setIndices (indices);
00097   est2.compute (output2);
00098 
00099   // All three of the above cases should produce equivalent results
00100   ASSERT_EQ (output0.size (), output1.size ());
00101   ASSERT_EQ (output1.size (), output2.size ());
00102   for (size_t i = 0; i < output1.size (); ++i)
00103   {
00104     for (int j = 0; j < ndims; ++j)
00105     {
00106       ASSERT_EQ (output0.points[i].histogram[j], output1.points[i].histogram[j]);
00107       ASSERT_EQ (output1.points[i].histogram[j], output2.points[i].histogram[j]);
00108     }
00109   }
00110 
00111   //
00112   // Test the combination of setIndices and setSearchSurface
00113   //
00114   PointCloud<OutputT> output3, output4;
00115 
00116   boost::shared_ptr<vector<int> > indices2 (new vector<int> (0));
00117   for (size_t i = 0; i < (indices->size ()/2); ++i)
00118     indices2->push_back (static_cast<int> (i));
00119 
00120   // Compute with all points as search surface + the specified sub-cloud as "input" but for only a subset of indices
00121   FeatureEstimation est3;
00122   est3.setSearchMethod (typename search::KdTree<PointT>::Ptr (new search::KdTree<PointT>));
00123   est3.setKSearch (10);
00124   est3.setSearchSurface (points);
00125   est3.setInputNormals (normals);
00126   est3.setInputCloud (subpoints);
00127   est3.setIndices (indices2);
00128   est3.compute (output3);
00129 
00130   // Start with features for each point in "subpoints" and then subsample the results
00131   copyPointCloud (output0, *indices2, output4); // (Re-using "output0" from above)
00132 
00133   // The two cases above should produce equivalent results
00134   ASSERT_EQ (output3.size (), output4.size ());
00135   for (size_t i = 0; i < output3.size (); ++i)
00136   {
00137     for (int j = 0; j < ndims; ++j)
00138     {
00139       ASSERT_EQ (output3.points[i].histogram[j], output4.points[i].histogram[j]);
00140     }
00141   }
00142 }
00143 
00145 TEST (PCL, PFHEstimation)
00146 {
00147   float f1, f2, f3, f4;
00148 
00149   // Estimate normals first
00150   NormalEstimation<PointXYZ, Normal> n;
00151   PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
00152   // set parameters
00153   n.setInputCloud (cloud.makeShared ());
00154   boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
00155   n.setIndices (indicesptr);
00156   n.setSearchMethod (tree);
00157   n.setKSearch (10); // Use 10 nearest neighbors to estimate the normals
00158   // estimate
00159   n.compute (*normals);
00160 
00161   PFHEstimation<PointXYZ, Normal, PFHSignature125> pfh;
00162   pfh.setInputNormals (normals);
00163   EXPECT_EQ (pfh.getInputNormals (), normals);
00164 
00165   // computePairFeatures
00166   pfh.computePairFeatures (cloud, *normals, 0, 12, f1, f2, f3, f4);
00167   EXPECT_NEAR (f1, -0.072575, 1e-4);
00168   EXPECT_NEAR (f2, -0.040221, 1e-4);
00169   EXPECT_NEAR (f3, 0.068133, 1e-4);
00170   EXPECT_NEAR (f4, 0.006130, 1e-4);
00171 
00172   // computePointPFHSignature
00173   int nr_subdiv = 3;
00174   Eigen::VectorXf pfh_histogram (nr_subdiv * nr_subdiv * nr_subdiv);
00175   pfh.computePointPFHSignature (cloud, *normals, indices, nr_subdiv, pfh_histogram);
00176   EXPECT_NEAR (pfh_histogram[0],  0.932506, 1e-2);
00177   EXPECT_NEAR (pfh_histogram[1],  2.32429 , 1e-2);
00178   EXPECT_NEAR (pfh_histogram[2],  0.357477, 1e-2);
00179   EXPECT_NEAR (pfh_histogram[3],  0.848541, 1e-2);
00180   EXPECT_NEAR (pfh_histogram[4],  3.65565 , 2e-2); // larger error w.r.t. considering all point pairs (feature bins=0,1,1 where 1 is middle, so angle of 0)
00181   EXPECT_NEAR (pfh_histogram[5],  0.178104, 1e-2);
00182   EXPECT_NEAR (pfh_histogram[6],  1.45284 , 1e-2);
00183   EXPECT_NEAR (pfh_histogram[7],  3.60666 , 1e-2);
00184   EXPECT_NEAR (pfh_histogram[8],  0.298959, 1e-2);
00185   EXPECT_NEAR (pfh_histogram[9],  0.295143, 1e-2);
00186   EXPECT_NEAR (pfh_histogram[10], 2.13474 , 1e-2);
00187   EXPECT_NEAR (pfh_histogram[11], 0.41218 , 1e-2);
00188   EXPECT_NEAR (pfh_histogram[12], 0.165382, 1e-2);
00189   EXPECT_NEAR (pfh_histogram[13], 8.97407 , 1e-2);
00190   EXPECT_NEAR (pfh_histogram[14], 0.306592, 1e-2);
00191   EXPECT_NEAR (pfh_histogram[15], 0.455432, 1e-2);
00192   EXPECT_NEAR (pfh_histogram[16], 4.5977 ,  1e-2);
00193   EXPECT_NEAR (pfh_histogram[17], 0.393097, 1e-2);
00194   EXPECT_NEAR (pfh_histogram[18], 7.54668 , 1e-2);
00195   EXPECT_NEAR (pfh_histogram[19], 6.78336 , 1e-2);
00196   EXPECT_NEAR (pfh_histogram[20], 1.63858 , 1e-2);
00197   EXPECT_NEAR (pfh_histogram[21], 9.93842 , 1e-2);
00198   EXPECT_NEAR (pfh_histogram[22], 18.4947 , 2e-2); // larger error w.r.t. considering all point pairs (feature bins=2,1,1 where 1 is middle, so angle of 0)
00199   EXPECT_NEAR (pfh_histogram[23], 1.96553 , 1e-4);
00200   EXPECT_NEAR (pfh_histogram[24], 8.04793 , 1e-4);
00201   EXPECT_NEAR (pfh_histogram[25], 11.2793  , 1e-4);
00202   EXPECT_NEAR (pfh_histogram[26], 2.91714 , 1e-4);
00203 
00204   // Sum of values should be 100
00205   EXPECT_NEAR (pfh_histogram.sum (), 100.0, 1e-2);
00206   //std::cerr << pfh_histogram << std::endl;
00207 
00208   // Object
00209   PointCloud<PFHSignature125>::Ptr pfhs (new PointCloud<PFHSignature125> ());
00210 
00211   // set parameters
00212   pfh.setInputCloud (cloud.makeShared ());
00213   pfh.setIndices (indicesptr);
00214   pfh.setSearchMethod (tree);
00215   pfh.setKSearch (static_cast<int> (indices.size ()));
00216 
00217   // estimate
00218   pfh.compute (*pfhs);
00219   EXPECT_EQ (pfhs->points.size (), indices.size ());
00220 
00221   for (size_t i = 0; i < pfhs->points.size (); ++i)
00222   {
00223     EXPECT_NEAR (pfhs->points[i].histogram[0],  0.156477  , 1e-4);
00224     EXPECT_NEAR (pfhs->points[i].histogram[1],  0.539396  , 1e-4);
00225     EXPECT_NEAR (pfhs->points[i].histogram[2],  0.410907  , 1e-4);
00226     EXPECT_NEAR (pfhs->points[i].histogram[3],  0.184465  , 1e-4);
00227     EXPECT_NEAR (pfhs->points[i].histogram[4],  0.115767  , 1e-4);
00228     EXPECT_NEAR (pfhs->points[i].histogram[5],  0.0572475 , 1e-4);
00229     EXPECT_NEAR (pfhs->points[i].histogram[6],  0.206092  , 1e-4);
00230     EXPECT_NEAR (pfhs->points[i].histogram[7],  0.339667  , 1e-4);
00231     EXPECT_NEAR (pfhs->points[i].histogram[8],  0.265883  , 1e-4);
00232     EXPECT_NEAR (pfhs->points[i].histogram[9],  0.0038165 , 1e-4);
00233     EXPECT_NEAR (pfhs->points[i].histogram[10], 0.103046  , 1e-4);
00234     EXPECT_NEAR (pfhs->points[i].histogram[11], 0.214997  , 1e-4);
00235     EXPECT_NEAR (pfhs->points[i].histogram[12], 0.398186  , 3e-2); // larger error w.r.t. considering all point pairs (feature bins=0,2,2 where 2 is middle, so angle of 0)
00236     EXPECT_NEAR (pfhs->points[i].histogram[13], 0.298959  , 1e-4);
00237     EXPECT_NEAR (pfhs->points[i].histogram[14], 0.00127217, 1e-4);
00238     EXPECT_NEAR (pfhs->points[i].histogram[15], 0.11704   , 1e-4);
00239     EXPECT_NEAR (pfhs->points[i].histogram[16], 0.255706  , 1e-4);
00240     EXPECT_NEAR (pfhs->points[i].histogram[17], 0.356205  , 1e-4);
00241     EXPECT_NEAR (pfhs->points[i].histogram[18], 0.265883  , 1e-4);
00242     EXPECT_NEAR (pfhs->points[i].histogram[19], 0.00127217, 1e-4);
00243     EXPECT_NEAR (pfhs->points[i].histogram[20], 0.148844  , 1e-4);
00244     //EXPECT_NEAR (pfhs->points[i].histogram[21], 0.721316  , 1e-3);
00245     //EXPECT_NEAR (pfhs->points[i].histogram[22], 0.438899  , 1e-2);
00246     EXPECT_NEAR (pfhs->points[i].histogram[23], 0.22263   , 1e-4);
00247     EXPECT_NEAR (pfhs->points[i].histogram[24], 0.0216269 , 1e-4);
00248     EXPECT_NEAR (pfhs->points[i].histogram[25], 0.223902  , 1e-4);
00249     EXPECT_NEAR (pfhs->points[i].histogram[26], 0.07633   , 1e-4);
00250   }
00251   //Eigen::Map<Eigen::VectorXf> h (&(pfhs->points[0].histogram[0]), 125);
00252   //std::cerr << h.head<27> () << std::endl;
00253 
00254   // Test results when setIndices and/or setSearchSurface are used
00255 
00256   boost::shared_ptr<vector<int> > test_indices (new vector<int> (0));
00257   for (size_t i = 0; i < cloud.size (); i+=3)
00258     test_indices->push_back (static_cast<int> (i));
00259 
00260   testIndicesAndSearchSurface<PFHEstimation<PointXYZ, Normal, PFHSignature125>, PointXYZ, Normal, PFHSignature125>
00261   (cloud.makeShared (), normals, test_indices, 125);
00262 }
00263 
00265 TEST (PCL, FPFHEstimation)
00266 {
00267   // Estimate normals first
00268   NormalEstimation<PointXYZ, Normal> n;
00269   PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
00270   // set parameters
00271   n.setInputCloud (cloud.makeShared ());
00272   boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
00273   n.setIndices (indicesptr);
00274   n.setSearchMethod (tree);
00275   n.setKSearch (10); // Use 10 nearest neighbors to estimate the normals
00276   // estimate
00277   n.compute (*normals);
00278 
00279   FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fpfh;
00280   fpfh.setInputNormals (normals);
00281   EXPECT_EQ (fpfh.getInputNormals (), normals);
00282 
00283   // computePointSPFHSignature
00284   int nr_subdiv = 11; // use the same number of bins for all three angular features
00285   Eigen::MatrixXf hist_f1 (indices.size (), nr_subdiv), hist_f2 (indices.size (), nr_subdiv), hist_f3 (indices.size (), nr_subdiv);
00286   hist_f1.setZero (); hist_f2.setZero (); hist_f3.setZero ();
00287   for (int i = 0; i < static_cast<int> (indices.size ()); ++i)
00288     fpfh.computePointSPFHSignature (cloud, *normals, i, i, indices, hist_f1, hist_f2, hist_f3);
00289 
00290   EXPECT_NEAR (hist_f1 (0, 0), 0.757576, 1e-4);
00291   EXPECT_NEAR (hist_f1 (0, 1), 0.757576, 1e-4);
00292   EXPECT_NEAR (hist_f1 (0, 2), 4.54545,  1e-4);
00293   EXPECT_NEAR (hist_f1 (0, 3), 19.697,   1e-4);
00294   EXPECT_NEAR (hist_f1 (0, 4), 40.6566,  1e-4);
00295   EXPECT_NEAR (hist_f1 (0, 5), 21.4647,  1e-4);
00296   EXPECT_NEAR (hist_f1 (0, 6), 7.575759, 1e-4);
00297   EXPECT_NEAR (hist_f1 (0, 7), 0.000000, 1e-4);
00298   EXPECT_NEAR (hist_f1 (0, 8), 0.000000, 1e-4);
00299   EXPECT_NEAR (hist_f1 (0, 9), 0.50505,  1e-4);
00300   EXPECT_NEAR (hist_f1 (0, 10), 4.0404,  1e-4);
00301 
00302   EXPECT_NEAR (hist_f2 (0, 0), 0.757576, 1e-4);
00303   EXPECT_NEAR (hist_f2 (0, 1), 1.51515,  1e-4);
00304   EXPECT_NEAR (hist_f2 (0, 2), 6.31313,  1e-4);
00305   EXPECT_NEAR (hist_f2 (0, 3), 9.59596,  1e-4);
00306   EXPECT_NEAR (hist_f2 (0, 4), 20.7071,  1e-4);
00307   EXPECT_NEAR (hist_f2 (0, 5), 18.9394,  1e-4);
00308   EXPECT_NEAR (hist_f2 (0, 6), 15.9091,  1e-4);
00309   EXPECT_NEAR (hist_f2 (0, 7), 12.8788,  1e-4);
00310   EXPECT_NEAR (hist_f2 (0, 8), 6.56566,  1e-4);
00311   EXPECT_NEAR (hist_f2 (0, 9), 4.29293,  1e-4);
00312   EXPECT_NEAR (hist_f2 (0, 10), 2.52525, 1e-4);
00313 
00314   EXPECT_NEAR (hist_f3 (0, 0), 0.000000, 1e-4);
00315   EXPECT_NEAR (hist_f3 (0, 1), 5.05051,  1e-4);
00316   EXPECT_NEAR (hist_f3 (0, 2), 4.54545,  1e-4);
00317   EXPECT_NEAR (hist_f3 (0, 3), 5.05051,  1e-4);
00318   EXPECT_NEAR (hist_f3 (0, 4), 1.76768,  1e-4);
00319   EXPECT_NEAR (hist_f3 (0, 5), 3.0303,   1e-4);
00320   EXPECT_NEAR (hist_f3 (0, 6), 9.09091,  1e-4);
00321   EXPECT_NEAR (hist_f3 (0, 7), 31.8182,  1e-4);
00322   EXPECT_NEAR (hist_f3 (0, 8), 22.2222,  1e-4);
00323   EXPECT_NEAR (hist_f3 (0, 9), 11.8687,  1e-4);
00324   EXPECT_NEAR (hist_f3 (0, 10), 5.55556, 1e-4);
00325 
00326   // weightPointSPFHSignature
00327   Eigen::VectorXf fpfh_histogram (nr_subdiv + nr_subdiv + nr_subdiv);
00328   fpfh_histogram.setZero ();
00329   vector<float> dists (indices.size ());
00330   for (size_t i = 0; i < dists.size (); ++i) dists[i] = static_cast<float> (i);
00331   fpfh.weightPointSPFHSignature (hist_f1, hist_f2, hist_f3, indices, dists, fpfh_histogram);
00332 
00333   EXPECT_NEAR (fpfh_histogram[0],  1.9798 ,  1e-2);
00334   EXPECT_NEAR (fpfh_histogram[1],  2.86927,  1e-2);
00335   EXPECT_NEAR (fpfh_histogram[2],  8.47911,  1e-2);
00336   EXPECT_NEAR (fpfh_histogram[3],  22.8784,  1e-2);
00337   EXPECT_NEAR (fpfh_histogram[4],  29.8597,  1e-2);
00338   EXPECT_NEAR (fpfh_histogram[5],  19.6877,  1e-2);
00339   EXPECT_NEAR (fpfh_histogram[6],  7.38611,  1e-2);
00340   EXPECT_NEAR (fpfh_histogram[7],  1.44265,  1e-2);
00341   EXPECT_NEAR (fpfh_histogram[8],  0.69677,  1e-2);
00342   EXPECT_NEAR (fpfh_histogram[9],  1.72609,  1e-2);
00343   EXPECT_NEAR (fpfh_histogram[10], 2.99435,  1e-2);
00344   EXPECT_NEAR (fpfh_histogram[11], 2.26313,  1e-2);
00345   EXPECT_NEAR (fpfh_histogram[12], 5.16573,  1e-2);
00346   EXPECT_NEAR (fpfh_histogram[13], 8.3263 ,  1e-2);
00347   EXPECT_NEAR (fpfh_histogram[14], 9.92427,  1e-2);
00348   EXPECT_NEAR (fpfh_histogram[15], 16.8062,  1e-2);
00349   EXPECT_NEAR (fpfh_histogram[16], 16.2767,  1e-2);
00350   EXPECT_NEAR (fpfh_histogram[17], 12.251 ,  1e-2);
00351   //EXPECT_NEAR (fpfh_histogram[18], 10.354,  1e-1);
00352   //EXPECT_NEAR (fpfh_histogram[19], 6.65578,  1e-2);
00353   EXPECT_NEAR (fpfh_histogram[20], 6.1437 ,  1e-2);
00354   EXPECT_NEAR (fpfh_histogram[21], 5.83341,  1e-2);
00355   EXPECT_NEAR (fpfh_histogram[22], 1.08809,  1e-2);
00356   EXPECT_NEAR (fpfh_histogram[23], 3.34133,  1e-2);
00357   EXPECT_NEAR (fpfh_histogram[24], 5.59236,  1e-2);
00358   EXPECT_NEAR (fpfh_histogram[25], 5.6355 ,  1e-2);
00359   EXPECT_NEAR (fpfh_histogram[26], 3.03257,  1e-2);
00360   EXPECT_NEAR (fpfh_histogram[27], 1.37437,  1e-2);
00361   EXPECT_NEAR (fpfh_histogram[28], 7.99746,  1e-2);
00362   EXPECT_NEAR (fpfh_histogram[29], 18.0343,  1e-2);
00363   EXPECT_NEAR (fpfh_histogram[30], 23.691 ,  1e-2);
00364   EXPECT_NEAR (fpfh_histogram[31], 19.8475,  1e-2);
00365   EXPECT_NEAR (fpfh_histogram[32], 10.3655,  1e-2);
00366 
00367   // Object
00368   PointCloud<FPFHSignature33>::Ptr fpfhs (new PointCloud<FPFHSignature33> ());
00369 
00370   // set parameters
00371   fpfh.setInputCloud (cloud.makeShared ());
00372   fpfh.setNrSubdivisions (11, 11, 11);
00373   fpfh.setIndices (indicesptr);
00374   fpfh.setSearchMethod (tree);
00375   fpfh.setKSearch (static_cast<int> (indices.size ()));
00376 
00377   // estimate
00378   fpfh.compute (*fpfhs);
00379   EXPECT_EQ (fpfhs->points.size (), indices.size ());
00380 
00381   EXPECT_NEAR (fpfhs->points[0].histogram[0],  1.58591, 1e-2);
00382   EXPECT_NEAR (fpfhs->points[0].histogram[1],  1.68365, 1e-2);
00383   EXPECT_NEAR (fpfhs->points[0].histogram[2],  6.71   , 1e-2);
00384   EXPECT_NEAR (fpfhs->points[0].histogram[3],  23.0717, 1e-2);
00385   EXPECT_NEAR (fpfhs->points[0].histogram[4],  33.3844, 1e-2);
00386   EXPECT_NEAR (fpfhs->points[0].histogram[5],  20.4002, 1e-2);
00387   EXPECT_NEAR (fpfhs->points[0].histogram[6],  7.31067, 1e-2);
00388   EXPECT_NEAR (fpfhs->points[0].histogram[7],  1.02635, 1e-2);
00389   EXPECT_NEAR (fpfhs->points[0].histogram[8],  0.48591, 1e-2);
00390   EXPECT_NEAR (fpfhs->points[0].histogram[9],  1.47069, 1e-2);
00391   EXPECT_NEAR (fpfhs->points[0].histogram[10], 2.87061, 1e-2);
00392   EXPECT_NEAR (fpfhs->points[0].histogram[11], 1.78321, 1e-2);
00393   EXPECT_NEAR (fpfhs->points[0].histogram[12], 4.30795, 1e-2);
00394   EXPECT_NEAR (fpfhs->points[0].histogram[13], 7.05514, 1e-2);
00395   EXPECT_NEAR (fpfhs->points[0].histogram[14], 9.37615, 1e-2);
00396   EXPECT_NEAR (fpfhs->points[0].histogram[15], 17.963 , 1e-2);
00397   EXPECT_NEAR (fpfhs->points[0].histogram[16], 18.2801, 1e-2);
00398   EXPECT_NEAR (fpfhs->points[0].histogram[17], 14.2766, 1e-2);
00399   //EXPECT_NEAR (fpfhs->points[0].histogram[18], 10.8542, 1e-2);
00400   //EXPECT_NEAR (fpfhs->points[0].histogram[19], 6.07925, 1e-2);
00401   EXPECT_NEAR (fpfhs->points[0].histogram[20], 5.28565, 1e-2);
00402   EXPECT_NEAR (fpfhs->points[0].histogram[21], 4.73887, 1e-2);
00403   EXPECT_NEAR (fpfhs->points[0].histogram[22], 0.56984, 1e-2);
00404   EXPECT_NEAR (fpfhs->points[0].histogram[23], 3.29826, 1e-2);
00405   EXPECT_NEAR (fpfhs->points[0].histogram[24], 5.28156, 1e-2);
00406   EXPECT_NEAR (fpfhs->points[0].histogram[25], 5.26939, 1e-2);
00407   EXPECT_NEAR (fpfhs->points[0].histogram[26], 3.13191, 1e-2);
00408   EXPECT_NEAR (fpfhs->points[0].histogram[27], 1.74453, 1e-2);
00409   EXPECT_NEAR (fpfhs->points[0].histogram[28], 9.41971, 1e-2);
00410   EXPECT_NEAR (fpfhs->points[0].histogram[29], 21.5894, 1e-2);
00411   EXPECT_NEAR (fpfhs->points[0].histogram[30], 24.6302, 1e-2);
00412   EXPECT_NEAR (fpfhs->points[0].histogram[31], 17.7764, 1e-2);
00413   EXPECT_NEAR (fpfhs->points[0].histogram[32], 7.28878, 1e-2);
00414 
00415   // Test results when setIndices and/or setSearchSurface are used
00416 
00417   boost::shared_ptr<vector<int> > test_indices (new vector<int> (0));
00418   for (size_t i = 0; i < cloud.size (); i+=3)
00419     test_indices->push_back (static_cast<int> (i));
00420 
00421   testIndicesAndSearchSurface<FPFHEstimation<PointXYZ, Normal, FPFHSignature33>, PointXYZ, Normal, FPFHSignature33>
00422   (cloud.makeShared (), normals, test_indices, 33);
00423 
00424 }
00425 
00427 TEST (PCL, FPFHEstimationOpenMP)
00428 {
00429   // Estimate normals first
00430   NormalEstimation<PointXYZ, Normal> n;
00431   PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
00432   // set parameters
00433   n.setInputCloud (cloud.makeShared ());
00434   boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
00435   n.setIndices (indicesptr);
00436   n.setSearchMethod (tree);
00437   n.setKSearch (10); // Use 10 nearest neighbors to estimate the normals
00438   // estimate
00439   n.compute (*normals);
00440   FPFHEstimationOMP<PointXYZ, Normal, FPFHSignature33> fpfh (4); // instantiate 4 threads
00441   fpfh.setInputNormals (normals);
00442 
00443   // Object
00444   PointCloud<FPFHSignature33>::Ptr fpfhs (new PointCloud<FPFHSignature33> ());
00445 
00446   // set parameters
00447   fpfh.setInputCloud (cloud.makeShared ());
00448   fpfh.setNrSubdivisions (11, 11, 11);
00449   fpfh.setIndices (indicesptr);
00450   fpfh.setSearchMethod (tree);
00451   fpfh.setKSearch (static_cast<int> (indices.size ()));
00452 
00453   // estimate
00454   fpfh.compute (*fpfhs);
00455   EXPECT_EQ (fpfhs->points.size (), indices.size ());
00456 
00457   EXPECT_NEAR (fpfhs->points[0].histogram[0],  1.58591, 1e-3);
00458   EXPECT_NEAR (fpfhs->points[0].histogram[1],  1.68365, 1e-2);
00459   EXPECT_NEAR (fpfhs->points[0].histogram[2],  6.71   , 1e-3);
00460   EXPECT_NEAR (fpfhs->points[0].histogram[3],  23.073, 1e-3);
00461   EXPECT_NEAR (fpfhs->points[0].histogram[4],  33.3828, 1e-2);
00462   EXPECT_NEAR (fpfhs->points[0].histogram[5],  20.4002, 1e-3);
00463   EXPECT_NEAR (fpfhs->points[0].histogram[6],  7.31067, 1e-3);
00464   EXPECT_NEAR (fpfhs->points[0].histogram[7],  1.02635, 1e-3);
00465   EXPECT_NEAR (fpfhs->points[0].histogram[8],  0.48591, 1e-3);
00466   EXPECT_NEAR (fpfhs->points[0].histogram[9],  1.47069, 1e-2);
00467   EXPECT_NEAR (fpfhs->points[0].histogram[10], 2.87061, 1e-3);
00468   EXPECT_NEAR (fpfhs->points[0].histogram[11], 1.78321, 1e-3);
00469   EXPECT_NEAR (fpfhs->points[0].histogram[12], 4.30795, 1e-3);
00470   EXPECT_NEAR (fpfhs->points[0].histogram[13], 7.05514, 1e-3);
00471   EXPECT_NEAR (fpfhs->points[0].histogram[14], 9.37615, 1e-3);
00472   EXPECT_NEAR (fpfhs->points[0].histogram[15], 17.963 , 1e-3);
00473   //EXPECT_NEAR (fpfhs->points[0].histogram[16], 18.2801, 1e-3);
00474   //EXPECT_NEAR (fpfhs->points[0].histogram[17], 14.2766, 1e-3);
00475   //EXPECT_NEAR (fpfhs->points[0].histogram[18], 10.8542, 1e-3);
00476   //EXPECT_NEAR (fpfhs->points[0].histogram[19], 6.07925, 1e-3);
00477   EXPECT_NEAR (fpfhs->points[0].histogram[20], 5.28991, 1e-3);
00478   EXPECT_NEAR (fpfhs->points[0].histogram[21], 4.73438, 1e-3);
00479   EXPECT_NEAR (fpfhs->points[0].histogram[22], 0.56984, 1e-3);
00480   EXPECT_NEAR (fpfhs->points[0].histogram[23], 3.29826, 1e-3);
00481   EXPECT_NEAR (fpfhs->points[0].histogram[24], 5.28156, 1e-3);
00482   EXPECT_NEAR (fpfhs->points[0].histogram[25], 5.26939, 1e-2);
00483   EXPECT_NEAR (fpfhs->points[0].histogram[26], 3.13191, 1e-3);
00484   EXPECT_NEAR (fpfhs->points[0].histogram[27], 1.74453, 1e-3);
00485   EXPECT_NEAR (fpfhs->points[0].histogram[28], 9.41971, 1e-3);
00486   EXPECT_NEAR (fpfhs->points[0].histogram[29], 21.5894, 1e-2);
00487   EXPECT_NEAR (fpfhs->points[0].histogram[30], 24.6302, 1e-3);
00488   EXPECT_NEAR (fpfhs->points[0].histogram[31], 17.7764, 1e-3);
00489   EXPECT_NEAR (fpfhs->points[0].histogram[32], 7.28878, 1e-3);
00490 
00491   // Test results when setIndices and/or setSearchSurface are used
00492 
00493   boost::shared_ptr<vector<int> > test_indices (new vector<int> (0));
00494   for (size_t i = 0; i < cloud.size (); i+=3)
00495     test_indices->push_back (static_cast<int> (i));
00496 
00497   testIndicesAndSearchSurface<FPFHEstimationOMP<PointXYZ, Normal, FPFHSignature33>, PointXYZ, Normal, FPFHSignature33>
00498   (cloud.makeShared (), normals, test_indices, 33);
00499 }
00500 
00502 TEST (PCL, VFHEstimation)
00503 {
00504   // Estimate normals first
00505   NormalEstimation<PointXYZ, Normal> n;
00506   PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
00507   // set parameters
00508   n.setInputCloud (cloud.makeShared ());
00509   boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
00510   n.setIndices (indicesptr);
00511   n.setSearchMethod (tree);
00512   n.setKSearch (10); // Use 10 nearest neighbors to estimate the normals
00513   // estimate
00514   n.compute (*normals);
00515   VFHEstimation<PointXYZ, Normal, VFHSignature308> vfh;
00516   vfh.setInputNormals (normals);
00517 
00518   //  PointCloud<PointNormal> cloud_normals;
00519   //  concatenateFields (cloud, normals, cloud_normals);
00520   //  savePCDFile ("bun0_n.pcd", cloud_normals);
00521 
00522   // Object
00523   PointCloud<VFHSignature308>::Ptr vfhs (new PointCloud<VFHSignature308> ());
00524 
00525   // set parameters
00526   vfh.setInputCloud (cloud.makeShared ());
00527   vfh.setIndices (indicesptr);
00528   vfh.setSearchMethod (tree);
00529 
00530   // estimate
00531   vfh.compute (*vfhs);
00532   EXPECT_EQ (int (vfhs->points.size ()), 1);
00533 
00534   //for (size_t d = 0; d < 308; ++d)
00535   //  std::cerr << vfhs.points[0].histogram[d] << std::endl;
00536 }
00537 
00538 #ifndef PCL_ONLY_CORE_POINT_TYPES
00539 
00540   template <typename FeatureEstimation, typename PointT, typename NormalT> void
00541   testIndicesAndSearchSurfaceEigen (const typename PointCloud<PointT>::Ptr & points,
00542                                     const typename PointCloud<NormalT>::Ptr & normals,
00543                                     const boost::shared_ptr<vector<int> > & indices, int ndims)
00544   {
00545     //
00546     // Test setIndices and setSearchSurface
00547     //
00548     PointCloud<Eigen::MatrixXf> full_output, output0, output1, output2;
00549 
00550     // Compute for all points and then subsample the results
00551     FeatureEstimation est0;
00552     est0.setSearchMethod (typename search::KdTree<PointT>::Ptr (new search::KdTree<PointT>));
00553     est0.setKSearch (10);
00554     est0.setInputCloud (points);
00555     est0.setInputNormals (normals);
00556     est0.computeEigen (full_output);
00557     output0 = PointCloud<Eigen::MatrixXf> (full_output, *indices);
00558     //copyPointCloud (full_output, *indices, output0);
00559 
00560     // Compute with all points as "search surface" and the specified sub-cloud as "input"
00561     typename PointCloud<PointT>::Ptr subpoints (new PointCloud<PointT>);
00562     copyPointCloud (*points, *indices, *subpoints);
00563     FeatureEstimation est1;
00564     est1.setSearchMethod (typename search::KdTree<PointT>::Ptr (new search::KdTree<PointT>));
00565     est1.setKSearch (10);
00566     est1.setInputCloud (subpoints);
00567     est1.setSearchSurface (points);
00568     est1.setInputNormals (normals);
00569     est1.computeEigen (output1);
00570 
00571     // Compute with all points as "input" and the specified indices
00572     FeatureEstimation est2;
00573     est2.setSearchMethod (typename search::KdTree<PointT>::Ptr (new search::KdTree<PointT>));
00574     est2.setKSearch (10);
00575     est2.setInputCloud (points);
00576     est2.setInputNormals (normals);
00577     est2.setIndices (indices);
00578     est2.computeEigen (output2);
00579 
00580     // All three of the above cases should produce equivalent results
00581     ASSERT_EQ (output0.points.rows (), output1.points.rows ());
00582     ASSERT_EQ (output1.points.rows (), output2.points.rows ());
00583     for (int i = 0; i < output1.points.rows (); ++i)
00584     {
00585       for (int j = 0; j < ndims; ++j)
00586       {
00587         ASSERT_EQ (output0.points (i, j), output1.points (i, j));
00588         ASSERT_EQ (output1.points (i, j), output2.points (i, j));
00589       }
00590     }
00591 
00592     //
00593     // Test the combination of setIndices and setSearchSurface
00594     //
00595     PointCloud<Eigen::MatrixXf> output3, output4;
00596 
00597     boost::shared_ptr<vector<int> > indices2 (new vector<int> (0));
00598     for (size_t i = 0; i < (indices->size ()/2); ++i)
00599       indices2->push_back (static_cast<int> (i));
00600 
00601     // Compute with all points as search surface + the specified sub-cloud as "input" but for only a subset of indices
00602     FeatureEstimation est3;
00603     est3.setSearchMethod (typename search::KdTree<PointT>::Ptr (new search::KdTree<PointT>));
00604     est3.setKSearch (10);
00605     est3.setSearchSurface (points);
00606     est3.setInputNormals (normals);
00607     est3.setInputCloud (subpoints);
00608     est3.setIndices (indices2);
00609     est3.computeEigen (output3);
00610 
00611     // Start with features for each point in "subpoints" and then subsample the results
00612     output4 = PointCloud<Eigen::MatrixXf> (output0, *indices2); // (Re-using "output0" from above)
00613     //copyPointCloud (output0, *indices2, output4);
00614 
00615     // The two cases above should produce equivalent results
00616     ASSERT_EQ (output3.points.rows (), output4.points.rows ());
00617     for (int i = 0; i < output3.points.rows (); ++i)
00618     {
00619       for (int j = 0; j < ndims; ++j)
00620       {
00621         ASSERT_EQ (output3.points (i, j), output4.points (i, j));
00622       }
00623     }
00624   }
00625 
00627   TEST (PCL, PFHEstimationEigen)
00628   {
00629     float f1, f2, f3, f4;
00630 
00631     // Estimate normals first
00632     NormalEstimation<PointXYZ, Normal> n;
00633     PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
00634     // set parameters
00635     n.setInputCloud (cloud.makeShared ());
00636     boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
00637     n.setIndices (indicesptr);
00638     n.setSearchMethod (tree);
00639     n.setKSearch (10); // Use 10 nearest neighbors to estimate the normals
00640     // estimate
00641     n.compute (*normals);
00642 
00643     PFHEstimation<PointXYZ, Normal, Eigen::MatrixXf> pfh;
00644     pfh.setInputNormals (normals);
00645     EXPECT_EQ (pfh.getInputNormals (), normals);
00646 
00647     // computePairFeatures
00648     pfh.computePairFeatures (cloud, *normals, 0, 12, f1, f2, f3, f4);
00649     EXPECT_NEAR (f1, -0.072575, 1e-4);
00650     EXPECT_NEAR (f2, -0.040221, 1e-4);
00651     EXPECT_NEAR (f3, 0.068133, 1e-4);
00652     EXPECT_NEAR (f4, 0.006130, 1e-4);
00653 
00654     // computePointPFHSignature
00655     int nr_subdiv = 3;
00656     Eigen::VectorXf pfh_histogram (nr_subdiv * nr_subdiv * nr_subdiv);
00657     pfh.computePointPFHSignature (cloud, *normals, indices, nr_subdiv, pfh_histogram);
00658     EXPECT_NEAR (pfh_histogram[0],  0.932506, 1e-2);
00659     EXPECT_NEAR (pfh_histogram[1],  2.32429 , 1e-2);
00660     EXPECT_NEAR (pfh_histogram[2],  0.357477, 1e-2);
00661     EXPECT_NEAR (pfh_histogram[3],  0.848541, 1e-2);
00662     EXPECT_NEAR (pfh_histogram[4],  3.65565 , 2e-2); // larger error w.r.t. considering all point pairs (feature bins=0,1,1 where 1 is middle, so angle of 0)
00663     EXPECT_NEAR (pfh_histogram[5],  0.178104, 1e-2);
00664     EXPECT_NEAR (pfh_histogram[6],  1.45284 , 1e-2);
00665     EXPECT_NEAR (pfh_histogram[7],  3.60666 , 1e-2);
00666     EXPECT_NEAR (pfh_histogram[8],  0.298959, 1e-2);
00667     EXPECT_NEAR (pfh_histogram[9],  0.295143, 1e-2);
00668     EXPECT_NEAR (pfh_histogram[10], 2.13474 , 1e-2);
00669     EXPECT_NEAR (pfh_histogram[11], 0.41218 , 1e-2);
00670     EXPECT_NEAR (pfh_histogram[12], 0.165382, 1e-2);
00671     EXPECT_NEAR (pfh_histogram[13], 8.97407 , 1e-2);
00672     EXPECT_NEAR (pfh_histogram[14], 0.306592, 1e-2);
00673     EXPECT_NEAR (pfh_histogram[15], 0.455432, 1e-2);
00674     EXPECT_NEAR (pfh_histogram[16], 4.5977 ,  1e-2);
00675     EXPECT_NEAR (pfh_histogram[17], 0.393097, 1e-2);
00676     EXPECT_NEAR (pfh_histogram[18], 7.54668 , 1e-2);
00677     EXPECT_NEAR (pfh_histogram[19], 6.78336 , 1e-2);
00678     EXPECT_NEAR (pfh_histogram[20], 1.63858 , 1e-2);
00679     EXPECT_NEAR (pfh_histogram[21], 9.93842 , 1e-2);
00680     EXPECT_NEAR (pfh_histogram[22], 18.4947 , 2e-2); // larger error w.r.t. considering all point pairs (feature bins=2,1,1 where 1 is middle, so angle of 0)
00681     EXPECT_NEAR (pfh_histogram[23], 1.96553 , 1e-4);
00682     EXPECT_NEAR (pfh_histogram[24], 8.04793 , 1e-4);
00683     EXPECT_NEAR (pfh_histogram[25], 11.2793  , 1e-4);
00684     EXPECT_NEAR (pfh_histogram[26], 2.91714 , 1e-4);
00685 
00686     // Sum of values should be 100
00687     EXPECT_NEAR (pfh_histogram.sum (), 100.0, 1e-2);
00688 
00689     // Object
00690     PointCloud<Eigen::MatrixXf>::Ptr pfhs (new PointCloud<Eigen::MatrixXf>);
00691 
00692     // set parameters
00693     pfh.setInputCloud (cloud.makeShared ());
00694     pfh.setIndices (indicesptr);
00695     pfh.setSearchMethod (tree);
00696     pfh.setKSearch (static_cast<int> (indices.size ()));
00697 
00698     // estimate
00699     pfh.computeEigen (*pfhs);
00700     EXPECT_EQ (pfhs->points.rows (), indices.size ());
00701 
00702     for (int i = 0; i < pfhs->points.rows (); ++i)
00703     {
00704       EXPECT_NEAR (pfhs->points (i, 0),  0.156477  , 1e-4);
00705       EXPECT_NEAR (pfhs->points (i, 1),  0.539396  , 1e-4);
00706       EXPECT_NEAR (pfhs->points (i, 2),  0.410907  , 1e-4);
00707       EXPECT_NEAR (pfhs->points (i, 3),  0.184465  , 1e-4);
00708       EXPECT_NEAR (pfhs->points (i, 4),  0.115767  , 1e-4);
00709       EXPECT_NEAR (pfhs->points (i, 5),  0.0572475 , 1e-4);
00710       EXPECT_NEAR (pfhs->points (i, 6),  0.206092  , 1e-4);
00711       EXPECT_NEAR (pfhs->points (i, 7),  0.339667  , 1e-4);
00712       EXPECT_NEAR (pfhs->points (i, 8),  0.265883  , 1e-4);
00713       EXPECT_NEAR (pfhs->points (i, 9),  0.0038165 , 1e-4);
00714       EXPECT_NEAR (pfhs->points (i, 10), 0.103046  , 1e-4);
00715       EXPECT_NEAR (pfhs->points (i, 11), 0.214997  , 1e-4);
00716       EXPECT_NEAR (pfhs->points (i, 12), 0.398186  , 3e-2); // larger error w.r.t. considering all point pairs (feature bins=0,2,2 where 2 is middle, so angle of 0)
00717       EXPECT_NEAR (pfhs->points (i, 13), 0.298959  , 1e-4);
00718       EXPECT_NEAR (pfhs->points (i, 14), 0.00127217, 1e-4);
00719       EXPECT_NEAR (pfhs->points (i, 15), 0.11704   , 1e-4);
00720       EXPECT_NEAR (pfhs->points (i, 16), 0.255706  , 1e-4);
00721       EXPECT_NEAR (pfhs->points (i, 17), 0.356205  , 1e-4);
00722       EXPECT_NEAR (pfhs->points (i, 18), 0.265883  , 1e-4);
00723       EXPECT_NEAR (pfhs->points (i, 19), 0.00127217, 1e-4);
00724       EXPECT_NEAR (pfhs->points (i, 20), 0.148844  , 1e-4);
00725       EXPECT_NEAR (pfhs->points (i, 21), 0.721316  , 1e-2);
00726       EXPECT_NEAR (pfhs->points (i, 22), 0.438899  , 1e-2);
00727       EXPECT_NEAR (pfhs->points (i, 23), 0.22263   , 1e-4);
00728       EXPECT_NEAR (pfhs->points (i, 24), 0.0216269 , 1e-4);
00729       EXPECT_NEAR (pfhs->points (i, 25), 0.223902  , 1e-4);
00730       EXPECT_NEAR (pfhs->points (i, 26), 0.07633   , 1e-4);
00731     }
00732 
00733 
00734     // Test results when setIndices and/or setSearchSurface are used
00735 
00736     boost::shared_ptr<vector<int> > test_indices (new vector<int> (0));
00737     for (size_t i = 0; i < cloud.size (); i+=3)
00738       test_indices->push_back (static_cast<int> (i));
00739 
00740     testIndicesAndSearchSurfaceEigen<PFHEstimation<PointXYZ, Normal, Eigen::MatrixXf>, PointXYZ, Normal>
00741     (cloud.makeShared (), normals, test_indices, 125);
00742 
00743   }
00744 
00746   TEST (PCL, FPFHEstimationEigen)
00747   {
00748     // Estimate normals first
00749     NormalEstimation<PointXYZ, Normal> n;
00750     PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
00751     // set parameters
00752     n.setInputCloud (cloud.makeShared ());
00753     boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
00754     n.setIndices (indicesptr);
00755     n.setSearchMethod (tree);
00756     n.setKSearch (10); // Use 10 nearest neighbors to estimate the normals
00757     // estimate
00758     n.compute (*normals);
00759 
00760     FPFHEstimation<PointXYZ, Normal, Eigen::MatrixXf> fpfh;
00761     fpfh.setInputNormals (normals);
00762     EXPECT_EQ (fpfh.getInputNormals (), normals);
00763 
00764     // computePointSPFHSignature
00765     int nr_subdiv = 11; // use the same number of bins for all three angular features
00766     Eigen::MatrixXf hist_f1 (indices.size (), nr_subdiv), hist_f2 (indices.size (), nr_subdiv), hist_f3 (indices.size (), nr_subdiv);
00767     hist_f1.setZero (); hist_f2.setZero (); hist_f3.setZero ();
00768     for (int i = 0; i < static_cast<int> (indices.size ()); ++i)
00769       fpfh.computePointSPFHSignature (cloud, *normals, i, i, indices, hist_f1, hist_f2, hist_f3);
00770 
00771     EXPECT_NEAR (hist_f1 (0, 0), 0.757576, 1e-4);
00772     EXPECT_NEAR (hist_f1 (0, 1), 0.757576, 1e-4);
00773     EXPECT_NEAR (hist_f1 (0, 2), 4.54545,  1e-4);
00774     EXPECT_NEAR (hist_f1 (0, 3), 19.697,   1e-4);
00775     EXPECT_NEAR (hist_f1 (0, 4), 40.6566,  1e-4);
00776     EXPECT_NEAR (hist_f1 (0, 5), 21.4647,  1e-4);
00777     EXPECT_NEAR (hist_f1 (0, 6), 7.575759, 1e-4);
00778     EXPECT_NEAR (hist_f1 (0, 7), 0.000000, 1e-4);
00779     EXPECT_NEAR (hist_f1 (0, 8), 0.000000, 1e-4);
00780     EXPECT_NEAR (hist_f1 (0, 9), 0.50505,  1e-4);
00781     EXPECT_NEAR (hist_f1 (0, 10), 4.0404,  1e-4);
00782 
00783     EXPECT_NEAR (hist_f2 (0, 0), 0.757576, 1e-4);
00784     EXPECT_NEAR (hist_f2 (0, 1), 1.51515,  1e-4);
00785     EXPECT_NEAR (hist_f2 (0, 2), 6.31313,  1e-4);
00786     EXPECT_NEAR (hist_f2 (0, 3), 9.59596,  1e-4);
00787     EXPECT_NEAR (hist_f2 (0, 4), 20.7071,  1e-4);
00788     EXPECT_NEAR (hist_f2 (0, 5), 18.9394,  1e-4);
00789     EXPECT_NEAR (hist_f2 (0, 6), 15.9091,  1e-4);
00790     EXPECT_NEAR (hist_f2 (0, 7), 12.8788,  1e-4);
00791     EXPECT_NEAR (hist_f2 (0, 8), 6.56566,  1e-4);
00792     EXPECT_NEAR (hist_f2 (0, 9), 4.29293,  1e-4);
00793     EXPECT_NEAR (hist_f2 (0, 10), 2.52525, 1e-4);
00794 
00795     EXPECT_NEAR (hist_f3 (0, 0), 0.000000, 1e-4);
00796     EXPECT_NEAR (hist_f3 (0, 1), 5.05051,  1e-4);
00797     EXPECT_NEAR (hist_f3 (0, 2), 4.54545,  1e-4);
00798     EXPECT_NEAR (hist_f3 (0, 3), 5.05051,  1e-4);
00799     EXPECT_NEAR (hist_f3 (0, 4), 1.76768,  1e-4);
00800     EXPECT_NEAR (hist_f3 (0, 5), 3.0303,   1e-4);
00801     EXPECT_NEAR (hist_f3 (0, 6), 9.09091,  1e-4);
00802     EXPECT_NEAR (hist_f3 (0, 7), 31.8182,  1e-4);
00803     EXPECT_NEAR (hist_f3 (0, 8), 22.2222,  1e-4);
00804     EXPECT_NEAR (hist_f3 (0, 9), 11.8687,  1e-4);
00805     EXPECT_NEAR (hist_f3 (0, 10), 5.55556, 1e-4);
00806 
00807     // weightPointSPFHSignature
00808     Eigen::VectorXf fpfh_histogram (nr_subdiv + nr_subdiv + nr_subdiv);
00809     fpfh_histogram.setZero ();
00810     vector<float> dists (indices.size ());
00811     for (size_t i = 0; i < dists.size (); ++i) dists[i] = static_cast<float> (i);
00812     fpfh.weightPointSPFHSignature (hist_f1, hist_f2, hist_f3, indices, dists, fpfh_histogram);
00813 
00814     EXPECT_NEAR (fpfh_histogram[0],  1.9798 ,  1e-2);
00815     EXPECT_NEAR (fpfh_histogram[1],  2.86927,  1e-2);
00816     EXPECT_NEAR (fpfh_histogram[2],  8.47911,  1e-2);
00817     EXPECT_NEAR (fpfh_histogram[3],  22.8784,  1e-2);
00818     EXPECT_NEAR (fpfh_histogram[4],  29.8597,  1e-2);
00819     EXPECT_NEAR (fpfh_histogram[5],  19.6877,  1e-2);
00820     EXPECT_NEAR (fpfh_histogram[6],  7.38611,  1e-2);
00821     EXPECT_NEAR (fpfh_histogram[7],  1.44265,  1e-2);
00822     EXPECT_NEAR (fpfh_histogram[8],  0.69677,  1e-2);
00823     EXPECT_NEAR (fpfh_histogram[9],  1.72609,  1e-2);
00824     EXPECT_NEAR (fpfh_histogram[10], 2.99435,  1e-2);
00825     EXPECT_NEAR (fpfh_histogram[11], 2.26313,  1e-2);
00826     EXPECT_NEAR (fpfh_histogram[12], 5.16573,  1e-2);
00827     EXPECT_NEAR (fpfh_histogram[13], 8.3263 ,  1e-2);
00828     EXPECT_NEAR (fpfh_histogram[14], 9.92427,  1e-2);
00829     EXPECT_NEAR (fpfh_histogram[15], 16.8062,  1e-2);
00830     EXPECT_NEAR (fpfh_histogram[16], 16.2767,  1e-2);
00831     EXPECT_NEAR (fpfh_histogram[17], 12.251 ,  1e-2);
00832     //EXPECT_NEAR (fpfh_histogram[18], 10.354,  1e-2);
00833     //EXPECT_NEAR (fpfh_histogram[19], 6.65578,  1e-2);
00834     EXPECT_NEAR (fpfh_histogram[20], 6.1437 ,  1e-2);
00835     EXPECT_NEAR (fpfh_histogram[21], 5.83341,  1e-2);
00836     EXPECT_NEAR (fpfh_histogram[22], 1.08809,  1e-2);
00837     EXPECT_NEAR (fpfh_histogram[23], 3.34133,  1e-2);
00838     EXPECT_NEAR (fpfh_histogram[24], 5.59236,  1e-2);
00839     EXPECT_NEAR (fpfh_histogram[25], 5.6355 ,  1e-2);
00840     EXPECT_NEAR (fpfh_histogram[26], 3.03257,  1e-2);
00841     EXPECT_NEAR (fpfh_histogram[27], 1.37437,  1e-2);
00842     EXPECT_NEAR (fpfh_histogram[28], 7.99746,  1e-2);
00843     EXPECT_NEAR (fpfh_histogram[29], 18.0343,  1e-2);
00844     EXPECT_NEAR (fpfh_histogram[30], 23.691 ,  1e-2);
00845     EXPECT_NEAR (fpfh_histogram[31], 19.8475,  1e-2);
00846     EXPECT_NEAR (fpfh_histogram[32], 10.3655,  1e-2);
00847 
00848     // Object
00849     PointCloud<Eigen::MatrixXf>::Ptr fpfhs (new PointCloud<Eigen::MatrixXf>);
00850 
00851     // set parameters
00852     fpfh.setInputCloud (cloud.makeShared ());
00853     fpfh.setNrSubdivisions (11, 11, 11);
00854     fpfh.setIndices (indicesptr);
00855     fpfh.setSearchMethod (tree);
00856     fpfh.setKSearch (static_cast<int> (indices.size ()));
00857 
00858     // estimate
00859     fpfh.computeEigen (*fpfhs);
00860     EXPECT_EQ (fpfhs->points.rows (), indices.size ());
00861 
00862     EXPECT_NEAR (fpfhs->points (0, 0),  1.58591, 1e-2);
00863     EXPECT_NEAR (fpfhs->points (0, 1),  1.68365, 1e-2);
00864     EXPECT_NEAR (fpfhs->points (0, 2),  6.71   , 1e-2);
00865     EXPECT_NEAR (fpfhs->points (0, 3),  23.0717, 1e-2);
00866     EXPECT_NEAR (fpfhs->points (0, 4),  33.3844, 1e-2);
00867     EXPECT_NEAR (fpfhs->points (0, 5),  20.4002, 1e-2);
00868     EXPECT_NEAR (fpfhs->points (0, 6),  7.31067, 1e-2);
00869     EXPECT_NEAR (fpfhs->points (0, 7),  1.02635, 1e-2);
00870     EXPECT_NEAR (fpfhs->points (0, 8),  0.48591, 1e-2);
00871     EXPECT_NEAR (fpfhs->points (0, 9),  1.47069, 1e-2);
00872     EXPECT_NEAR (fpfhs->points (0, 10), 2.87061, 1e-2);
00873     EXPECT_NEAR (fpfhs->points (0, 11), 1.78321, 1e-2);
00874     EXPECT_NEAR (fpfhs->points (0, 12), 4.30795, 1e-2);
00875     EXPECT_NEAR (fpfhs->points (0, 13), 7.05514, 1e-2);
00876     EXPECT_NEAR (fpfhs->points (0, 14), 9.37615, 1e-2);
00877     EXPECT_NEAR (fpfhs->points (0, 15), 17.963 , 1e-2);
00878     EXPECT_NEAR (fpfhs->points (0, 16), 18.2801, 1e-2);
00879     EXPECT_NEAR (fpfhs->points (0, 17), 14.2766, 1e-2);
00880     //EXPECT_NEAR (fpfhs->points (0, 18), 10.8542, 1e-2);
00881     //EXPECT_NEAR (fpfhs->points (0, 19), 6.07925, 1e-2);
00882     EXPECT_NEAR (fpfhs->points (0, 20), 5.28565, 1e-2);
00883     EXPECT_NEAR (fpfhs->points (0, 21), 4.73887, 1e-2);
00884     EXPECT_NEAR (fpfhs->points (0, 22), 0.56984, 1e-2);
00885     EXPECT_NEAR (fpfhs->points (0, 23), 3.29826, 1e-2);
00886     EXPECT_NEAR (fpfhs->points (0, 24), 5.28156, 1e-2);
00887     EXPECT_NEAR (fpfhs->points (0, 25), 5.26939, 1e-2);
00888     EXPECT_NEAR (fpfhs->points (0, 26), 3.13191, 1e-2);
00889     EXPECT_NEAR (fpfhs->points (0, 27), 1.74453, 1e-2);
00890     EXPECT_NEAR (fpfhs->points (0, 28), 9.41971, 1e-2);
00891     EXPECT_NEAR (fpfhs->points (0, 29), 21.5894, 1e-2);
00892     EXPECT_NEAR (fpfhs->points (0, 30), 24.6302, 1e-2);
00893     EXPECT_NEAR (fpfhs->points (0, 31), 17.7764, 1e-2);
00894     EXPECT_NEAR (fpfhs->points (0, 32), 7.28878, 1e-2);
00895 
00896     // Test results when setIndices and/or setSearchSurface are used
00897     boost::shared_ptr<vector<int> > test_indices (new vector<int> (0));
00898     for (size_t i = 0; i < cloud.size (); i+=3)
00899       test_indices->push_back (static_cast<int> (i));
00900 
00901     testIndicesAndSearchSurfaceEigen <FPFHEstimation<PointXYZ, Normal, Eigen::MatrixXf>, PointXYZ, Normal>
00902     (cloud.makeShared (), normals, test_indices, 33);
00903   }
00904 #endif
00905 
00906 /* ---[ */
00907 int
00908 main (int argc, char** argv)
00909 {
00910   if (argc < 2)
00911   {
00912     std::cerr << "No test file given. Please download `bun0.pcd` and pass its path to the test." << std::endl;
00913     return (-1);
00914   }
00915 
00916   if (loadPCDFile<PointXYZ> (argv[1], cloud) < 0)
00917   {
00918     std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl;
00919     return (-1);
00920   }
00921 
00922   indices.resize (cloud.points.size ());
00923   for (size_t i = 0; i < indices.size (); ++i)
00924     indices[i] = static_cast<int> (i);
00925 
00926   tree.reset (new search::KdTree<PointXYZ> (false));
00927   tree->setInputCloud (cloud.makeShared ());
00928 
00929   testing::InitGoogleTest (&argc, argv);
00930   return (RUN_ALL_TESTS ());
00931 }
00932 /* ]--- */

pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:18:26