00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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;
00046 static const double height = 0.5;
00047 static const double depth = 0.5;
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
00083
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
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
00117
00118
00119
00120
00121
00122
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
00130
00131
00132
00133 check_distance_field( df, points, numX, numY, numZ);
00134
00135
00136 points.clear();
00137 points.push_back(point1);
00138 df.updatePointsInField(points,true);
00139
00140 check_distance_field( df, points, numX, numY, numZ);
00141
00142
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
00150
00151 }
00152
00153 int main(int argc, char **argv){
00154 testing::InitGoogleTest(&argc, argv);
00155
00156 return RUN_ALL_TESTS();
00157 }