test_distance_field.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 the Willow Garage 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 
00037 #include <gtest/gtest.h>
00038 
00039 #include <distance_field/voxel_grid.h>
00040 #include <distance_field/propagation_distance_field.h>
00041 #include <ros/ros.h>
00042 
00043 using namespace distance_field;
00044 
00045 static const double width = 0.5;//2.0;
00046 static const double height = 0.5;//2.0;
00047 static const double depth = 0.5;//1.0;
00048 static const double resolution = 0.1;
00049 static const double origin_x = 0.0;
00050 static const double origin_y = 0.0;
00051 static const double origin_z = 0.0;
00052 static const double max_dist = 0.3;
00053 
00054 static const int max_dist_in_voxels = max_dist/resolution+0.5;
00055 static const int max_dist_sq_in_voxels = max_dist_in_voxels*max_dist_in_voxels;
00056 
00057 static const tf::Vector3 point1(0.0,0.0,0.0);
00058 static const tf::Vector3 point2(0.2,0.2,0.2);
00059 
00060 
00061 int dist_sq(int x, int y, int z)
00062 {
00063   return x*x + y*y +z*z;
00064 }
00065 
00066 void print( PropagationDistanceField& pdf, int numX, int numY, int numZ)
00067 {
00068   for (int x=0; x<numX; x++) {
00069     for (int y=0; y<numY; y++) {
00070       for (int z=0; z<numZ; z++) {
00071         std::cout << pdf.getCell(x,y,z).distance_square_ << " ";
00072       }
00073       std::cout << std::endl;
00074     }
00075     std::cout << std::endl;
00076   }
00077 }
00078 
00079 
00080 void check_distance_field(const PropagationDistanceField & df, const std::vector<tf::Vector3>& points, int numX, int numY, int numZ)
00081 {
00082   // Check after adding point(s)
00083   // Fairly heavy computation.  Try to keep voxel grid small when doing this test
00084   for (int x=0; x<numX; x++) {
00085     for (int y=0; y<numY; y++) {
00086       for (int z=0; z<numZ; z++) {
00087         int min_dist_square = max_dist_sq_in_voxels;
00088         for( unsigned int i=0; i<points.size(); i++) {
00089           int dx = points[i].x()/resolution - x;
00090           int dy = points[i].y()/resolution - y;
00091           int dz = points[i].z()/resolution - z;
00092           int dist_square = dist_sq(dx,dy,dz);
00093           min_dist_square = std::min(dist_square, min_dist_square);
00094         }
00095         ASSERT_EQ(df.getCell(x,y,z).distance_square_, min_dist_square);
00096       }
00097     }
00098   }
00099 }
00100 
00101 
00102 TEST(TestPropagationDistanceField, TestAddPoints)
00103 {
00104 
00105   PropagationDistanceField df( width, height, depth, resolution, origin_x, origin_y, origin_z, max_dist);
00106 
00107   // Check size
00108   int numX = df.getNumCells(PropagationDistanceField::DIM_X);
00109   int numY = df.getNumCells(PropagationDistanceField::DIM_Y);
00110   int numZ = df.getNumCells(PropagationDistanceField::DIM_Z);
00111 
00112   EXPECT_EQ( numX, (int)(width/resolution+0.5) );
00113   EXPECT_EQ( numY, (int)(height/resolution+0.5) );
00114   EXPECT_EQ( numZ, (int)(depth/resolution+0.5) );
00115 
00116   // Error checking
00117   //print(df, numX, numY, numZ);
00118 
00119   // TODO - check initial values
00120   //EXPECT_EQ( df.getCell(0,0,0).distance_square_, max_dist_sq_in_voxels );
00121 
00122   // Add points to the grid
00123   std::vector<tf::Vector3> points;
00124   points.push_back(point1);
00125   points.push_back(point2);
00126   df.reset();
00127   df.updatePointsInField(points);
00128 
00129   // Error checking
00130   //print(df, numX, numY, numZ);
00131 
00132   // Check correctness
00133   check_distance_field( df, points, numX, numY, numZ);
00134 
00135         // Update - iterative
00136   points.clear();
00137   points.push_back(point1);
00138   df.updatePointsInField(points,true);
00139   //print(df, numX, numY, numZ);
00140   check_distance_field( df, points, numX, numY, numZ);
00141 
00142         // Update - not iterative
00143   points.clear();
00144   points.push_back(point2);
00145   df.updatePointsInField(points,false);
00146   print(df, numX, numY, numZ);
00147   check_distance_field( df, points, numX, numY, numZ);
00148 
00149   // TODO - test gradient and closest point location
00150 
00151 }
00152 
00153 int main(int argc, char **argv){
00154   testing::InitGoogleTest(&argc, argv);
00155 
00156   return RUN_ALL_TESTS();
00157 }


distance_field
Author(s): Mrinal Kalakrishnan / mail@mrinal.net
autogenerated on Thu Dec 12 2013 11:08:36