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 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/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.388018399477005, 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 #ifndef PCL_ONLY_CORE_POINT_TYPES
00284 
00285   TEST (PCL, SpinImageEstimationEigen)
00286   {
00287     // Estimate normals first
00288     double mr = 0.002;
00289     NormalEstimation<PointXYZ, Normal> n;
00290     PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
00291     // set parameters
00292     n.setInputCloud (cloud.makeShared ());
00293     boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
00294     n.setIndices (indicesptr);
00295     n.setSearchMethod (tree);
00296     n.setRadiusSearch (20 * mr);
00297     n.compute (*normals);
00298 
00299     EXPECT_NEAR (normals->points[103].normal_x, 0.36683175, 1e-4);
00300     EXPECT_NEAR (normals->points[103].normal_y, -0.44696972, 1e-4);
00301     EXPECT_NEAR (normals->points[103].normal_z, -0.81587529, 1e-4);
00302     EXPECT_NEAR (normals->points[200].normal_x, -0.71414840, 1e-4);
00303     EXPECT_NEAR (normals->points[200].normal_y, -0.06002361, 1e-4);
00304     EXPECT_NEAR (normals->points[200].normal_z, -0.69741613, 1e-4);
00305 
00306     EXPECT_NEAR (normals->points[140].normal_x, -0.45109111, 1e-4);
00307     EXPECT_NEAR (normals->points[140].normal_y, -0.19499126, 1e-4);
00308     EXPECT_NEAR (normals->points[140].normal_z, -0.87091631, 1e-4);
00309 
00310     SpinImageEstimation<PointXYZ, Normal, Eigen::MatrixXf> spin_est (8, 0.5, 16);
00311     // set parameters
00312     //spin_est.setInputWithNormals (cloud.makeShared (), normals);
00313     spin_est.setInputCloud (cloud.makeShared ());
00314     spin_est.setInputNormals (normals);
00315     spin_est.setIndices (indicesptr);
00316     spin_est.setSearchMethod (tree);
00317     spin_est.setRadiusSearch (40*mr);
00318 
00319     // Object
00320     PointCloud<Eigen::MatrixXf>::Ptr spin_images (new PointCloud<Eigen::MatrixXf>);
00321 
00322     // radial SI
00323     spin_est.setRadialStructure ();
00324 
00325     // estimate
00326     spin_est.computeEigen (*spin_images);
00327     EXPECT_EQ (spin_images->points.rows (), indices.size ());
00328 
00329     EXPECT_NEAR (spin_images->points (100, 0), 0, 1e-4);
00330     EXPECT_NEAR (spin_images->points (100, 12), 0, 1e-4);
00331     EXPECT_NEAR (spin_images->points (100, 24), 0.00233226, 1e-4);
00332     EXPECT_NEAR (spin_images->points (100, 36), 0, 1e-4);
00333     EXPECT_NEAR (spin_images->points (100, 48), 8.48662e-005, 1e-4);
00334     EXPECT_NEAR (spin_images->points (100, 60), 0.0266387, 1e-4);
00335     EXPECT_NEAR (spin_images->points (100, 72), 0, 1e-4);
00336     EXPECT_NEAR (spin_images->points (100, 84), 0, 1e-4);
00337     EXPECT_NEAR (spin_images->points (100, 96), 0.0414662, 1e-4);
00338     EXPECT_NEAR (spin_images->points (100, 108), 0, 1e-4);
00339     EXPECT_NEAR (spin_images->points (100, 120), 0, 1e-4);
00340     EXPECT_NEAR (spin_images->points (100, 132), 0, 1e-4);
00341     EXPECT_NEAR (spin_images->points (100, 144), 0.0128513, 1e-4);
00342     EXPECT_NEAR (spin_images->points (300, 0), 0, 1e-4);
00343     EXPECT_NEAR (spin_images->points (300, 12), 0, 1e-4);
00344     EXPECT_NEAR (spin_images->points (300, 24), 0.00932424, 1e-4);
00345     EXPECT_NEAR (spin_images->points (300, 36), 0, 1e-4);
00346     EXPECT_NEAR (spin_images->points (300, 48), 0, 1e-4);
00347     EXPECT_NEAR (spin_images->points (300, 60), 0.0145733, 1e-4);
00348     EXPECT_NEAR (spin_images->points (300, 72), 0, 1e-4);
00349     EXPECT_NEAR (spin_images->points (300, 84), 0, 1e-4);
00350     EXPECT_NEAR (spin_images->points (300, 96), 0.00034457, 1e-4);
00351     EXPECT_NEAR (spin_images->points (300, 108), 0, 1e-4);
00352     EXPECT_NEAR (spin_images->points (300, 120), 0, 1e-4);
00353     EXPECT_NEAR (spin_images->points (300, 132), 0, 1e-4);
00354     EXPECT_NEAR (spin_images->points (300, 144), 0.0121195, 1e-4);
00355 
00356 
00357     // radial SI, angular spin-images
00358     spin_est.setAngularDomain ();
00359 
00360     // estimate
00361     spin_est.computeEigen (*spin_images);
00362     EXPECT_EQ (spin_images->points.rows (), indices.size ());
00363 
00364     EXPECT_NEAR (spin_images->points (100, 0), 0, 1e-4);
00365     EXPECT_NEAR (spin_images->points (100, 12), 0, 1e-4);
00366     EXPECT_NEAR (spin_images->points (100, 24), 0.13213, 1e-4);
00367     EXPECT_NEAR (spin_images->points (100, 36), 0, 1e-4);
00368     EXPECT_NEAR (spin_images->points (100, 48), 0.908804, 1.1e-4);
00369     EXPECT_NEAR (spin_images->points (100, 60), 0.63875, 1e-4);
00370     EXPECT_NEAR (spin_images->points (100, 72), 0, 1e-4);
00371     EXPECT_NEAR (spin_images->points (100, 84), 0, 1e-4);
00372     EXPECT_NEAR (spin_images->points (100, 96), 0.550392, 1e-4);
00373     EXPECT_NEAR (spin_images->points (100, 108), 0, 1e-4);
00374     EXPECT_NEAR (spin_images->points (100, 120), 0, 1e-4);
00375     EXPECT_NEAR (spin_images->points (100, 132), 0, 1e-4);
00376     EXPECT_NEAR (spin_images->points (100, 144), 0.25713, 1e-4);
00377     EXPECT_NEAR (spin_images->points (300, 0), 0, 1e-4);
00378     EXPECT_NEAR (spin_images->points (300, 12), 0, 1e-4);
00379     EXPECT_NEAR (spin_images->points (300, 24), 0.230605, 1e-4);
00380     EXPECT_NEAR (spin_images->points (300, 36), 0, 1e-4);
00381     EXPECT_NEAR (spin_images->points (300, 48), 0, 1e-4);
00382     EXPECT_NEAR (spin_images->points (300, 60), 0.764872, 1e-4);
00383     EXPECT_NEAR (spin_images->points (300, 72), 0, 1e-4);
00384     EXPECT_NEAR (spin_images->points (300, 84), 0, 1e-4);
00385     EXPECT_NEAR (spin_images->points (300, 96), 1.02824, 1e-4);
00386     EXPECT_NEAR (spin_images->points (300, 108), 0, 1e-4);
00387     EXPECT_NEAR (spin_images->points (300, 120), 0, 1e-4);
00388     EXPECT_NEAR (spin_images->points (300, 132), 0, 1e-4);
00389     EXPECT_NEAR (spin_images->points (300, 144), 0.293567, 1e-4);
00390 
00391 
00392     // rectangular SI
00393     spin_est.setRadialStructure (false);
00394     spin_est.setAngularDomain (false);
00395 
00396     // estimate
00397     spin_est.computeEigen (*spin_images);
00398     EXPECT_EQ (spin_images->points.rows (), indices.size ());
00399 
00400     EXPECT_NEAR (spin_images->points (100, 0), 0, 1e-4);
00401     EXPECT_NEAR (spin_images->points (100, 12), 0, 1e-4);
00402     EXPECT_NEAR (spin_images->points (100, 24), 0.000889345, 1e-4);
00403     EXPECT_NEAR (spin_images->points (100, 36), 0, 1e-4);
00404     EXPECT_NEAR (spin_images->points (100, 48), 0, 1e-4);
00405     EXPECT_NEAR (spin_images->points (100, 60), 0.0489534, 1e-4);
00406     EXPECT_NEAR (spin_images->points (100, 72), 0, 1e-4);
00407     EXPECT_NEAR (spin_images->points (100, 84), 0, 1e-4);
00408     EXPECT_NEAR (spin_images->points (100, 96), 0.0747141, 1e-4);
00409     EXPECT_NEAR (spin_images->points (100, 108), 0, 1e-4);
00410     EXPECT_NEAR (spin_images->points (100, 120), 0, 1e-4);
00411     EXPECT_NEAR (spin_images->points (100, 132), 0.0173423, 1e-4);
00412     EXPECT_NEAR (spin_images->points (100, 144), 0, 1e-4);
00413     EXPECT_NEAR (spin_images->points (300, 0), 0, 1e-4);
00414     EXPECT_NEAR (spin_images->points (300, 12), 0, 1e-4);
00415     EXPECT_NEAR (spin_images->points (300, 24), 0.0267132, 1e-4);
00416     EXPECT_NEAR (spin_images->points (300, 36), 0, 1e-4);
00417     EXPECT_NEAR (spin_images->points (300, 48), 0, 1e-4);
00418     EXPECT_NEAR (spin_images->points (300, 60), 0, 1e-4);
00419     EXPECT_NEAR (spin_images->points (300, 72), 0, 1e-4);
00420     EXPECT_NEAR (spin_images->points (300, 84), 0, 1e-4);
00421     EXPECT_NEAR (spin_images->points (300, 96), 0, 1e-4);
00422     EXPECT_NEAR (spin_images->points (300, 108), 0.0209709, 1e-4);
00423     EXPECT_NEAR (spin_images->points (300, 120), 0, 1e-4);
00424     EXPECT_NEAR (spin_images->points (300, 132), 0, 1e-4);
00425     EXPECT_NEAR (spin_images->points (300, 144), 0.029372, 1e-4);
00426 
00427     // rectangular SI, angular spin-images
00428     spin_est.setAngularDomain ();
00429 
00430     // estimate
00431     spin_est.computeEigen (*spin_images);
00432     EXPECT_EQ (spin_images->points.rows (), indices.size ());
00433 
00434     EXPECT_NEAR (spin_images->points (100, 0), 0, 1e-4);
00435     EXPECT_NEAR (spin_images->points (100, 12), 0, 1e-4);
00436     EXPECT_NEAR (spin_images->points (100, 24), 0.132126, 1e-4);
00437     EXPECT_NEAR (spin_images->points (100, 36), 0, 1e-4);
00438     EXPECT_NEAR (spin_images->points (100, 48), 0, 1e-4);
00439     EXPECT_NEAR (spin_images->points (100, 60), 0.388011, 1e-4);
00440     EXPECT_NEAR (spin_images->points (100, 72), 0, 1e-4);
00441     EXPECT_NEAR (spin_images->points (100, 84), 0, 1e-4);
00442     EXPECT_NEAR (spin_images->points (100, 96), 0.468881, 1e-4);
00443     EXPECT_NEAR (spin_images->points (100, 108), 0, 1e-4);
00444     EXPECT_NEAR (spin_images->points (100, 120), 0, 1e-4);
00445     EXPECT_NEAR (spin_images->points (100, 132), 0.678995, 1e-4);
00446     EXPECT_NEAR (spin_images->points (100, 144), 0, 1e-4);
00447     EXPECT_NEAR (spin_images->points (300, 0), 0, 1e-4);
00448     EXPECT_NEAR (spin_images->points (300, 12), 0, 1e-4);
00449     EXPECT_NEAR (spin_images->points (300, 24), 0.143845, 1e-4);
00450     EXPECT_NEAR (spin_images->points (300, 36), 0, 1e-4);
00451     EXPECT_NEAR (spin_images->points (300, 48), 0, 1e-4);
00452     EXPECT_NEAR (spin_images->points (300, 60), 0, 1e-4);
00453     EXPECT_NEAR (spin_images->points (300, 72), 0, 1e-4);
00454     EXPECT_NEAR (spin_images->points (300, 84), 0, 1e-4);
00455     EXPECT_NEAR (spin_images->points (300, 96), 0, 1e-4);
00456     EXPECT_NEAR (spin_images->points (300, 108), 0.706084, 1e-4);
00457     EXPECT_NEAR (spin_images->points (300, 120), 0, 1e-4);
00458     EXPECT_NEAR (spin_images->points (300, 132), 0, 1e-4);
00459     EXPECT_NEAR (spin_images->points (300, 144), 0.272542, 1e-4);
00460   }
00461 
00462 
00464   TEST (PCL, IntensitySpinEstimationEigen)
00465   {
00466     // Generate a sample point cloud
00467     PointCloud<PointXYZI> cloud_xyzi;
00468     cloud_xyzi.height = 1;
00469     cloud_xyzi.is_dense = true;
00470 
00471     for (float x = -10.0f; x <= 10.0f; x += 1.0f)
00472     {
00473       for (float y = -10.0f; y <= 10.0f; y += 1.0f)
00474       {
00475         PointXYZI p;
00476         p.x = x;
00477         p.y = y;
00478         p.z = sqrtf (400.0f - x * x - y * y);
00479         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))
00480                                                                                    / (2.0f * 4.0f));
00481 
00482         cloud_xyzi.points.push_back (p);
00483       }
00484     }
00485     cloud_xyzi.width = static_cast<uint32_t> (cloud_xyzi.points.size ());
00486 
00487     // Compute the intensity-domain spin features
00488     typedef Histogram<20> IntensitySpin;
00489     IntensitySpinEstimation<PointXYZI, Eigen::MatrixXf> ispin_est;
00490     search::KdTree<PointXYZI>::Ptr treept3 (new search::KdTree<PointXYZI> (false));
00491     ispin_est.setSearchMethod (treept3);
00492     ispin_est.setRadiusSearch (10.0);
00493     ispin_est.setNrDistanceBins (4);
00494     ispin_est.setNrIntensityBins (5);
00495 
00496     ispin_est.setInputCloud (cloud_xyzi.makeShared ());
00497     PointCloud<Eigen::MatrixXf> ispin_output;
00498     ispin_est.computeEigen (ispin_output);
00499 
00500     // Compare to independently verified values
00501     Eigen::VectorXf ispin = ispin_output.points.row (220);
00502     const float correct_ispin_feature_values[20] = {2.4387f, 9.4737f, 21.3232f, 28.3025f, 22.5639f, 13.2426f, 35.7026f, 60.0755f,
00503                                                     66.9240f, 50.4225f, 42.7086f, 83.5818f, 105.4513f, 97.8454f, 67.3801f,
00504                                                     75.7127f, 119.4726f, 120.9649f, 93.4829f, 55.4045f};
00505     for (int i = 0; i < 20; ++i)
00506     {
00507       EXPECT_NEAR (ispin[i], correct_ispin_feature_values[i], 1e-4);
00508     }
00509   }
00510 #endif
00511 
00512 /* ---[ */
00513 int
00514 main (int argc, char** argv)
00515 {
00516   if (argc < 2)
00517   {
00518     std::cerr << "No test file given. Please download `bun0.pcd` and pass its path to the test." << std::endl;
00519     return (-1);
00520   }
00521 
00522   if (loadPCDFile<PointXYZ> (argv[1], cloud) < 0)
00523   {
00524     std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl;
00525     return (-1);
00526   }
00527 
00528   indices.resize (cloud.points.size ());
00529   for (size_t i = 0; i < indices.size (); ++i)
00530     indices[i] = static_cast<int> (i);
00531 
00532   tree.reset (new search::KdTree<PointXYZ> (false));
00533   tree->setInputCloud (cloud.makeShared ());
00534 
00535   testing::InitGoogleTest (&argc, argv);
00536   return (RUN_ALL_TESTS ());
00537 }
00538 /* ]--- */

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