test_gradient_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/intensity_gradient.h>
00044 
00045 using namespace pcl;
00046 using namespace pcl::io;
00047 using namespace std;
00048 
00050 TEST (PCL, IntensityGradientEstimation)
00051 {
00052   // Create a test cloud
00053   PointCloud<PointXYZI> cloud_xyzi;
00054   cloud_xyzi.height = 1;
00055   cloud_xyzi.is_dense = true;
00056   for (float x = -5.0f; x <= 5.0f; x += 0.1f)
00057   {
00058     for (float y = -5.0f; y <= 5.0f; y += 0.1f)
00059     {
00060       PointXYZI p;
00061       p.x = x;
00062       p.y = y;
00063       p.z = 0.1f * powf (x, 2.0f) + 0.5f * y + 1.0f;
00064       p.intensity = 0.1f * powf (x, 3.0f) + 0.2f * powf (y, 2.0f) + 1.0f * p.z + 20000.0f;
00065 
00066       cloud_xyzi.points.push_back (p);
00067     }
00068   }
00069   cloud_xyzi.width = static_cast<uint32_t> (cloud_xyzi.points.size ());
00070   PointCloud<PointXYZI>::ConstPtr cloud_ptr = cloud_xyzi.makeShared ();
00071 
00072   // Estimate surface normals
00073   PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
00074   NormalEstimation<PointXYZI, Normal> norm_est;
00075   norm_est.setInputCloud (cloud_ptr);
00076   search::KdTree<PointXYZI>::Ptr treept1 (new search::KdTree<PointXYZI> (false));
00077   norm_est.setSearchMethod (treept1);
00078   norm_est.setRadiusSearch (0.25);
00079   norm_est.compute (*normals);
00080 
00081   // Estimate intensity gradient
00082   PointCloud<IntensityGradient> gradient;
00083   IntensityGradientEstimation<PointXYZI, Normal, IntensityGradient> grad_est;
00084   grad_est.setInputCloud (cloud_ptr);
00085   grad_est.setInputNormals (normals);
00086   search::KdTree<PointXYZI>::Ptr treept2 (new search::KdTree<PointXYZI> (false));
00087   grad_est.setSearchMethod (treept2);
00088   grad_est.setRadiusSearch (0.25);
00089   grad_est.compute (gradient);
00090 
00091   // Compare to gradient estimates to actual values
00092   for (size_t i = 0; i < cloud_ptr->points.size (); ++i)
00093   {
00094     const PointXYZI &p = cloud_ptr->points[i];
00095 
00096     // A pointer to the estimated gradient values
00097     const float * g_est = gradient.points[i].gradient;
00098 
00099     // Compute the surface normal analytically.
00100     float nx = -0.2f * p.x;
00101     float ny = -0.5f;
00102     float nz = 1.0f;
00103     float magnitude = sqrtf (nx * nx + ny * ny + nz * nz);
00104     nx /= magnitude;
00105     ny /= magnitude;
00106     nz /= magnitude;
00107 
00108     // Compute the intensity gradient analytically...
00109     float tmpx = 0.3f * powf (p.x, 2.0f);
00110     float tmpy = 0.4f * p.y;
00111     float tmpz = 1.0f;
00112     // ...and project the 3-D gradient vector onto the surface's tangent plane.
00113     float gx = (1 - nx * nx) * tmpx + (-nx * ny) * tmpy + (-nx * nz) * tmpz;
00114     float gy = (-ny * nx) * tmpx + (1 - ny * ny) * tmpy + (-ny * nz) * tmpz;
00115     float gz = (-nz * nx) * tmpx + (-nz * ny) * tmpy + (1 - nz * nz) * tmpz;
00116 
00117     // Compare the estimates to the derived values.
00118     const float tolerance = 0.11f;
00119     EXPECT_NEAR (g_est[0], gx, tolerance);
00120     EXPECT_NEAR (g_est[1], gy, tolerance);
00121     EXPECT_NEAR (g_est[2], gz, tolerance);
00122   }
00123 }
00124 
00125 #ifndef PCL_ONLY_CORE_POINT_TYPES
00126 
00127   TEST (PCL, IntensityGradientEstimationEigen)
00128   {
00129     // Create a test cloud
00130     PointCloud<PointXYZI> cloud_xyzi;
00131     cloud_xyzi.height = 1;
00132     cloud_xyzi.is_dense = true;
00133     for (float x = -5.0f; x <= 5.0f; x += 0.1f)
00134     {
00135       for (float y = -5.0f; y <= 5.0f; y += 0.1f)
00136       {
00137         PointXYZI p;
00138         p.x = x;
00139         p.y = y;
00140         p.z = 0.1f * powf (x, 2.0f) + 0.5f * y + 1.0f;
00141         p.intensity = 0.1f * powf (x, 3.0f) + 0.2f * powf (y, 2.0f) + 1.0f * p.z + 20000.0f;
00142 
00143         cloud_xyzi.points.push_back (p);
00144       }
00145     }
00146     cloud_xyzi.width = static_cast<uint32_t> (cloud_xyzi.points.size ());
00147     PointCloud<PointXYZI>::ConstPtr cloud_ptr = cloud_xyzi.makeShared ();
00148 
00149     // Estimate surface normals
00150     PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
00151     NormalEstimation<PointXYZI, Normal> norm_est;
00152     norm_est.setInputCloud (cloud_ptr);
00153     search::KdTree<PointXYZI>::Ptr treept1 (new search::KdTree<PointXYZI> (false));
00154     norm_est.setSearchMethod (treept1);
00155     norm_est.setRadiusSearch (0.25);
00156     norm_est.compute (*normals);
00157 
00158     // Estimate intensity gradient
00159     PointCloud<Eigen::MatrixXf> gradient;
00160     IntensityGradientEstimation<PointXYZI, Normal, Eigen::MatrixXf> grad_est;
00161     grad_est.setInputCloud (cloud_ptr);
00162     grad_est.setInputNormals (normals);
00163     search::KdTree<PointXYZI>::Ptr treept2 (new search::KdTree<PointXYZI> (false));
00164     grad_est.setSearchMethod (treept2);
00165     grad_est.setRadiusSearch (0.25);
00166     grad_est.computeEigen (gradient);
00167 
00168     // Compare to gradient estimates to actual values
00169     for (size_t i = 0; i < cloud_ptr->points.size (); ++i)
00170     {
00171       const PointXYZI &p = cloud_ptr->points[i];
00172 
00173       // Compute the surface normal analytically.
00174       float nx = -0.2f * p.x;
00175       float ny = -0.5f;
00176       float nz = 1.0f;
00177       float magnitude = sqrtf (nx * nx + ny * ny + nz * nz);
00178       nx /= magnitude;
00179       ny /= magnitude;
00180       nz /= magnitude;
00181 
00182       // Compute the intensity gradient analytically...
00183       float tmpx = 0.3f * powf (p.x, 2.0f);
00184       float tmpy = 0.4f * p.y;
00185       float tmpz = 1.0f;
00186       // ...and project the 3-D gradient vector onto the surface's tangent plane.
00187       float gx = (1 - nx * nx) * tmpx + (-nx * ny) * tmpy + (-nx * nz) * tmpz;
00188       float gy = (-ny * nx) * tmpx + (1 - ny * ny) * tmpy + (-ny * nz) * tmpz;
00189       float gz = (-nz * nx) * tmpx + (-nz * ny) * tmpy + (1 - nz * nz) * tmpz;
00190 
00191       // Compare the estimates to the derived values.
00192       const float tolerance = 0.11f;
00193       ASSERT_NEAR (gradient.points (i, 0), gx, tolerance);
00194       ASSERT_NEAR (gradient.points (i, 1), gy, tolerance);
00195       ASSERT_NEAR (gradient.points (i, 2), gz, tolerance);
00196     }
00197   }
00198 #endif
00199 
00200 /* ---[ */
00201 int
00202 main (int argc, char** argv)
00203 {
00204   testing::InitGoogleTest (&argc, argv);
00205   return (RUN_ALL_TESTS ());
00206 }
00207 /* ]--- */


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