test_ii_normals.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: test_ii_normals.cpp 5066 2012-03-14 06:42:21Z rusu $
00035  *
00036  */
00037 
00038 #include <gtest/gtest.h>
00039 
00040 #include <pcl/point_types.h>
00041 #include <pcl/features/normal_3d.h>
00042 #include <pcl/features/integral_image_normal.h>
00043 
00044 #include <iostream>
00045 
00046 using namespace pcl;
00047 using namespace std;
00048 
00049 typedef search::KdTree<PointXYZ>::Ptr KdTreePtr;
00050 PointCloud<PointXYZ> cloud;
00051 KdTreePtr tree;
00052 
00053 NormalEstimation<PointXYZ, Normal> n;
00054 IntegralImageNormalEstimation<PointXYZ, Normal> ne;
00055 
00057 TEST(PCL, IntegralImage1D)
00058 {
00059   const unsigned width = 640;
00060   const unsigned height = 480;
00061   const unsigned max_window_size = 5;
00062   const unsigned min_window_size = 1;
00063   IntegralImage2D<float,1> integral_image1(true); // calculate second order
00064   IntegralImage2D<float,1> integral_image2(false);// calculate just first order (other if branch)
00065 
00066   // test for dense data with element stride = 1
00067   float* data = new float[width * height];
00068   for(unsigned yIdx = 0; yIdx < height; ++yIdx)
00069   {
00070     for(unsigned xIdx = 0; xIdx < width; ++xIdx)
00071     {
00072       data[width * yIdx + xIdx] = static_cast<float> (xIdx);
00073     }
00074   }
00075 
00076   // calculate integral images
00077   integral_image1.setInput (data, width, height, 1, width);
00078   integral_image2.setInput (data, width, height, 1, width);
00079 
00080   // check results
00081   for (unsigned window_width = min_window_size; window_width < max_window_size; ++window_width)
00082   {
00083     for (unsigned window_height = min_window_size; window_height < max_window_size; ++window_height)
00084     {
00085       for(unsigned yIdx = 0; yIdx < height - window_height; ++yIdx)
00086       {
00087         for(unsigned xIdx = 0; xIdx < width - window_width; ++xIdx)
00088         {
00089           //cout << xIdx << " : " << yIdx << " - " << window_width << " x " << window_height << " :: " << integral_image1.getFirstOrderSum (xIdx, yIdx, window_width, window_height) * 2 << endl;
00090           EXPECT_EQ (window_height * window_width * (window_width + 2 * xIdx - 1), integral_image1.getFirstOrderSum (xIdx, yIdx, window_width, window_height) * 2);
00091           EXPECT_EQ (window_height * window_width * (window_width + 2 * xIdx - 1), integral_image2.getFirstOrderSum (xIdx, yIdx, window_width, window_height) * 2);
00092           EXPECT_EQ (window_height * window_width, integral_image1.getFiniteElementsCount (xIdx, yIdx, window_width, window_height));
00093           EXPECT_EQ (window_height * window_width, integral_image2.getFiniteElementsCount (xIdx, yIdx, window_width, window_height));
00094 
00095           int w = window_width + xIdx - 1;
00096           long result = w * (w + 1) * (2*w + 1) - xIdx * (xIdx - 1) * (2*xIdx - 1);
00097           EXPECT_EQ (window_height * result, integral_image1.getSecondOrderSum (xIdx, yIdx, window_width, window_height) * 6);
00098         }
00099       }
00100     }
00101   }
00102   delete[] data;
00103 
00104   //now test with element-stride 2 and modulo even row stride (no padding)
00105   unsigned element_stride = 2;
00106   unsigned row_stride = width * element_stride;
00107   data = new float[row_stride * height];
00108   for(unsigned yIdx = 0; yIdx < height; ++yIdx)
00109   {
00110     for(unsigned xIdx = 0; xIdx < row_stride; xIdx += element_stride)
00111     {
00112       data[row_stride * yIdx + xIdx] = static_cast<float> (xIdx >> 1);
00113       data[row_stride * yIdx + xIdx + 1] = -1;
00114     }
00115   }
00116   integral_image1.setInput (data, width, height, element_stride, row_stride);
00117   integral_image2.setInput (data, width, height, element_stride, row_stride);
00118   for (unsigned window_width = min_window_size; window_width < max_window_size; ++window_width)
00119   {
00120     for (unsigned window_height = min_window_size; window_height < max_window_size; ++window_height)
00121     {
00122       for(unsigned yIdx = 0; yIdx < height - window_height; ++yIdx)
00123       {
00124         for(unsigned xIdx = 0; xIdx < width - window_width; ++xIdx)
00125         {
00126           EXPECT_EQ (window_height * window_width * (window_width + 2 * xIdx - 1), integral_image1.getFirstOrderSum (xIdx, yIdx, window_width, window_height) * 2);
00127           EXPECT_EQ (window_height * window_width * (window_width + 2 * xIdx - 1), integral_image2.getFirstOrderSum (xIdx, yIdx, window_width, window_height) * 2);
00128           EXPECT_EQ (window_height * window_width, integral_image1.getFiniteElementsCount (xIdx, yIdx, window_width, window_height));
00129           EXPECT_EQ (window_height * window_width, integral_image2.getFiniteElementsCount (xIdx, yIdx, window_width, window_height));
00130 
00131           int w = window_width + xIdx - 1;
00132           long result = w * (w + 1) * (2*w + 1) - xIdx * (xIdx - 1) * (2*xIdx - 1);
00133           EXPECT_EQ (window_height * result, integral_image1.getSecondOrderSum (xIdx, yIdx, window_width, window_height) * 6);
00134         }
00135       }
00136     }
00137   }
00138   delete[] data;
00139 
00140   //now test with odd element-stride 3 and modulo-uneven row_stride
00141   element_stride = 3;
00142   row_stride = width * element_stride + 1; // +1 to enforce a non-zero modulo
00143   data = new float[row_stride * height];
00144   for (unsigned yIdx = 0; yIdx < height; ++yIdx)
00145   {
00146     for (unsigned xIdx = 0; xIdx < width; ++xIdx)
00147     {
00148       data[row_stride * yIdx + element_stride * xIdx] = 1.0f;
00149       data[row_stride * yIdx + element_stride * xIdx + 1] = 2.0f;
00150       data[row_stride * yIdx + element_stride * xIdx + 2] = static_cast<float> (xIdx);
00151     }
00152   }
00153   integral_image1.setInput (data, width, height, element_stride, row_stride);
00154   integral_image2.setInput (data, width, height, element_stride, row_stride);
00155   for (unsigned window_width = min_window_size; window_width < max_window_size; ++window_width)
00156   {
00157     for (unsigned window_height = min_window_size; window_height < max_window_size; ++window_height)
00158     {
00159       for(unsigned yIdx = 0; yIdx < height - window_height; ++yIdx)
00160       {
00161         for(unsigned xIdx = 0; xIdx < width - window_width; ++xIdx)
00162         {
00163           EXPECT_EQ (window_width * window_height, integral_image1.getFirstOrderSum (xIdx, yIdx, window_width, window_height));
00164           EXPECT_EQ (window_width * window_height, integral_image2.getFirstOrderSum (xIdx, yIdx, window_width, window_height));
00165           EXPECT_EQ (window_width * window_height, integral_image1.getFiniteElementsCount (xIdx, yIdx, window_width, window_height));
00166           EXPECT_EQ (window_width * window_height, integral_image2.getFiniteElementsCount (xIdx, yIdx, window_width, window_height));
00167 
00168           EXPECT_EQ (window_width * window_height, integral_image1.getSecondOrderSum (xIdx, yIdx, window_width, window_height));
00169         }
00170       }
00171     }
00172   }
00173   //check for second channel
00174   integral_image1.setInput (data + 1, width, height, element_stride, row_stride);
00175   integral_image2.setInput (data + 1, width, height, element_stride, row_stride);
00176   for (unsigned window_width = min_window_size; window_width < max_window_size; ++window_width)
00177   {
00178     for (unsigned window_height = min_window_size; window_height < max_window_size; ++window_height)
00179     {
00180       for(unsigned yIdx = 0; yIdx < height - window_height; ++yIdx)
00181       {
00182         for(unsigned xIdx = 0; xIdx < width - window_width; ++xIdx)
00183         {
00184           EXPECT_EQ (window_width * window_height * 2, integral_image1.getFirstOrderSum (xIdx, yIdx, window_width, window_height));
00185           EXPECT_EQ (window_width * window_height * 2, integral_image2.getFirstOrderSum (xIdx, yIdx, window_width, window_height));
00186           EXPECT_EQ (window_width * window_height, integral_image1.getFiniteElementsCount (xIdx, yIdx, window_width, window_height));
00187           EXPECT_EQ (window_width * window_height, integral_image2.getFiniteElementsCount (xIdx, yIdx, window_width, window_height));
00188 
00189           EXPECT_EQ (window_width * window_height * 4, integral_image1.getSecondOrderSum (xIdx, yIdx, window_width, window_height));
00190         }
00191       }
00192     }
00193   }
00194 
00195   //check for third channel
00196   integral_image1.setInput (data + 2, width, height, element_stride, row_stride);
00197   integral_image2.setInput (data + 2, width, height, element_stride, row_stride);
00198   for (unsigned window_width = min_window_size; window_width < max_window_size; ++window_width)
00199   {
00200     for (unsigned window_height = min_window_size; window_height < max_window_size; ++window_height)
00201     {
00202       for(unsigned yIdx = 0; yIdx < height - window_height; ++yIdx)
00203       {
00204         for(unsigned xIdx = 0; xIdx < width - window_width; ++xIdx)
00205         {
00206           EXPECT_EQ (window_height * window_width * (window_width + 2 * xIdx - 1), integral_image1.getFirstOrderSum (xIdx, yIdx, window_width, window_height) * 2);
00207           EXPECT_EQ (window_height * window_width * (window_width + 2 * xIdx - 1), integral_image2.getFirstOrderSum (xIdx, yIdx, window_width, window_height) * 2);
00208           EXPECT_EQ (window_width * window_height, integral_image1.getFiniteElementsCount (xIdx, yIdx, window_width, window_height));
00209           EXPECT_EQ (window_width * window_height, integral_image2.getFiniteElementsCount (xIdx, yIdx, window_width, window_height));
00210 
00211           int w = window_width + xIdx - 1;
00212           long result = w * (w + 1) * (2*w + 1) - xIdx * (xIdx - 1) * (2*xIdx - 1);
00213           EXPECT_EQ (window_height * result, integral_image1.getSecondOrderSum (xIdx, yIdx, window_width, window_height) * 6);
00214         }
00215       }
00216     }
00217   }
00218   delete[] data;
00219 
00220   //now test whether count of non finite elements are correct
00221   element_stride = 3;
00222   row_stride = width * element_stride + 1; // +1 to enforce a non-zero modulo
00223   data = new float[row_stride * height];
00224   for(unsigned yIdx = 0; yIdx < height; ++yIdx)
00225   {
00226     for(unsigned xIdx = 0; xIdx < width; ++xIdx)
00227     {
00228       // set nans for odd fields
00229       if (xIdx & 1)
00230       {
00231         data[row_stride * yIdx + element_stride * xIdx]     = std::numeric_limits<float>::quiet_NaN ();;
00232         data[row_stride * yIdx + element_stride * xIdx + 1] = std::numeric_limits<float>::quiet_NaN ();;
00233         data[row_stride * yIdx + element_stride * xIdx + 2] = std::numeric_limits<float>::quiet_NaN ();;
00234       }
00235       else
00236       {
00237         data[row_stride * yIdx + element_stride * xIdx] = 3.0f;
00238         data[row_stride * yIdx + element_stride * xIdx + 1] = 2.0f;
00239         data[row_stride * yIdx + element_stride * xIdx + 2] = static_cast<float> (xIdx);
00240       }
00241     }
00242   }
00243   integral_image1.setInput (data, width, height, element_stride, row_stride);
00244   integral_image2.setInput (data, width, height, element_stride, row_stride);
00245   for (unsigned window_width = min_window_size; window_width < max_window_size; ++window_width)
00246   {
00247     for (unsigned window_height = min_window_size; window_height < max_window_size; ++window_height)
00248     {
00249       for(unsigned yIdx = 0; yIdx < height - window_height; ++yIdx)
00250       {
00251         for(unsigned xIdx = 0; xIdx < width - window_width; ++xIdx)
00252         {
00253           int count = window_height * ((window_width - (xIdx&1) + 1) >> 1);
00254 
00255           EXPECT_EQ (count * 3, integral_image1.getFirstOrderSum (xIdx, yIdx, window_width, window_height));
00256           EXPECT_EQ (count * 3, integral_image2.getFirstOrderSum (xIdx, yIdx, window_width, window_height));
00257           EXPECT_EQ (count, integral_image1.getFiniteElementsCount (xIdx, yIdx, window_width, window_height));
00258           EXPECT_EQ (count, integral_image2.getFiniteElementsCount (xIdx, yIdx, window_width, window_height));
00259           EXPECT_EQ (count * 9, integral_image1.getSecondOrderSum (xIdx, yIdx, window_width, window_height));
00260         }
00261       }
00262     }
00263   }
00264   delete[] data;
00265 
00266 
00267 }
00268 
00270 TEST(PCL, IntegralImage3D)
00271 {
00272   const unsigned width = 640;
00273   const unsigned height = 480;
00274   const unsigned max_window_size = 5;
00275   const unsigned min_window_size = 1;
00276   IntegralImage2D<float, 3> integral_image3(true);
00277   unsigned element_stride = 4;
00278   unsigned row_stride = width * element_stride + 1;
00279   float* data = new float[row_stride * height];
00280   for(unsigned yIdx = 0; yIdx < height; ++yIdx)
00281   {
00282     for(unsigned xIdx = 0; xIdx < width; ++xIdx)
00283     {
00284       data[row_stride * yIdx + xIdx * element_stride] = static_cast<float> (xIdx);
00285       data[row_stride * yIdx + xIdx * element_stride + 1] = static_cast<float> (yIdx);
00286       data[row_stride * yIdx + xIdx * element_stride + 2] = static_cast<float> (xIdx + yIdx);
00287       data[row_stride * yIdx + xIdx * element_stride + 3] = -1000.0f;
00288     }
00289   }
00290   integral_image3.setInput (data, width, height, element_stride, row_stride);
00291   for (unsigned window_width = min_window_size; window_width < max_window_size; ++window_width)
00292   {
00293     for (unsigned window_height = min_window_size; window_height < max_window_size; ++window_height)
00294     {
00295       for(unsigned yIdx = 0; yIdx < height - window_height; ++yIdx)
00296       {
00297         for(unsigned xIdx = 0; xIdx < width - window_width; ++xIdx)
00298         {
00299           IntegralImage2D<float, 3>::ElementType sum = integral_image3.getFirstOrderSum (xIdx, yIdx, window_width, window_height);
00300 
00301           EXPECT_EQ (window_height * window_width * (window_width + 2 * xIdx - 1), sum[0] * 2);
00302           EXPECT_EQ (window_width * window_height * (window_height + 2 * yIdx - 1), sum[1] * 2);
00303           EXPECT_EQ (window_width * window_height * (window_height + 2 * yIdx - 1) + window_height * window_width * (window_width + 2 * xIdx - 1), sum[2] * 2);
00304 
00305           IntegralImage2D<float, 3>::SecondOrderType sumSqr = integral_image3.getSecondOrderSum (xIdx, yIdx, window_width, window_height);
00306 
00307           IntegralImage2D<float, 3>::SecondOrderType ground_truth;
00308           ground_truth.setZero ();
00309           for (unsigned wy = yIdx; wy < yIdx + window_height; ++wy)
00310           {
00311             for (unsigned wx = xIdx; wx < xIdx + window_width; ++wx)
00312             {
00313               float* val = data + (wy * row_stride + wx * element_stride);
00314               //ground_truth[0] += val[0] * val[0];
00315               ground_truth[1] += val[0] * val[1];
00316               ground_truth[2] += val[0] * val[2];
00317               //ground_truth[3] += val[1] * val[1];
00318               ground_truth[4] += val[1] * val[2];
00319               ground_truth[5] += val[2] * val[2];
00320             }
00321           }
00322 
00323           //EXPECT_EQ (ground_truth [0], sumSqr[0]);
00324           EXPECT_EQ (ground_truth [1], sumSqr[1]);
00325           EXPECT_EQ (ground_truth [2], sumSqr[2]);
00326           //EXPECT_EQ (ground_truth [3], sumSqr[3]);
00327           EXPECT_EQ (ground_truth [4], sumSqr[4]);
00328           EXPECT_EQ (ground_truth [5], sumSqr[5]);
00329 
00330           int w = window_width + xIdx - 1;
00331           long result = w * (w + 1) * (2*w + 1) - xIdx * (xIdx - 1) * (2*xIdx - 1);
00332 
00333           EXPECT_EQ (window_height * result, sumSqr[0] * 6);
00334 
00335           int h = window_height + yIdx - 1;
00336           result = h * (h + 1) * (2*h + 1) - yIdx * (yIdx - 1) * (2*yIdx - 1);
00337           EXPECT_EQ (window_width * result, sumSqr[3] * 6);
00338         }
00339       }
00340     }
00341   }
00342   delete[] data;
00343 }
00344 
00346 TEST (PCL, NormalEstimation)
00347 {
00348   tree.reset (new search::KdTree<PointXYZ> (false));
00349   n.setSearchMethod (tree);
00350   n.setKSearch (10);
00351 
00352   n.setInputCloud (cloud.makeShared ());
00353 
00354   PointCloud<Normal> output;
00355   n.compute (output);
00356 
00357   EXPECT_EQ (output.points.size (), cloud.points.size ());
00358   EXPECT_EQ (output.width, cloud.width);
00359   EXPECT_EQ (output.height, cloud.height);
00360 
00361   for (size_t i = 0; i < cloud.points.size (); ++i)
00362   {
00363     EXPECT_NEAR (fabs (output.points[i].normal_x),   0, 1e-2);
00364     EXPECT_NEAR (fabs (output.points[i].normal_y),   0, 1e-2);
00365     EXPECT_NEAR (fabs (output.points[i].normal_z), 1.0, 1e-2);
00366   }
00367 }
00368 
00370 TEST (PCL, IINormalEstimationCovariance)
00371 {
00372   PointCloud<Normal> output;
00373   ne.setRectSize (3, 3);
00374   ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
00375   ne.compute (output);
00376 
00377   EXPECT_EQ (output.points.size (), cloud.points.size ());
00378   EXPECT_EQ (output.width, cloud.width);
00379   EXPECT_EQ (output.height, cloud.height);
00380 
00381   for (size_t v = 0; v < cloud.height; ++v)
00382   {
00383     for (size_t u = 0; u < cloud.width; ++u)
00384     {
00385       if (!pcl_isfinite(output (u, v).normal_x) &&
00386           !pcl_isfinite(output (u, v).normal_y) &&
00387           !pcl_isfinite(output (u, v).normal_z))
00388         continue;
00389 
00390       EXPECT_NEAR (fabs (output (u, v).normal_x),   0, 1e-2);
00391       EXPECT_NEAR (fabs (output (u, v).normal_y),   0, 1e-2);
00392       EXPECT_NEAR (fabs (output (u, v).normal_z), 1.0, 1e-2);
00393     }
00394   }
00395 }
00396 
00398 TEST (PCL, IINormalEstimationAverage3DGradient)
00399 {
00400   PointCloud<Normal> output;
00401   ne.setRectSize (3, 3);
00402   ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
00403   ne.compute (output);
00404 
00405   EXPECT_EQ (output.points.size (), cloud.points.size ());
00406   EXPECT_EQ (output.width, cloud.width);
00407   EXPECT_EQ (output.height, cloud.height);
00408 
00409   for (size_t v = 0; v < cloud.height; ++v)
00410   {
00411     for (size_t u = 0; u < cloud.width; ++u)
00412     {
00413       if (!pcl_isfinite(output (u, v).normal_x) &&
00414           !pcl_isfinite(output (u, v).normal_y) &&
00415           !pcl_isfinite(output (u, v).normal_z))
00416         continue;
00417 
00418       if (fabs(fabs (output (u, v).normal_z) - 1) > 1e-2)
00419       {
00420         std::cout << "T:" << u << " , " << v << " : " << output (u, v).normal_x << " , " << output (u, v).normal_y << " , " << output (u, v).normal_z <<std::endl;
00421       }
00422       EXPECT_NEAR (fabs (output (u, v).normal_x),   0, 1e-2);
00423       EXPECT_NEAR (fabs (output (u, v).normal_y),   0, 1e-2);
00424       //EXPECT_NEAR (fabs (output (u, v).normal_z), 1.0, 1e-2);
00425     }
00426   }
00427 }
00428 
00430 TEST (PCL, IINormalEstimationAverageDepthChange)
00431 {
00432   PointCloud<Normal> output;
00433   ne.setRectSize (3, 3);
00434   ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE);
00435   ne.compute (output);
00436 
00437   EXPECT_EQ (output.points.size (), cloud.points.size ());
00438   EXPECT_EQ (output.width, cloud.width);
00439   EXPECT_EQ (output.height, cloud.height);
00440 
00441   for (size_t v = 0; v < cloud.height; ++v)
00442   {
00443     for (size_t u = 0; u < cloud.width; ++u)
00444     {
00445       if (!pcl_isfinite(output (u, v).normal_x) &&
00446           !pcl_isfinite(output (u, v).normal_y) &&
00447           !pcl_isfinite(output (u, v).normal_z))
00448         continue;
00449 
00450       if (fabs(fabs (output (u, v).normal_z) - 1) > 1e-2)
00451       {
00452         std::cout << "T:" << u << " , " << v << " : " << output (u, v).normal_x << " , " << output (u, v).normal_y << " , " << output (u, v).normal_z <<std::endl;
00453       }
00454       EXPECT_NEAR (fabs (output (u, v).normal_x),   0, 1e-2);
00455       EXPECT_NEAR (fabs (output (u, v).normal_y),   0, 1e-2);
00456       //EXPECT_NEAR (fabs (output (u, v).normal_z), 1.0, 1e-2);
00457     }
00458   }
00459 }
00460 
00462 TEST (PCL, IINormalEstimationSimple3DGradient)
00463 {
00464   PointCloud<Normal> output;
00465   ne.setRectSize (3, 3);
00466   ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT);
00467   ne.compute (output);
00468 
00469   EXPECT_EQ (output.points.size (), cloud.points.size ());
00470   EXPECT_EQ (output.width, cloud.width);
00471   EXPECT_EQ (output.height, cloud.height);
00472 
00473   for (size_t v = 0; v < cloud.height; ++v)
00474   {
00475     for (size_t u = 0; u < cloud.width; ++u)
00476     {
00477       if (!pcl_isfinite(output (u, v).normal_x) &&
00478           !pcl_isfinite(output (u, v).normal_y) &&
00479           !pcl_isfinite(output (u, v).normal_z))
00480         continue;
00481 
00482       if (fabs(fabs (output (u, v).normal_z) - 1) > 1e-2)
00483       {
00484         std::cout << "T:" << u << " , " << v << " : " << output (u, v).normal_x << " , " << output (u, v).normal_y << " , " << output (u, v).normal_z <<std::endl;
00485       }
00486       EXPECT_NEAR (fabs (output (u, v).normal_x),   0, 1e-2);
00487       EXPECT_NEAR (fabs (output (u, v).normal_y),   0, 1e-2);
00488       //EXPECT_NEAR (fabs (output (u, v).normal_z), 1.0, 1e-2);
00489     }
00490   }
00491 }
00492 
00494 TEST (PCL, IINormalEstimationSimple3DGradientUnorganized)
00495 {
00496   PointCloud<Normal> output;
00497   cloud.height = 1;
00498   cloud.points.resize (cloud.height * cloud.width);
00499   ne.setInputCloud (cloud.makeShared ());
00500   ne.setRectSize (3, 3);
00501   ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT);
00502   ne.compute (output);
00503 
00504   EXPECT_EQ (output.points.size (), 0);
00505   EXPECT_EQ (output.width, 0);
00506   EXPECT_EQ (output.height, 0);
00507 }
00508 
00509 /* ---[ */
00510 int
00511 main (int argc, char** argv)
00512 {
00513   cloud.width = 640;
00514   cloud.height = 480;
00515   cloud.points.resize (cloud.width * cloud.height);
00516   cloud.is_dense = true;
00517   for (size_t v = 0; v < cloud.height; ++v)
00518   {
00519     for (size_t u = 0; u < cloud.width; ++u)
00520     {
00521       cloud (u, v).x = static_cast<float> (u);
00522       cloud (u, v).y = static_cast<float> (v);
00523       cloud (u, v).z = 10.0f;
00524     }
00525   }
00526 
00527   ne.setInputCloud (cloud.makeShared ());
00528   testing::InitGoogleTest (&argc, argv);
00529   return (RUN_ALL_TESTS ());
00530 
00531   return 1;
00532 }
00533 /* ]--- */
00534 


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