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 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 
00035 /* Author: Mrinal Kalakrishnan, Ken Anderson */
00036 
00037 #include <gtest/gtest.h>
00038 
00039 #include <moveit/distance_field/voxel_grid.h>
00040 #include <moveit/distance_field/propagation_distance_field.h>
00041 #include <moveit/distance_field/find_internal_points.h>
00042 #include <console_bridge/console.h>
00043 #include <geometric_shapes/body_operations.h>
00044 #include <eigen_conversions/eigen_msg.h>
00045 #include <octomap/octomap.h>
00046 #include <boost/make_shared.hpp>
00047 
00048 
00049 using namespace distance_field;
00050 
00051 static const double width = 1.0;
00052 static const double height = 1.0;
00053 static const double depth = 1.0;
00054 static const double resolution = 0.1;
00055 static const double origin_x = 0.0;
00056 static const double origin_y = 0.0;
00057 static const double origin_z = 0.0;
00058 static const double max_dist = 0.3;
00059 
00060 static const int max_dist_in_voxels = max_dist/resolution+0.5;
00061 static const int max_dist_sq_in_voxels = max_dist_in_voxels*max_dist_in_voxels;
00062 
00063 static const Eigen::Vector3d point1(0.1,0.0,0.0);
00064 static const Eigen::Vector3d point2(0.0,0.1,0.2);
00065 static const Eigen::Vector3d point3(0.4,0.0,0.0);
00066 
00067 int dist_sq(int x, int y, int z)
00068 {
00069   return x*x + y*y +z*z;
00070 }
00071 
00072 void print( PropagationDistanceField& pdf, int numX, int numY, int numZ)
00073 {
00074   for (int z=0; z<numZ; z++) {
00075     for (int y=0; y<numY; y++) {
00076       for (int x=0; x<numX; x++) {
00077         std::cout << pdf.getCell(x,y,z).distance_square_ << " ";
00078       }
00079       std::cout << std::endl;
00080     }
00081     std::cout << std::endl;
00082   }
00083   for (int z=0; z<numZ; z++) {
00084     for (int y=0; y<numY; y++) {
00085       for (int x=0; x<numX; x++) {
00086         if(pdf.getCell(x,y,z).distance_square_ == 0) {
00087           //logInform("Obstacle cell %d %d %d", x, y, z);
00088         }
00089       }
00090     }
00091   }
00092 }
00093 
00094 void printNeg(PropagationDistanceField& pdf, int numX, int numY, int numZ)
00095 {
00096   for (int z=0; z<numZ; z++) {
00097     for (int y=0; y<numY; y++) {
00098       for (int x=0; x<numX; x++) {
00099         std::cout << pdf.getCell(x,y,z).negative_distance_square_ << " ";
00100       }
00101       std::cout << std::endl;
00102     }
00103     std::cout << std::endl;
00104   }
00105 }
00106 
00107 void printPointCoords(const Eigen::Vector3i& p)
00108 {
00109   if (p.x() < 0)
00110     std::cout << '-';
00111   else
00112     std::cout << p.x();
00113 
00114   if (p.y() < 0)
00115     std::cout << '-';
00116   else
00117     std::cout << p.y();
00118 
00119   if (p.z() < 0)
00120     std::cout << '-';
00121   else
00122     std::cout << p.z();
00123 
00124   std::cout << " ";
00125 }
00126 
00127 void printBoth(PropagationDistanceField& pdf, int numX, int numY, int numZ)
00128 {
00129   std::cout << "Positive distance square ... negative distance square" << std::endl;
00130   for (int z=0; z<numZ; z++) {
00131     std::cout << "Z=" << z << std::endl;
00132     for (int y=0; y<numY; y++) {
00133       for (int x=0; x<numX; x++) {
00134         std::cout << pdf.getCell(x,y,z).distance_square_ << " ";
00135       }
00136       std::cout << "   ";
00137       for (int x=0; x<numX; x++) {
00138         std::cout << pdf.getCell(x,y,z).negative_distance_square_ << " ";
00139       }
00140       std::cout << "     ";
00141       for (int x=0; x<numX; x++) {
00142         printPointCoords(pdf.getCell(x,y,z).closest_point_);
00143       }
00144       std::cout << "   ";
00145       for (int x=0; x<numX; x++) {
00146         printPointCoords(pdf.getCell(x,y,z).closest_negative_point_);
00147       }
00148       std::cout << std::endl;
00149     }
00150     std::cout << std::endl;
00151   }
00152 }
00153 
00154 bool areDistanceFieldsDistancesEqual(const PropagationDistanceField& df1,
00155                                      const PropagationDistanceField& df2)
00156 {
00157   if(df1.getXNumCells() != df2.getXNumCells()) return false;
00158   if(df1.getYNumCells() != df2.getYNumCells()) return false;
00159   if(df1.getZNumCells() != df2.getZNumCells()) return false;
00160   for (int z=0; z<df1.getZNumCells(); z++) {
00161     for (int x=0; x<df1.getXNumCells(); x++) {
00162       for (int y=0; y<df1.getYNumCells(); y++) {
00163         if(df1.getCell(x,y,z).distance_square_ != df2.getCell(x,y,z).distance_square_) {
00164           printf("Cell %d %d %d distances not equal %d %d\n",x,y,z,
00165                  df1.getCell(x,y,z).distance_square_,df2.getCell(x,y,z).distance_square_);
00166           return false;
00167         }
00168         if(df1.getCell(x,y,z).negative_distance_square_ != df2.getCell(x,y,z).negative_distance_square_) {
00169           printf("Cell %d %d %d neg distances not equal %d %d\n",x,y,z,
00170                  df1.getCell(x,y,z).negative_distance_square_,df2.getCell(x,y,z).negative_distance_square_);
00171           return false;
00172         }
00173       }
00174     }
00175   }
00176   return true;
00177 }
00178 
00179 bool checkOctomapVersusDistanceField(const PropagationDistanceField& df,
00180                                      const octomap::OcTree& octree)
00181 {
00182   //just one way for now
00183   for (int z=0; z<df.getZNumCells(); z++) {
00184     for (int x=0; x<df.getXNumCells(); x++) {
00185       for (int y=0; y<df.getYNumCells(); y++) {
00186         if(df.getCell(x,y,z).distance_square_ == 0) {
00187           //making sure that octomap also shows it as occupied
00188           double qx, qy, qz;
00189           df.gridToWorld(x,y,z,
00190                          qx, qy, qz);
00191           octomap::point3d query(qx, qy, qz);
00192           octomap::OcTreeNode* result = octree.search(query);
00193           if(!result) {
00194             for(float boundary_x = query.x()-df.getResolution();
00195                 boundary_x <= query.x()+df.getResolution() && !result;
00196                 boundary_x += df.getResolution()) {
00197               for(float boundary_y = query.y()-df.getResolution();
00198                   boundary_y <= query.y()+df.getResolution() && !result;
00199                   boundary_y += df.getResolution()) {
00200                 for(float boundary_z = query.z()-df.getResolution();
00201                     boundary_z <= query.z()+df.getResolution();
00202                     boundary_z += df.getResolution()) {
00203                   octomap::point3d query_boundary(boundary_x, boundary_y, boundary_z);
00204                   result = octree.search(query_boundary);
00205                   if(result) {
00206                     break;
00207                   }
00208                 }
00209               }
00210             }
00211             if(!result) {
00212               std::cout << "No point at potential boundary query " << query.x() << " " << query.y() << " " << query.z() << std::endl;
00213               return false;
00214             }
00215           }
00216           if(!octree.isNodeOccupied(result)) {
00217             std::cout << "Disagreement at " << qx << " " << qy << " " << qz << std::endl;
00218             return false;
00219           }
00220         }
00221       }
00222     }
00223   }
00224   return true;
00225 }
00226 
00227 unsigned int countOccupiedCells(const PropagationDistanceField& df)
00228 {
00229   unsigned int count = 0;
00230   for (int z=0; z<df.getZNumCells(); z++) {
00231     for (int x=0; x<df.getXNumCells(); x++) {
00232       for (int y=0; y<df.getYNumCells(); y++) {
00233         if(df.getCell(x,y,z).distance_square_ == 0) {
00234           count++;
00235         }
00236       }
00237     }
00238   }
00239   return count;
00240 }
00241 
00242 unsigned int countLeafNodes(const octomap::OcTree& octree)
00243 {
00244   unsigned int count = 0;
00245   for(octomap::OcTree::leaf_iterator it = octree.begin_leafs(),
00246         end=octree.end_leafs(); it!= end; ++it)
00247   {
00248     if (octree.isNodeOccupied(*it))
00249     {
00250       std::cout << "Leaf node " << it.getX() << " " << it.getY() << " " << it.getZ() << std::endl;
00251       count++;
00252     }
00253   }
00254   return count;
00255 }
00256 
00257 //points should contain all occupied points
00258 void check_distance_field(const PropagationDistanceField & df,
00259                           const EigenSTL::vector_Vector3d& points,
00260                           int numX, int numY, int numZ,
00261                           bool do_negs)
00262 {
00263   std::vector<Eigen::Vector3i> points_ind(points.size());
00264   for(unsigned int i = 0; i < points.size(); i++) {
00265     Eigen::Vector3i loc;
00266     bool valid = df.worldToGrid(points[i].x(), points[i].y(), points[i].z(),
00267                                 loc.x(), loc.y(), loc.z() );
00268     points_ind[i] = loc;
00269   }
00270 
00271   for (int x=0; x<numX; x++) {
00272     for (int y=0; y<numY; y++) {
00273       for (int z=0; z<numZ; z++) {
00274         double dsq = df.getCell(x, y, z).distance_square_;
00275         double ndsq = df.getCell(x, y, z).negative_distance_square_;
00276         if(dsq == 0) {
00277           bool found = false;
00278           for(unsigned int i = 0; i < points_ind.size(); i++) {
00279             if(points_ind[i].x() == x && points_ind[i].y() == y && points_ind[i].z() == z) {
00280               found = true;
00281               break;
00282             }
00283           }
00284           if(do_negs) {
00285             ASSERT_GT(ndsq, 0) << "Obstacle point " << x << " " << y << " " << z << " has zero negative value";
00286           }
00287           ASSERT_TRUE(found) << "Obstacle point " << x << " " << y << " " << z << " not found";
00288         }
00289       }
00290     }
00291   }
00292 }
00293 
00294 
00295 TEST(TestPropagationDistanceField, TestAddRemovePoints)
00296 {
00297   PropagationDistanceField df( width, height, depth, resolution, origin_x, origin_y, origin_z, max_dist);
00298 
00299   // Check size
00300   int numX = df.getXNumCells();
00301   int numY = df.getYNumCells();
00302   int numZ = df.getZNumCells();
00303 
00304   EXPECT_EQ( numX, (int)(width/resolution+0.5) );
00305   EXPECT_EQ( numY, (int)(height/resolution+0.5) );
00306   EXPECT_EQ( numZ, (int)(depth/resolution+0.5) );
00307 
00308   //getting a bad point
00309   double tgx, tgy, tgz;
00310   bool in_bounds;
00311   EXPECT_NEAR(df.getDistance(1000.0,1000.0,1000.0), max_dist, .0001);
00312   EXPECT_NEAR(df.getDistanceGradient(1000.0,1000.0,1000.0, tgx, tgy, tgz, in_bounds), max_dist, .0001);
00313   EXPECT_FALSE(in_bounds);
00314 
00315   // Add points to the grid
00316   EigenSTL::vector_Vector3d points;
00317   points.push_back(point1);
00318   points.push_back(point2);
00319   logInform("Adding %u points", points.size());
00320   df.addPointsToField(points);
00321   //print(df, numX, numY, numZ);
00322 
00323   // Update - iterative
00324   points.clear();
00325   points.push_back(point2);
00326   points.push_back(point3);
00327   EigenSTL::vector_Vector3d old_points;
00328   old_points.push_back(point1);
00329   df.updatePointsInField(old_points, points);
00330   //std::cout << "One removal, one addition" << std::endl;
00331   //print(df, numX, numY, numZ);
00332   //printNeg(df, numX, numY, numZ);
00333   check_distance_field(df, points, numX, numY, numZ, false);
00334 
00335   // Remove
00336   points.clear();
00337   points.push_back(point2);
00338   df.removePointsFromField(points);
00339   points.clear();
00340   points.push_back(point3);
00341   check_distance_field( df, points, numX, numY, numZ, false);
00342 
00343   //now testing gradient calls
00344   df.reset();
00345   points.clear();
00346   points.push_back(point1);
00347   df.addPointsToField(points);
00348   bool first = true;
00349   for (int z=1; z<df.getZNumCells()-1; z++) {
00350     for (int x=1; x<df.getXNumCells()-1; x++) {
00351       for (int y=1; y<df.getYNumCells()-1; y++) {
00352         double dist = df.getDistance(x,y,z);
00353         double wx, wy, wz;
00354         df.gridToWorld(x,y,z,wx,wy,wz);
00355         Eigen::Vector3d grad(0.0,0.0,0.0);
00356         bool grad_in_bounds;
00357         double dist_grad = df.getDistanceGradient(wx, wy, wz,
00358                                                   grad.x(), grad.y(), grad.z(), grad_in_bounds);
00359         ASSERT_TRUE(grad_in_bounds) << x << " " << y << " " << z;
00360         ASSERT_NEAR(dist, dist_grad, .0001);
00361         if(dist > 0 &&
00362            dist < max_dist) {
00363           double xscale = grad.x()/grad.norm();
00364           double yscale = grad.y()/grad.norm();
00365           double zscale = grad.z()/grad.norm();
00366 
00367           double comp_x = wx-xscale*dist;
00368           double comp_y = wy-yscale*dist;
00369           double comp_z = wz-zscale*dist;
00370           if(first) {
00371             first = false;
00372             std::cout << "Dist " << dist << std::endl;
00373             std::cout << "Cell " << x << " " << y << " " << z << " " << wx << " " << wy << " " << wz << std::endl;
00374             std::cout << "Scale " << xscale << " " << yscale << " " << zscale << std::endl;
00375             std::cout << "Grad " << grad.x() << " " << grad.y() << " " << grad.z() << " comp " << comp_x << " " << comp_y << " " << comp_z << std::endl;
00376           }
00377           ASSERT_NEAR(comp_x, point1.x(), resolution) << dist << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() << " " << xscale << " " << yscale << " " << zscale << std::endl;
00378           ASSERT_NEAR(comp_y, point1.y(), resolution) << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() << " " << xscale << " " << yscale << " " << zscale << std::endl;
00379           ASSERT_NEAR(comp_z, point1.z(), resolution) << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() << " " << xscale << " " << yscale << " " << zscale << std::endl;
00380         }
00381       }
00382     }
00383   }
00384   ASSERT_FALSE(first);
00385 }
00386 
00387 TEST(TestSignedPropagationDistanceField, TestSignedAddRemovePoints)
00388 {
00389 
00390   PropagationDistanceField df( width, height, depth, resolution, origin_x, origin_y, origin_z, max_dist, true);
00391 
00392   // Check size
00393   int numX = df.getXNumCells();
00394   int numY = df.getYNumCells();
00395   int numZ = df.getZNumCells();
00396 
00397   EXPECT_EQ( numX, (int)(width/resolution+0.5) );
00398   EXPECT_EQ( numY, (int)(height/resolution+0.5) );
00399   EXPECT_EQ( numZ, (int)(depth/resolution+0.5) );
00400 
00401   // Error checking
00402   //print(df, numX, numY, numZ);
00403 
00404   // TODO - check initial values
00405   //EXPECT_EQ( df.getCell(0,0,0).distance_square_, max_dist_sq_in_voxels );
00406 
00407   // Add points to the grid
00408   double lwx, lwy, lwz;
00409   double hwx, hwy, hwz;
00410   df.gridToWorld(1,1,1,lwx,lwy,lwz);
00411   df.gridToWorld(8,8,8,hwx,hwy,hwz);
00412 
00413   EigenSTL::vector_Vector3d points;
00414   for(double x = lwx; x <= hwx; x+= .1) {
00415     for(double y = lwy; y <= hwy; y+= .1) {
00416       for(double z = lwz; z <= hwz; z+= .1) {
00417         points.push_back(Eigen::Vector3d(x,y,z));
00418       }
00419     }
00420   }
00421 
00422   df.reset();
00423   logInform("Adding %u points", points.size());
00424   df.addPointsToField(points);
00425   //print(df, numX, numY, numZ);
00426   //printNeg(df, numX, numY, numZ);
00427 
00428   double cx, cy, cz;
00429   df.gridToWorld(5,5,5,cx,cy,cz);
00430 
00431   Eigen::Vector3d center_point(cx,cy,cz);
00432 
00433   EigenSTL::vector_Vector3d rem_points;
00434   rem_points.push_back(center_point);
00435   df.removePointsFromField(rem_points);
00436   //std::cout << "Pos "<< std::endl;
00437   //print(df, numX, numY, numZ);
00438   //std::cout << "Neg "<< std::endl;
00439   //printNeg(df, numX, numY, numZ);
00440 
00441   //testing equality with initial add of points without the center point
00442   PropagationDistanceField test_df( width, height, depth, resolution, origin_x, origin_y, origin_z, max_dist, true);
00443   EigenSTL::vector_Vector3d test_points;
00444   for(unsigned int i = 0; i < points.size(); i++) {
00445     if(points[i].x() != center_point.x() ||
00446        points[i].y() != center_point.y() ||
00447        points[i].z() != center_point.z())
00448     {
00449       test_points.push_back(points[i]);
00450     }
00451   }
00452   test_df.addPointsToField(test_points);
00453   ASSERT_TRUE(areDistanceFieldsDistancesEqual(df, test_df));
00454 
00455   PropagationDistanceField gradient_df( width, height, depth, resolution, origin_x, origin_y, origin_z, max_dist, true);
00456 
00457   shapes::Sphere sphere(.25);
00458 
00459   geometry_msgs::Pose p;
00460   p.orientation.w = 1.0;
00461   p.position.x = .5;
00462   p.position.y = .5;
00463   p.position.z = .5;
00464 
00465   gradient_df.addShapeToField(&sphere, p);
00466   printBoth(gradient_df, numX, numY, numZ);
00467   EXPECT_GT(gradient_df.getCell(5,5,5).negative_distance_square_, 1);
00468   //all negative cells should have gradients that point towards cells with distance 1
00469   for (int z=1; z<df.getZNumCells()-1; z++) {
00470     for (int x=1; x<df.getXNumCells()-1; x++) {
00471       for (int y=1; y<df.getYNumCells()-1; y++) {
00472         
00473         double dist = gradient_df.getDistance(x,y,z);
00474         double ncell_dist;
00475         Eigen::Vector3i ncell_pos;
00476         const PropDistanceFieldVoxel* ncell = gradient_df.getNearestCell(x,y,z, ncell_dist, ncell_pos);
00477 
00478         EXPECT_EQ(ncell_dist, dist);
00479 
00480         if (ncell == NULL)
00481         {
00482           if (ncell_dist > 0)
00483           {
00484             EXPECT_GE(ncell_dist, gradient_df.getUninitializedDistance()) << "dist=" << dist << " xyz=" << x << " " << y << " " << z << " ncell=" << ncell_pos.x() << " " << ncell_pos.y() << " " << ncell_pos.z() << std::endl;
00485 
00486           }
00487           else if (ncell_dist < 0)
00488           {
00489             EXPECT_LE(ncell_dist, -gradient_df.getUninitializedDistance()) << "dist=" << dist << " xyz=" << x << " " << y << " " << z << " ncell=" << ncell_pos.x() << " " << ncell_pos.y() << " " << ncell_pos.z() << std::endl;
00490           }
00491         }
00492 
00493         if(gradient_df.getCell(x,y,z).negative_distance_square_ > 0) {
00494           ASSERT_LT(dist, 0) << "Pos " << gradient_df.getCell(x,y,z).distance_square_ << " " << gradient_df.getCell(x,y,z).negative_distance_square_;
00495           double wx, wy, wz;
00496           df.gridToWorld(x,y,z,wx,wy,wz);
00497           Eigen::Vector3d grad(0.0,0.0,0.0);
00498           bool grad_in_bounds;
00499           double dist_grad = gradient_df.getDistanceGradient(wx, wy, wz,
00500                                                              grad.x(), grad.y(), grad.z(), grad_in_bounds);
00501           ASSERT_TRUE(grad_in_bounds) << x << " " << y << " " << z;
00502           ASSERT_NEAR(dist, dist_grad, .0001);
00503 
00504           if (!ncell)
00505             continue;
00506 
00507           EXPECT_GE(gradient_df.getCell(ncell_pos.x(), ncell_pos.y(), ncell_pos.z()).distance_square_, 1) << "dist=" << dist << " xyz=" << x << " " << y << " " << z << " grad=" << grad.x() << " " << grad.y() << " " << grad.z() << " ncell=" << ncell_pos.x() << " " << ncell_pos.y() << " " << ncell_pos.z() << std::endl;
00508 
00509           double grad_size_sq = grad.squaredNorm();
00510           if (grad_size_sq < std::numeric_limits<double>::epsilon())
00511             continue;
00512 
00513           double oo_grad_size = 1.0 / sqrt(grad_size_sq);
00514           double xscale = grad.x() * oo_grad_size;
00515           double yscale = grad.y() * oo_grad_size;
00516           double zscale = grad.z() * oo_grad_size;
00517 
00518           double comp_x = wx-xscale*dist;
00519           double comp_y = wy-yscale*dist;
00520           double comp_z = wz-zscale*dist;
00521 
00522           int cell_x, cell_y, cell_z;
00523           bool cell_in_bounds = gradient_df.worldToGrid(comp_x, comp_y, comp_z,
00524                                   cell_x, cell_y, cell_z);
00525 
00526           ASSERT_EQ(cell_in_bounds, true);
00527           const PropDistanceFieldVoxel* cell = &gradient_df.getCell(cell_x, cell_y, cell_z);
00528 
00529 #if 0
00530           EXPECT_EQ(ncell_pos.x(), cell_x);
00531           EXPECT_EQ(ncell_pos.y(), cell_y);
00532           EXPECT_EQ(ncell_pos.z(), cell_z);
00533           EXPECT_EQ(ncell, cell);
00534 #endif
00535           EXPECT_GE(cell->distance_square_, 1) << dist << " " << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() << " " << xscale << " " << yscale << " " << zscale << " cell " << comp_x << " " << comp_y << " " << comp_z << std::endl;
00536         }
00537       }
00538     }
00539   }
00540 }
00541 
00542 
00543 TEST(TestSignedPropagationDistanceField, TestShape)
00544 {
00545 
00546   PropagationDistanceField df( width, height, depth, resolution, origin_x, origin_y, origin_z, max_dist, true);
00547 
00548   int numX = df.getXNumCells();
00549   int numY = df.getYNumCells();
00550   int numZ = df.getZNumCells();
00551 
00552   shapes::Sphere sphere(.25);
00553 
00554   geometry_msgs::Pose p;
00555   p.orientation.w = 1.0;
00556   p.position.x = .5;
00557   p.position.y = .5;
00558   p.position.z = .5;
00559 
00560   geometry_msgs::Pose np;
00561   np.orientation.w = 1.0;
00562   np.position.x = .7;
00563   np.position.y = .7;
00564   np.position.z = .7;
00565 
00566   df.addShapeToField(&sphere, p);
00567 
00568   bodies::Body* body = bodies::createBodyFromShape(&sphere);
00569   Eigen::Affine3d pose_e;
00570   tf::poseMsgToEigen(p, pose_e);
00571   body->setPose(pose_e);
00572   EigenSTL::vector_Vector3d point_vec;
00573   findInternalPointsConvex(*body, resolution, point_vec);
00574   delete body;
00575   check_distance_field(df, point_vec,
00576                        numX, numY, numZ, true);
00577 
00578   // std::cout << "Shape pos "<< std::endl;
00579   // print(df, numX, numY, numZ);
00580   // std::cout << "Shape neg "<< std::endl;
00581   // printNeg(df, numX, numY, numZ);
00582 
00583   df.addShapeToField(&sphere, np);
00584 
00585   body = bodies::createBodyFromShape(&sphere);
00586   Eigen::Affine3d npose_e;
00587   tf::poseMsgToEigen(np, npose_e);
00588   body->setPose(npose_e);
00589 
00590   EigenSTL::vector_Vector3d point_vec_2;
00591   findInternalPointsConvex(*body, resolution, point_vec_2);
00592   delete body;
00593   EigenSTL::vector_Vector3d point_vec_union = point_vec_2;
00594   point_vec_union.insert(point_vec_union.end(),
00595                          point_vec.begin(),
00596                          point_vec.end());
00597   check_distance_field(df, point_vec_union,
00598                        numX, numY, numZ, true);
00599 
00600   //should get rid of old pose
00601   df.moveShapeInField(&sphere, p, np);
00602 
00603   check_distance_field(df, point_vec_2,
00604                        numX, numY, numZ, true);
00605 
00606   //should be equivalent to just adding second shape
00607   PropagationDistanceField test_df( width, height, depth, resolution, origin_x, origin_y, origin_z, max_dist, true);
00608   test_df.addShapeToField(&sphere, np);
00609   ASSERT_TRUE(areDistanceFieldsDistancesEqual(df, test_df));
00610 }
00611 
00612 static const double PERF_WIDTH = 3.0;
00613 static const double PERF_HEIGHT = 3.0;
00614 static const double PERF_DEPTH = 4.0;
00615 static const double PERF_RESOLUTION = 0.02;
00616 static const double PERF_ORIGIN_X = 0.0;
00617 static const double PERF_ORIGIN_Y = 0.0;
00618 static const double PERF_ORIGIN_Z = 0.0;
00619 static const double PERF_MAX_DIST = .25;
00620 static const unsigned int UNIFORM_DISTANCE = 10;
00621 
00622 TEST(TestSignedPropagationDistanceField, TestPerformance)
00623 {
00624   std::cout << "Creating distance field with " <<
00625     (PERF_WIDTH/PERF_RESOLUTION)*(PERF_HEIGHT/PERF_RESOLUTION)*(PERF_DEPTH/PERF_RESOLUTION)
00626             << " entries" << std::endl;
00627 
00628   ros::WallTime dt = ros::WallTime::now();
00629   PropagationDistanceField df( PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
00630                                PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, false);
00631   std::cout << "Creating unsigned took " << (ros::WallTime::now()-dt).toSec() << std::endl;
00632 
00633   dt = ros::WallTime::now();
00634   PropagationDistanceField sdf( PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
00635                                 PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, true);
00636 
00637   std::cout << "Creating signed took " << (ros::WallTime::now()-dt).toSec() << std::endl;
00638 
00639   shapes::Box big_table(2.0,2.0,.5);
00640 
00641   geometry_msgs::Pose p;
00642   p.orientation.w = 1.0;
00643   p.position.x = PERF_WIDTH/2.0;
00644   p.position.y = PERF_DEPTH/2.0;
00645   p.position.z = PERF_HEIGHT/2.0;
00646 
00647   geometry_msgs::Pose np;
00648   np.orientation.w = 1.0;
00649   np.position.x = p.position.x+.01;
00650   np.position.y = p.position.y;
00651   np.position.z = p.position.z;
00652 
00653   unsigned int big_num_points = ceil(2.0/PERF_RESOLUTION)*ceil(2.0/PERF_RESOLUTION)*ceil(.5/PERF_RESOLUTION);
00654 
00655   std::cout << "Adding " << big_num_points << " points" << std::endl;
00656 
00657   dt = ros::WallTime::now();
00658   df.addShapeToField(&big_table, p);
00659   std::cout << "Adding to unsigned took "
00660             << (ros::WallTime::now()-dt).toSec()
00661             << " avg " << (ros::WallTime::now()-dt).toSec()/(big_num_points*1.0)
00662             << std::endl;
00663 
00664   dt = ros::WallTime::now();
00665   df.addShapeToField(&big_table, p);
00666   std::cout << "Re-adding to unsigned took " << (ros::WallTime::now()-dt).toSec() << std::endl;
00667 
00668   dt = ros::WallTime::now();
00669   sdf.addShapeToField(&big_table, p);
00670   std::cout << "Adding to signed took "
00671             << (ros::WallTime::now()-dt).toSec()
00672             << " avg " << (ros::WallTime::now()-dt).toSec()/(big_num_points*1.0)
00673             << std::endl;
00674 
00675   dt = ros::WallTime::now();
00676   df.moveShapeInField(&big_table, p, np);
00677   std::cout << "Moving in unsigned took " << (ros::WallTime::now()-dt).toSec() << std::endl;
00678 
00679   dt = ros::WallTime::now();
00680   sdf.moveShapeInField(&big_table, p, np);
00681   std::cout << "Moving in signed took " << (ros::WallTime::now()-dt).toSec() << std::endl;
00682 
00683   dt = ros::WallTime::now();
00684   df.removeShapeFromField(&big_table, np);
00685   std::cout << "Removing from unsigned took " << (ros::WallTime::now()-dt).toSec() << std::endl;
00686 
00687   dt = ros::WallTime::now();
00688   sdf.removeShapeFromField(&big_table, np);
00689   std::cout << "Removing from signed took " << (ros::WallTime::now()-dt).toSec() << std::endl;
00690 
00691   dt = ros::WallTime::now();
00692   df.reset();
00693 
00694   shapes::Box small_table(.25,.25,.05);
00695 
00696   unsigned int small_num_points = (13)*(13)*(3);
00697 
00698   std::cout << "Adding " << small_num_points << " points" << std::endl;
00699 
00700   dt = ros::WallTime::now();
00701   df.addShapeToField(&small_table, p);
00702   std::cout << "Adding to unsigned took "
00703             << (ros::WallTime::now()-dt).toSec()
00704             << " avg " << (ros::WallTime::now()-dt).toSec()/(small_num_points*1.0)
00705             << std::endl;
00706 
00707   dt = ros::WallTime::now();
00708   sdf.addShapeToField(&small_table, p);
00709   std::cout << "Adding to signed took "
00710             << (ros::WallTime::now()-dt).toSec()
00711             << " avg " << (ros::WallTime::now()-dt).toSec()/(small_num_points*1.0)
00712             << std::endl;
00713 
00714   dt = ros::WallTime::now();
00715   df.moveShapeInField(&small_table, p, np);
00716   std::cout << "Moving in unsigned took " << (ros::WallTime::now()-dt).toSec() << std::endl;
00717 
00718   dt = ros::WallTime::now();
00719   sdf.moveShapeInField(&small_table, p, np);
00720   std::cout << "Moving in signed took " << (ros::WallTime::now()-dt).toSec() << std::endl;
00721 
00722   //uniformly spaced points - a worst case scenario
00723   PropagationDistanceField worstdfu(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
00724                                    PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, false);
00725 
00726   PropagationDistanceField worstdfs(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
00727                                    PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, true);
00728 
00729 
00730 
00731   EigenSTL::vector_Vector3d bad_vec;
00732   unsigned int count = 0;
00733   for(unsigned int z = UNIFORM_DISTANCE; z < worstdfu.getZNumCells()-UNIFORM_DISTANCE; z += UNIFORM_DISTANCE) {
00734     for(unsigned int x = UNIFORM_DISTANCE; x < worstdfu.getXNumCells()-UNIFORM_DISTANCE; x += UNIFORM_DISTANCE) {
00735       for(unsigned int y = UNIFORM_DISTANCE; y < worstdfu.getYNumCells()-UNIFORM_DISTANCE; y += UNIFORM_DISTANCE) {
00736         count++;
00737         Eigen::Vector3d loc;
00738         bool valid = worstdfu.gridToWorld(x,y,z,
00739                                           loc.x(), loc.y(), loc.z());
00740 
00741         if(!valid) {
00742           logWarn("Something wrong");
00743           continue;
00744         }
00745         bad_vec.push_back(loc);
00746       }
00747     }
00748   }
00749 
00750   dt = ros::WallTime::now();
00751   worstdfu.addPointsToField(bad_vec);
00752   ros::WallDuration wd = ros::WallTime::now()-dt;
00753   printf("Time for unsigned adding %u uniform points is %g average %g\n", (unsigned int)bad_vec.size(), wd.toSec(), wd.toSec()/(bad_vec.size()*1.0));
00754   dt = ros::WallTime::now();
00755   worstdfs.addPointsToField(bad_vec);
00756   wd = ros::WallTime::now()-dt;
00757   printf("Time for signed adding %u uniform points is %g average %g\n", (unsigned int)bad_vec.size(), wd.toSec(), wd.toSec()/(bad_vec.size()*1.0));
00758 
00759 }
00760 
00761 TEST(TestSignedPropagationDistanceField, TestOcTree)
00762 {
00763   PropagationDistanceField df(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
00764                               PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, false);
00765 
00766   octomap::OcTree tree(.02);
00767 
00768   unsigned int count = 0;
00769   for(float x = 1.01; x < 1.5; x += .02) {
00770     for(float y = 1.01; y < 1.5; y += .02) {
00771       for(float z = 1.01; z < 1.5; z += .02, count++) {
00772         octomap::point3d point(x,y,z);
00773         tree.updateNode(point, true);
00774       }
00775     }
00776   }
00777 
00778   //more points at the border of the distance field
00779   for(float x = 2.51; x < 3.5; x += .02) {
00780     for(float y = 1.01; y < 3.5; y += .02) {
00781       for(float z = 1.01; z < 3.5; z += .02, count++) {
00782         octomap::point3d point(x,y,z);
00783         tree.updateNode(point, true);
00784       }
00785     }
00786   }
00787 
00788   std::cout << "OcTree nodes " << count << std::endl;
00789   df.addOcTreeToField(&tree);
00790 
00791   EXPECT_TRUE(checkOctomapVersusDistanceField(df, tree));
00792 
00793   //more cells
00794   for(float x = .01; x < .50; x += .02) {
00795     for(float y = .01; y < .50; y += .02) {
00796       for(float z = .01; z < .50; z += .02, count++) {
00797         octomap::point3d point(x,y,z);
00798         tree.updateNode(point, true);
00799       }
00800     }
00801   }
00802   df.addOcTreeToField(&tree);
00803   EXPECT_TRUE(checkOctomapVersusDistanceField(df, tree));
00804 
00805   PropagationDistanceField df_oct(tree,
00806                                  octomap::point3d(0.5,0.5,0.5),
00807                                  octomap::point3d(5.0,5.0,5.0),
00808                                  PERF_MAX_DIST,
00809                                  false);
00810 
00811   EXPECT_TRUE(checkOctomapVersusDistanceField(df_oct, tree));
00812 
00813   //now try different resolutions
00814   octomap::OcTree tree_lowres(.05);
00815   octomap::point3d point1(.5,.5,.5);
00816   octomap::point3d point2(.7,.5,.5);
00817   octomap::point3d point3(1.0,.5,.5);
00818   tree_lowres.updateNode(point1, true);
00819   tree_lowres.updateNode(point2, true);
00820   tree_lowres.updateNode(point3, true);
00821   ASSERT_EQ(countLeafNodes(tree_lowres), 3);
00822 
00823   PropagationDistanceField df_highres(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
00824                                       PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, false);
00825 
00826   df_highres.addOcTreeToField(&tree_lowres);
00827   EXPECT_EQ(countOccupiedCells(df_highres), 3*(4*4*4));
00828   std::cout << "Occupied cells " << countOccupiedCells(df_highres) << std::endl;
00829 
00830   //testing adding shape that happens to be octree
00831   boost::shared_ptr<octomap::OcTree> tree_shape(new octomap::OcTree(.05));
00832   octomap::point3d tpoint1(1.0,.5,1.0);
00833   octomap::point3d tpoint2(1.7,.5,.5);
00834   octomap::point3d tpoint3(1.8,.5,.5);
00835   tree_shape->updateNode(tpoint1, true);
00836   tree_shape->updateNode(tpoint2, true);
00837   tree_shape->updateNode(tpoint3, true);
00838 
00839   boost::shared_ptr<shapes::OcTree> shape_oc(new shapes::OcTree(tree_shape));
00840 
00841   PropagationDistanceField df_test_shape_1(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
00842                                            PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, false);
00843 
00844   PropagationDistanceField df_test_shape_2(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
00845                                            PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, false);
00846 
00847   df_test_shape_1.addOcTreeToField(tree_shape.get());
00848   df_test_shape_2.addShapeToField(shape_oc.get(), geometry_msgs::Pose());
00849   EXPECT_TRUE(areDistanceFieldsDistancesEqual(df_test_shape_1, df_test_shape_2));
00850 }
00851 
00852 TEST(TestSignedPropagationDistanceField, TestReadWrite)
00853 {
00854 
00855   PropagationDistanceField small_df( width, height, depth, resolution, origin_x, origin_y, origin_z, max_dist);
00856 
00857   EigenSTL::vector_Vector3d points;
00858   points.push_back(point1);
00859   points.push_back(point2);
00860   points.push_back(point3);
00861   small_df.addPointsToField(points);
00862 
00863   std::ofstream sf("test_small.df", std::ios::out);
00864 
00865   small_df.writeToStream(sf);
00866   //must close to make sure that the buffer is written
00867   sf.close();
00868 
00869   std::ifstream si("test_small.df", std::ios::in | std::ios::binary);
00870   PropagationDistanceField small_df2(si, PERF_MAX_DIST, false);
00871   ASSERT_TRUE(areDistanceFieldsDistancesEqual(small_df, small_df2));
00872 
00873   PropagationDistanceField df(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
00874                               PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, false);
00875 
00876   shapes::Sphere sphere(.5);
00877 
00878   geometry_msgs::Pose p;
00879   p.orientation.w = 1.0;
00880   p.position.x = .5;
00881   p.position.y = .5;
00882   p.position.z = .5;
00883 
00884   df.addShapeToField(&sphere, p);
00885 
00886   std::ofstream f("test_big.df", std::ios::out);
00887 
00888   df.writeToStream(f);
00889   f.close();
00890 
00891   std::ifstream i("test_big.df", std::ios::in);
00892   PropagationDistanceField df2(i, PERF_MAX_DIST, false);
00893   EXPECT_TRUE(areDistanceFieldsDistancesEqual(df, df2));
00894 
00895   //we shouldn't segfault if we start with an old ifstream
00896   PropagationDistanceField dfx(i, PERF_MAX_DIST, false);
00897 
00898   std::ifstream i2("test_big.df", std::ios::in);
00899   ros::WallTime wt = ros::WallTime::now();
00900   PropagationDistanceField df3(i2, PERF_MAX_DIST+.02, false);
00901   std::cout << "Reconstruction for big file took " << (ros::WallTime::now()-wt).toSec() << std::endl;
00902   EXPECT_FALSE(areDistanceFieldsDistancesEqual(df, df3));
00903 }
00904 
00905 int main(int argc, char **argv){
00906   testing::InitGoogleTest(&argc, argv);
00907 
00908   return RUN_ALL_TESTS();
00909 }
00910 


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:53