test_spin_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 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 
00040 #include <gtest/gtest.h>
00041 #include <pcl/point_cloud.h>
00042 #include <pcl/features/normal_3d.h>
00043 #include <pcl/io/pcd_io.h>
00044 #include <pcl/features/spin_image.h>
00045 #include <pcl/features/intensity_spin.h>
00046 
00047 using namespace pcl;
00048 using namespace pcl::io;
00049 using namespace std;
00050 
00051 typedef search::KdTree<PointXYZ>::Ptr KdTreePtr;
00052 
00053 PointCloud<PointXYZ> cloud;
00054 vector<int> indices;
00055 KdTreePtr tree;
00056 
00058 TEST (PCL, SpinImageEstimation)
00059 {
00060   // Estimate normals first
00061   double mr = 0.002;
00062   NormalEstimation<PointXYZ, Normal> n;
00063   PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
00064   // set parameters
00065   n.setInputCloud (cloud.makeShared ());
00066   boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
00067   n.setIndices (indicesptr);
00068   n.setSearchMethod (tree);
00069   n.setRadiusSearch (20 * mr);
00070   n.compute (*normals);
00071 
00072   EXPECT_NEAR (normals->points[103].normal_x, 0.36683175, 1e-4);
00073   EXPECT_NEAR (normals->points[103].normal_y, -0.44696972, 1e-4);
00074   EXPECT_NEAR (normals->points[103].normal_z, -0.81587529, 1e-4);
00075   EXPECT_NEAR (normals->points[200].normal_x, -0.71414840, 1e-4);
00076   EXPECT_NEAR (normals->points[200].normal_y, -0.06002361, 1e-4);
00077   EXPECT_NEAR (normals->points[200].normal_z, -0.69741613, 1e-4);
00078 
00079   EXPECT_NEAR (normals->points[140].normal_x, -0.45109111, 1e-4);
00080   EXPECT_NEAR (normals->points[140].normal_y, -0.19499126, 1e-4);
00081   EXPECT_NEAR (normals->points[140].normal_z, -0.87091631, 1e-4);
00082 
00083   typedef Histogram<153> SpinImage;
00084   SpinImageEstimation<PointXYZ, Normal, SpinImage> spin_est(8, 0.5, 16);
00085   // set parameters
00086   //spin_est.setInputWithNormals (cloud.makeShared (), normals);
00087   spin_est.setInputCloud (cloud.makeShared ());
00088   spin_est.setInputNormals (normals);
00089   spin_est.setIndices (indicesptr);
00090   spin_est.setSearchMethod (tree);
00091   spin_est.setRadiusSearch (40*mr);
00092 
00093   // Object
00094   PointCloud<SpinImage>::Ptr spin_images (new PointCloud<SpinImage> ());
00095 
00096 
00097   // radial SI
00098   spin_est.setRadialStructure();
00099 
00100   // estimate
00101   spin_est.compute (*spin_images);
00102   EXPECT_EQ (spin_images->points.size (), indices.size ());
00103 
00104   EXPECT_NEAR (spin_images->points[100].histogram[0], 0, 1e-4);
00105   EXPECT_NEAR (spin_images->points[100].histogram[12], 0, 1e-4);
00106   EXPECT_NEAR (spin_images->points[100].histogram[24], 0.00233226, 1e-4);
00107   EXPECT_NEAR (spin_images->points[100].histogram[36], 0, 1e-4);
00108   EXPECT_NEAR (spin_images->points[100].histogram[48], 8.48662e-005, 1e-4);
00109   EXPECT_NEAR (spin_images->points[100].histogram[60], 0.0266387, 1e-4);
00110   EXPECT_NEAR (spin_images->points[100].histogram[72], 0, 1e-4);
00111   EXPECT_NEAR (spin_images->points[100].histogram[84], 0, 1e-4);
00112   EXPECT_NEAR (spin_images->points[100].histogram[96], 0.0414662, 1e-4);
00113   EXPECT_NEAR (spin_images->points[100].histogram[108], 0, 1e-4);
00114   EXPECT_NEAR (spin_images->points[100].histogram[120], 0, 1e-4);
00115   EXPECT_NEAR (spin_images->points[100].histogram[132], 0, 1e-4);
00116   EXPECT_NEAR (spin_images->points[100].histogram[144], 0.0128513, 1e-4);
00117   EXPECT_NEAR (spin_images->points[300].histogram[0], 0, 1e-4);
00118   EXPECT_NEAR (spin_images->points[300].histogram[12], 0, 1e-4);
00119   EXPECT_NEAR (spin_images->points[300].histogram[24], 0.00932424, 1e-4);
00120   EXPECT_NEAR (spin_images->points[300].histogram[36], 0, 1e-4);
00121   EXPECT_NEAR (spin_images->points[300].histogram[48], 0, 1e-4);
00122   EXPECT_NEAR (spin_images->points[300].histogram[60], 0.0145733, 1e-4);
00123   EXPECT_NEAR (spin_images->points[300].histogram[72], 0, 1e-4);
00124   EXPECT_NEAR (spin_images->points[300].histogram[84], 0, 1e-4);
00125   EXPECT_NEAR (spin_images->points[300].histogram[96], 0.00034457, 1e-4);
00126   EXPECT_NEAR (spin_images->points[300].histogram[108], 0, 1e-4);
00127   EXPECT_NEAR (spin_images->points[300].histogram[120], 0, 1e-4);
00128   EXPECT_NEAR (spin_images->points[300].histogram[132], 0, 1e-4);
00129   EXPECT_NEAR (spin_images->points[300].histogram[144], 0.0121195, 1e-4);
00130 
00131   // radial SI, angular spin-images
00132   spin_est.setAngularDomain ();
00133 
00134   // estimate
00135   spin_est.compute (*spin_images);
00136   EXPECT_EQ (spin_images->points.size (), indices.size ());
00137 
00138   EXPECT_NEAR (spin_images->points[100].histogram[0], 0, 1e-4);
00139   EXPECT_NEAR (spin_images->points[100].histogram[12], 0, 1e-4);
00140   EXPECT_NEAR (spin_images->points[100].histogram[24], 0.132139, 1e-4);
00141   EXPECT_NEAR (spin_images->points[100].histogram[36], 0, 1e-4);
00142   EXPECT_NEAR (spin_images->points[100].histogram[48], 0.908814, 1e-4);
00143   EXPECT_NEAR (spin_images->points[100].histogram[60], 0.63875, 1e-4);
00144   EXPECT_NEAR (spin_images->points[100].histogram[72], 0, 1e-4);
00145   EXPECT_NEAR (spin_images->points[100].histogram[84], 0, 1e-4);
00146   EXPECT_NEAR (spin_images->points[100].histogram[96], 0.550392, 1e-4);
00147   EXPECT_NEAR (spin_images->points[100].histogram[108], 0, 1e-4);
00148   EXPECT_NEAR (spin_images->points[100].histogram[120], 0, 1e-4);
00149   EXPECT_NEAR (spin_images->points[100].histogram[132], 0, 1e-4);
00150   EXPECT_NEAR (spin_images->points[100].histogram[144], 0.257136, 1e-4);
00151   EXPECT_NEAR (spin_images->points[300].histogram[0], 0, 1e-4);
00152   EXPECT_NEAR (spin_images->points[300].histogram[12], 0, 1e-4);
00153   EXPECT_NEAR (spin_images->points[300].histogram[24], 0.230605, 1e-4);
00154   EXPECT_NEAR (spin_images->points[300].histogram[36], 0, 1e-4);
00155   EXPECT_NEAR (spin_images->points[300].histogram[48], 0, 1e-4);
00156   EXPECT_NEAR (spin_images->points[300].histogram[60], 0.764872, 1e-4);
00157   EXPECT_NEAR (spin_images->points[300].histogram[72], 0, 1e-4);
00158   EXPECT_NEAR (spin_images->points[300].histogram[84], 0, 1e-4);
00159   EXPECT_NEAR (spin_images->points[300].histogram[96], 1.02824, 1e-4);
00160   EXPECT_NEAR (spin_images->points[300].histogram[108], 0, 1e-4);
00161   EXPECT_NEAR (spin_images->points[300].histogram[120], 0, 1e-4);
00162   EXPECT_NEAR (spin_images->points[300].histogram[132], 0, 1e-4);
00163   EXPECT_NEAR (spin_images->points[300].histogram[144], 0.293567, 1e-4);
00164 
00165   // rectangular SI
00166   spin_est.setRadialStructure (false);
00167   spin_est.setAngularDomain (false);
00168 
00169   // estimate
00170   spin_est.compute (*spin_images);
00171   EXPECT_EQ (spin_images->points.size (), indices.size ());
00172 
00173   EXPECT_NEAR (spin_images->points[100].histogram[0], 0, 1e-4);
00174   EXPECT_NEAR (spin_images->points[100].histogram[12], 0, 1e-4);
00175   EXPECT_NEAR (spin_images->points[100].histogram[24], 0.000889345, 1e-4);
00176   EXPECT_NEAR (spin_images->points[100].histogram[36], 0, 1e-4);
00177   EXPECT_NEAR (spin_images->points[100].histogram[48], 0, 1e-4);
00178   EXPECT_NEAR (spin_images->points[100].histogram[60], 0.0489534, 1e-4);
00179   EXPECT_NEAR (spin_images->points[100].histogram[72], 0, 1e-4);
00180   EXPECT_NEAR (spin_images->points[100].histogram[84], 0, 1e-4);
00181   EXPECT_NEAR (spin_images->points[100].histogram[96], 0.0747141, 1e-4);
00182   EXPECT_NEAR (spin_images->points[100].histogram[108], 0, 1e-4);
00183   EXPECT_NEAR (spin_images->points[100].histogram[120], 0, 1e-4);
00184   EXPECT_NEAR (spin_images->points[100].histogram[132], 0.0173423, 1e-4);
00185   EXPECT_NEAR (spin_images->points[100].histogram[144], 0, 1e-4);
00186   EXPECT_NEAR (spin_images->points[300].histogram[0], 0, 1e-4);
00187   EXPECT_NEAR (spin_images->points[300].histogram[12], 0, 1e-4);
00188   EXPECT_NEAR (spin_images->points[300].histogram[24], 0.0267132, 1e-4);
00189   EXPECT_NEAR (spin_images->points[300].histogram[36], 0, 1e-4);
00190   EXPECT_NEAR (spin_images->points[300].histogram[48], 0, 1e-4);
00191   EXPECT_NEAR (spin_images->points[300].histogram[60], 0, 1e-4);
00192   EXPECT_NEAR (spin_images->points[300].histogram[72], 0, 1e-4);
00193   EXPECT_NEAR (spin_images->points[300].histogram[84], 0, 1e-4);
00194   EXPECT_NEAR (spin_images->points[300].histogram[96], 0, 1e-4);
00195   EXPECT_NEAR (spin_images->points[300].histogram[108], 0.0209709, 1e-4);
00196   EXPECT_NEAR (spin_images->points[300].histogram[120], 0, 1e-4);
00197   EXPECT_NEAR (spin_images->points[300].histogram[132], 0, 1e-4);
00198   EXPECT_NEAR (spin_images->points[300].histogram[144], 0.029372, 1e-4);
00199 
00200   // rectangular SI, angular spin-images
00201   spin_est.setAngularDomain ();
00202 
00203   // estimate
00204   spin_est.compute (*spin_images);
00205   EXPECT_EQ (spin_images->points.size (), indices.size ());
00206 
00207   EXPECT_NEAR (spin_images->points[100].histogram[0], 0, 1e-4);
00208   EXPECT_NEAR (spin_images->points[100].histogram[12], 0, 1e-4);
00209   EXPECT_NEAR (spin_images->points[100].histogram[24], 0.132139, 1e-4);
00210   EXPECT_NEAR (spin_images->points[100].histogram[36], 0, 1e-4);
00211   EXPECT_NEAR (spin_images->points[100].histogram[48], 0, 1e-4);
00212   EXPECT_NEAR (spin_images->points[100].histogram[60], 0.38800787925720215, 1e-4);
00213   EXPECT_NEAR (spin_images->points[100].histogram[72], 0, 1e-4);
00214   EXPECT_NEAR (spin_images->points[100].histogram[84], 0, 1e-4);
00215   EXPECT_NEAR (spin_images->points[100].histogram[96], 0.468881, 1e-4);
00216   EXPECT_NEAR (spin_images->points[100].histogram[108], 0, 1e-4);
00217   EXPECT_NEAR (spin_images->points[100].histogram[120], 0, 1e-4);
00218   EXPECT_NEAR (spin_images->points[100].histogram[132], 0.67901438474655151, 1e-4);
00219   EXPECT_NEAR (spin_images->points[100].histogram[144], 0, 1e-4);
00220   EXPECT_NEAR (spin_images->points[300].histogram[0], 0, 1e-4);
00221   EXPECT_NEAR (spin_images->points[300].histogram[12], 0, 1e-4);
00222   EXPECT_NEAR (spin_images->points[300].histogram[24], 0.143845, 1e-4);
00223   EXPECT_NEAR (spin_images->points[300].histogram[36], 0, 1e-4);
00224   EXPECT_NEAR (spin_images->points[300].histogram[48], 0, 1e-4);
00225   EXPECT_NEAR (spin_images->points[300].histogram[60], 0, 1e-4);
00226   EXPECT_NEAR (spin_images->points[300].histogram[72], 0, 1e-4);
00227   EXPECT_NEAR (spin_images->points[300].histogram[84], 0, 1e-4);
00228   EXPECT_NEAR (spin_images->points[300].histogram[96], 0, 1e-4);
00229   EXPECT_NEAR (spin_images->points[300].histogram[108], 0.706084, 1e-4);
00230   EXPECT_NEAR (spin_images->points[300].histogram[120], 0, 1e-4);
00231   EXPECT_NEAR (spin_images->points[300].histogram[132], 0, 1e-4);
00232   EXPECT_NEAR (spin_images->points[300].histogram[144], 0.272542, 1e-4);
00233 }
00234 
00236 TEST (PCL, IntensitySpinEstimation)
00237 {
00238   // Generate a sample point cloud
00239   PointCloud<PointXYZI> cloud_xyzi;
00240   cloud_xyzi.height = 1;
00241   cloud_xyzi.is_dense = true;
00242 
00243   for (float x = -10.0f; x <= 10.0f; x += 1.0f)
00244   {
00245     for (float y = -10.0f; y <= 10.0f; y += 1.0f)
00246     {
00247       PointXYZI p;
00248       p.x = x;
00249       p.y = y;
00250       p.z = sqrtf (400.0f - x * x - y * y);
00251       p.intensity = expf (-(powf (x - 3.0f, 2.0f) + powf (y + 2.0f, 2.0f)) / (2.0f * 25.0f)) + expf (-(powf (x + 5.0f, 2.0f) + powf (y - 5.0f, 2.0f))
00252                                                                                  / (2.0f * 4.0f));
00253 
00254       cloud_xyzi.points.push_back (p);
00255     }
00256   }
00257   cloud_xyzi.width = static_cast<uint32_t> (cloud_xyzi.points.size ());
00258 
00259   // Compute the intensity-domain spin features
00260   typedef Histogram<20> IntensitySpin;
00261   IntensitySpinEstimation<PointXYZI, IntensitySpin> ispin_est;
00262   search::KdTree<PointXYZI>::Ptr treept3 (new search::KdTree<PointXYZI> (false));
00263   ispin_est.setSearchMethod (treept3);
00264   ispin_est.setRadiusSearch (10.0);
00265   ispin_est.setNrDistanceBins (4);
00266   ispin_est.setNrIntensityBins (5);
00267 
00268   ispin_est.setInputCloud (cloud_xyzi.makeShared ());
00269   PointCloud<IntensitySpin> ispin_output;
00270   ispin_est.compute (ispin_output);
00271 
00272   // Compare to independently verified values
00273   const IntensitySpin &ispin = ispin_output.points[220];
00274   const float correct_ispin_feature_values[20] = {2.4387f, 9.4737f, 21.3232f, 28.3025f, 22.5639f, 13.2426f, 35.7026f, 60.0755f,
00275                                                   66.9240f, 50.4225f, 42.7086f, 83.5818f, 105.4513f, 97.8454f, 67.3801f,
00276                                                   75.7127f, 119.4726f, 120.9649f, 93.4829f, 55.4045f};
00277   for (int i = 0; i < 20; ++i)
00278   {
00279     EXPECT_NEAR (ispin.histogram[i], correct_ispin_feature_values[i], 1e-4);
00280   }
00281 }
00282 
00283 /* ---[ */
00284 int
00285 main (int argc, char** argv)
00286 {
00287   if (argc < 2)
00288   {
00289     std::cerr << "No test file given. Please download `bun0.pcd` and pass its path to the test." << std::endl;
00290     return (-1);
00291   }
00292 
00293   if (loadPCDFile<PointXYZ> (argv[1], cloud) < 0)
00294   {
00295     std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl;
00296     return (-1);
00297   }
00298 
00299   indices.resize (cloud.points.size ());
00300   for (size_t i = 0; i < indices.size (); ++i)
00301     indices[i] = static_cast<int> (i);
00302 
00303   tree.reset (new search::KdTree<PointXYZ> (false));
00304   tree->setInputCloud (cloud.makeShared ());
00305 
00306   testing::InitGoogleTest (&argc, argv);
00307   return (RUN_ALL_TESTS ());
00308 }
00309 /* ]--- */


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