test_iterator.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, 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  */
00035 
00036 #include <gtest/gtest.h>
00037 #include <pcl/common/common.h>
00038 #include <pcl/geometry/line_iterator.h>
00039 #include <pcl/point_types.h>
00040 #include <pcl/point_cloud.h>
00041 #include <math.h>
00042 
00043 using namespace pcl;
00044 
00045 template <typename PointT>
00046 void checkSimpleLine8 (unsigned x_start, unsigned y_start, unsigned x_end, unsigned y_end, PointCloud<PointT>& cloud)
00047 {
00048   PointXYZ point;
00049   point.x = point.y = point.z = 0.0f;
00050   for (unsigned yIdx = 0; yIdx < cloud.height; ++yIdx)
00051   {
00052     for (unsigned xIdx = 0; xIdx < cloud.width; ++xIdx)
00053     {
00054       PointT& point = cloud.points [yIdx * cloud.width + xIdx];
00055       point.x = float(xIdx);
00056       point.y = float(yIdx);
00057       point.z = 0.0f;
00058     }
00059   }
00060 
00061   LineIterator lineIt (x_start, y_start, x_end, y_end, cloud.width, LineIterator::Neighbor8);
00062   // use polymorphic
00063   OrganizedIndexIterator& iterator = lineIt;
00064   unsigned idx = 0;
00065   while (iterator.isValid ())
00066   {
00067     PointT& point = cloud[*iterator];
00068     EXPECT_EQ (point.x, iterator.getColumnIndex ());
00069     EXPECT_EQ (point.y, iterator.getRowIndex ());
00070     point.z = 1.0f;
00071     ++iterator;
00072     ++idx;
00073   }
00074   int dx = x_end - x_start;
00075   int dy = y_end - y_start;
00076   unsigned dmax = std::max (abs(dx), abs(dy));
00077   
00078   EXPECT_EQ (dmax, idx);
00079   
00080   int x_step = 0;
00081   int y_step = 0;
00082   
00083   EXPECT_GT (dmax, 0);
00084   if (dx == 0)
00085   {
00086     x_step = 0;
00087     y_step = (dy > 0) ? 1 : -1;
00088   }
00089   else if (dy == 0)
00090   {
00091     y_step = 0;
00092     x_step = (dx > 0) ? 1 : -1;
00093   }
00094   else if (abs(dx) == abs(dy))
00095   {
00096     y_step = (dy > 0) ? 1 : -1;
00097     x_step = (dx > 0) ? 1 : -1;
00098   }
00099   else
00100   {
00101     // only horizontal, vertical and 45deg diagonal lines handled here
00102     EXPECT_TRUE (false);
00103   }
00104   unsigned xIdx = x_start;
00105   unsigned yIdx = y_start;
00106   for (unsigned idx = 0; idx < dmax; ++idx, xIdx += x_step, yIdx += y_step)
00107   {
00108     PointT& point = cloud.points [yIdx * cloud.width + xIdx];
00109     EXPECT_EQ (point.z, 1.0f);
00110     point.z = 0.0;
00111   }
00112   // now all z-values should be 0 again!
00113   for (unsigned yIdx = 0; yIdx < cloud.height; ++yIdx)
00114   {
00115     for (unsigned xIdx = 0; xIdx < cloud.width; ++xIdx)
00116     {
00117       //std::cout << "testing  point: " << xIdx << " , " << yIdx << std::endl;
00118       PointT& point = cloud.points [yIdx * cloud.width + xIdx];
00119 //      if (point.z != 0.0f)
00120 //        std::cout << "point.z != 0.0f at: " << xIdx << " , " << yIdx << std::endl;
00121       EXPECT_EQ (point.z, 0.0f);
00122     }
00123   }
00124 }
00125 
00126 template <typename PointT>
00127 void checkGeneralLine (unsigned x_start, unsigned y_start, unsigned x_end, unsigned y_end, PointCloud<PointT>& cloud, bool neighorhood)
00128 {
00129   PointXYZ point;
00130   point.x = point.y = point.z = 0.0f;
00131   for (unsigned yIdx = 0; yIdx < cloud.height; ++yIdx)
00132   {
00133     for (unsigned xIdx = 0; xIdx < cloud.width; ++xIdx)
00134     {
00135       PointT& point = cloud.points [yIdx * cloud.width + xIdx];
00136       point.x = float(xIdx);
00137       point.y = float(yIdx);
00138       point.z = 0.0f;
00139     }
00140   }
00141 
00142   LineIterator::Neighborhood neighbors;
00143   if (neighorhood)
00144     neighbors = LineIterator::Neighbor8;
00145   else
00146     neighbors = LineIterator::Neighbor4;
00147     
00148   LineIterator lineIt (x_start, y_start, x_end, y_end, cloud.width, neighbors);
00149   // use polymorphic
00150   OrganizedIndexIterator& iterator = lineIt;
00151   unsigned idx = 0;
00152   while (iterator.isValid ())
00153   {
00154     PointT& point = cloud [*iterator];
00155     EXPECT_EQ (point.x, iterator.getColumnIndex ());
00156     EXPECT_EQ (point.y, iterator.getRowIndex ());
00157     //std::cout << idx << " :: " << iterator.getPointIndex () << " :: " << iterator.getColumnIndex () << " , " << iterator.getRowIndex () << std::endl;
00158     point.z = 1.0f;
00159     ++iterator;
00160     ++idx;
00161   }
00162   
00163   int dx = x_end - x_start;
00164   int dy = y_end - y_start;
00165   unsigned dmax = std::max (abs(dx), abs(dy));
00166 
00167   if (neighorhood)
00168     EXPECT_EQ (dmax, idx);
00169   else
00170     EXPECT_EQ (abs(dx) + abs(dy), idx);
00171   
00172   float length = sqrt (float (dx * dx + dy * dy));
00173   float dir_x = float(dx) / length;
00174   float dir_y = float(dy) / length;
00175   
00176   // now all z-values should be 0 again!
00177   for (int yIdx = 0; yIdx < int(cloud.height); ++yIdx)
00178   {
00179     for (int xIdx = 0; xIdx < int(cloud.width); ++xIdx)
00180     {
00181       PointT& point = cloud.points [yIdx * cloud.width + xIdx];
00182       if (point.z != 0)
00183       {
00184         // point need to be close to line
00185         float distance = dir_x * float(yIdx - int(y_start)) - dir_y * float(xIdx - int(x_start));
00186         if (neighorhood)        
00187           EXPECT_LE (fabs(distance), 0.5f);
00188         else
00189           EXPECT_LE (fabs(distance), 0.70711f);
00190         
00191         // and within the endpoints
00192         float lambda = dir_y * float(yIdx - int(y_start)) + dir_x * float(xIdx - int(x_start));
00193         EXPECT_LE (lambda, length);
00194         EXPECT_GE (lambda, 0.0f);
00195       }
00196     }
00197   }
00198 }
00199 
00201 TEST (PCL, LineIterator8Neighbors)
00202 {
00203   PointCloud <PointXYZ> cloud;
00204   cloud.width = 100;
00205   cloud.height = 100;
00206   cloud.resize (cloud.width * cloud.height);  
00207   
00208   unsigned center_x = 50;
00209   unsigned center_y = 50;
00210   unsigned length = 45;
00211   
00212   // right
00213   checkSimpleLine8 (center_x, center_y, center_x + length, center_y, cloud);
00214   
00215   // left
00216   checkSimpleLine8 (center_x, center_y, center_x - length, center_y, cloud);
00217 
00218   // down
00219   checkSimpleLine8 (center_x, center_y, center_x, center_y - length, cloud);
00220   
00221   // up
00222   checkSimpleLine8 (center_x, center_y, center_x, center_y + length, cloud);
00223 
00224   // up-right
00225   checkSimpleLine8 (center_x, center_y, center_x + length, center_y + length, cloud);
00226   
00227   // up-left
00228   checkSimpleLine8 (center_x, center_y, center_x - length, center_y + length, cloud);
00229 
00230   // down-right
00231   checkSimpleLine8 (center_x, center_y, center_x + length, center_y - length, cloud);
00232 
00233   // down-left
00234   checkSimpleLine8 (center_x, center_y, center_x - length, center_y - length, cloud);
00235 }
00236 
00238 TEST (PCL, LineIterator8NeighborsGeneral)
00239 {
00240   PointCloud <PointXYZ> cloud;
00241   cloud.width = 100;
00242   cloud.height = 100;
00243   cloud.resize (cloud.width * cloud.height);  
00244   
00245   unsigned center_x = 50;
00246   unsigned center_y = 50;
00247   unsigned length = 45;
00248   
00249   const unsigned angular_resolution = 180;
00250   float d_alpha = float(M_PI / angular_resolution);
00251   for (unsigned idx = 0; idx < angular_resolution; ++idx)
00252   {
00253     unsigned x_end = unsigned (length * cos (float(idx) * d_alpha) + center_x + 0.5);
00254     unsigned y_end = unsigned (length * sin (float(idx) * d_alpha) + center_y + 0.5);
00255     
00256     // right
00257     checkGeneralLine (center_x, center_y, x_end, y_end, cloud, true);
00258   }
00259 }
00260 
00262 TEST (PCL, LineIterator4NeighborsGeneral)
00263 {
00264   PointCloud <PointXYZ> cloud;
00265   cloud.width = 100;
00266   cloud.height = 100;
00267   cloud.resize (cloud.width * cloud.height);  
00268   
00269   unsigned center_x = 50;
00270   unsigned center_y = 50;
00271   unsigned length = 45;
00272   
00273   const unsigned angular_resolution = 360;
00274   float d_alpha = float(2.0 * M_PI / angular_resolution);
00275   for (unsigned idx = 0; idx < angular_resolution; ++idx)
00276   {
00277     unsigned x_end = unsigned (length * cos (float(idx) * d_alpha) + center_x + 0.5);
00278     unsigned y_end = unsigned (length * sin (float(idx) * d_alpha) + center_y + 0.5);
00279     
00280     // right
00281     checkGeneralLine (center_x, center_y, x_end, y_end, cloud, false);
00282   }
00283 }
00284 
00285 //* ---[ */
00286 int
00287 main (int argc, char** argv)
00288 {
00289   testing::InitGoogleTest (&argc, argv);
00290   return (RUN_ALL_TESTS ());
00291 }
00292 /* ]--- */


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