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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jan 17 2018 03:31:36