test_distance_field.cpp

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2009, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

#include <gtest/gtest.h>

#include <moveit/distance_field/voxel_grid.h>
#include <moveit/distance_field/propagation_distance_field.h>
#include <moveit/distance_field/distance_field_common.h>
#include <console_bridge/console.h>
#include <geometric_shapes/body_operations.h>
#include <eigen_conversions/eigen_msg.h>
#include <octomap/octomap.h>
#include <boost/make_shared.hpp>

using namespace distance_field;

static const double width = 1.0;
static const double height = 1.0;
static const double depth = 1.0;
static const double resolution = 0.1;
static const double origin_x = 0.0;
static const double origin_y = 0.0;
static const double origin_z = 0.0;
static const double max_dist = 0.3;

static const int max_dist_in_voxels = max_dist/resolution+0.5;
static const int max_dist_sq_in_voxels = max_dist_in_voxels*max_dist_in_voxels;

static const Eigen::Vector3d point1(0.1,0.0,0.0);
static const Eigen::Vector3d point2(0.0,0.1,0.2);
static const Eigen::Vector3d point3(0.4,0.0,0.0);

int dist_sq(int x, int y, int z)
{
  return x*x + y*y +z*z;
}

void print( PropagationDistanceField& pdf, int numX, int numY, int numZ)
{
  for (int z=0; z<numZ; z++) {
    for (int x=0; x<numX; x++) {
      for (int y=0; y<numY; y++) {
        std::cout << pdf.getCell(x,y,z).distance_square_ << " ";
      }
      std::cout << std::endl;
    }
    std::cout << std::endl;
  }
  for (int z=0; z<numZ; z++) {
    for (int x=0; x<numX; x++) {
      for (int y=0; y<numY; y++) {
        if(pdf.getCell(x,y,z).distance_square_ == 0) {
          //logInform("Obstacle cell %d %d %d", x, y, z);
        }
      }
    }
  }
}

void printNeg(PropagationDistanceField& pdf, int numX, int numY, int numZ)
{
  for (int z=0; z<numZ; z++) {
    for (int x=0; x<numX; x++) {
      for (int y=0; y<numY; y++) {
        std::cout << pdf.getCell(x,y,z).negative_distance_square_ << " ";
      }
      std::cout << std::endl;
    }
    std::cout << std::endl;
  }
}

bool areDistanceFieldsDistancesEqual(const PropagationDistanceField& df1,
                                     const PropagationDistanceField& df2)
{
  if(df1.getXNumCells() != df2.getXNumCells()) return false;
  if(df1.getYNumCells() != df2.getYNumCells()) return false;
  if(df1.getZNumCells() != df2.getZNumCells()) return false;
  for (int z=0; z<df1.getZNumCells(); z++) {
    for (int x=0; x<df1.getXNumCells(); x++) {
      for (int y=0; y<df1.getYNumCells(); y++) {
        if(df1.getCell(x,y,z).distance_square_ != df2.getCell(x,y,z).distance_square_) {
          printf("Cell %d %d %d distances not equal %d %d\n",x,y,z,
                 df1.getCell(x,y,z).distance_square_,df2.getCell(x,y,z).distance_square_);
          return false;
        }
        if(df1.getCell(x,y,z).negative_distance_square_ != df2.getCell(x,y,z).negative_distance_square_) {
          printf("Cell %d %d %d neg distances not equal %d %d\n",x,y,z,
                 df1.getCell(x,y,z).negative_distance_square_,df2.getCell(x,y,z).negative_distance_square_);
          return false;
        }
      }
    }
  }
  return true;
}

bool checkOctomapVersusDistanceField(const PropagationDistanceField& df,
                                     const octomap::OcTree& octree)
{
  //just one way for now
  for (int z=0; z<df.getZNumCells(); z++) {
    for (int x=0; x<df.getXNumCells(); x++) {
      for (int y=0; y<df.getYNumCells(); y++) {
        if(df.getCell(x,y,z).distance_square_ == 0) {
          //making sure that octomap also shows it as occupied
          double qx, qy, qz;
          df.gridToWorld(x,y,z,
                         qx, qy, qz);
          octomap::point3d query(qx, qy, qz);
          octomap::OcTreeNode* result = octree.search(query);
          if(!result) {
            for(float boundary_x = query.x()-df.getResolution();
                boundary_x <= query.x()+df.getResolution() && !result;
                boundary_x += df.getResolution()) {
              for(float boundary_y = query.y()-df.getResolution();
                  boundary_y <= query.y()+df.getResolution() && !result;
                  boundary_y += df.getResolution()) {
                for(float boundary_z = query.z()-df.getResolution();
                    boundary_z <= query.z()+df.getResolution();
                    boundary_z += df.getResolution()) {
                  octomap::point3d query_boundary(boundary_x, boundary_y, boundary_z);
                  result = octree.search(query_boundary);
                  if(result) {
                    break;
                  }
                }
              }
            }
            if(!result) {
              std::cout << "No point at potential boundary query " << query.x() << " " << query.y() << " " << query.z() << std::endl;
              return false;
            }
          }
          if(!octree.isNodeOccupied(result)) {
            std::cout << "Disagreement at " << qx << " " << qy << " " << qz << std::endl;
            return false;
          }
        }
      }
    }
  }
  return true;
}

unsigned int countOccupiedCells(const PropagationDistanceField& df)
{
  unsigned int count = 0;
  for (int z=0; z<df.getZNumCells(); z++) {
    for (int x=0; x<df.getXNumCells(); x++) {
      for (int y=0; y<df.getYNumCells(); y++) {
        if(df.getCell(x,y,z).distance_square_ == 0) {
          count++;
        }
      }
    }
  }
  return count;
}

unsigned int countLeafNodes(const octomap::OcTree& octree)
{
  unsigned int count = 0;
  for(octomap::OcTree::leaf_iterator it = octree.begin_leafs(),
        end=octree.end_leafs(); it!= end; ++it)
  {
    if (octree.isNodeOccupied(*it))
    {
      std::cout << "Leaf node " << it.getX() << " " << it.getY() << " " << it.getZ() << std::endl;
      count++;
    }
  }
  return count;
}

//points should contain all occupied points
void check_distance_field(const PropagationDistanceField & df,
                          const EigenSTL::vector_Vector3d& points,
                          int numX, int numY, int numZ,
                          bool do_negs)
{
  std::vector<Eigen::Vector3i> points_ind(points.size());
  for(unsigned int i = 0; i < points.size(); i++) {
    Eigen::Vector3i loc;
    bool valid = df.worldToGrid(points[i].x(), points[i].y(), points[i].z(),
                                loc.x(), loc.y(), loc.z() );
    points_ind[i] = loc;
  }

  for (int x=0; x<numX; x++) {
    for (int y=0; y<numY; y++) {
      for (int z=0; z<numZ; z++) {
        double dsq = df.getCell(x, y, z).distance_square_;
        double ndsq = df.getCell(x, y, z).negative_distance_square_;
        if(dsq == 0) {
          bool found = false;
          for(unsigned int i = 0; i < points_ind.size(); i++) {
            if(points_ind[i].x() == x && points_ind[i].y() == y && points_ind[i].z() == z) {
              found = true;
              break;
            }
          }
          if(do_negs) {
            ASSERT_GT(ndsq, 0) << "Obstacle point " << x << " " << y << " " << z << " has zero negative value";
          }
          ASSERT_TRUE(found) << "Obstacle point " << x << " " << y << " " << z << " not found";
        }
      }
    }
  }
}


TEST(TestPropagationDistanceField, TestAddRemovePoints)
{
  PropagationDistanceField df( width, height, depth, resolution, origin_x, origin_y, origin_z, max_dist);

  // Check size
  int numX = df.getXNumCells();
  int numY = df.getYNumCells();
  int numZ = df.getZNumCells();

  EXPECT_EQ( numX, (int)(width/resolution+0.5) );
  EXPECT_EQ( numY, (int)(height/resolution+0.5) );
  EXPECT_EQ( numZ, (int)(depth/resolution+0.5) );

  //getting a bad point
  double tgx, tgy, tgz;
  bool in_bounds;
  EXPECT_NEAR(df.getDistance(1000.0,1000.0,1000.0), max_dist, .0001);
  EXPECT_NEAR(df.getDistanceGradient(1000.0,1000.0,1000.0, tgx, tgy, tgz, in_bounds), max_dist, .0001);
  EXPECT_FALSE(in_bounds);

  // Add points to the grid
  EigenSTL::vector_Vector3d points;
  points.push_back(point1);
  points.push_back(point2);
  logInform("Adding %u points", points.size());
  df.addPointsToField(points);
  //print(df, numX, numY, numZ);

  // Update - iterative
  points.clear();
  points.push_back(point2);
  points.push_back(point3);
  EigenSTL::vector_Vector3d old_points;
  old_points.push_back(point1);
  df.updatePointsInField(old_points, points);
  //std::cout << "One removal, one addition" << std::endl;
  //print(df, numX, numY, numZ);
  //printNeg(df, numX, numY, numZ);
  check_distance_field(df, points, numX, numY, numZ, false);

  // Remove
  points.clear();
  points.push_back(point2);
  df.removePointsFromField(points);
  points.clear();
  points.push_back(point3);
  check_distance_field( df, points, numX, numY, numZ, false);

  //now testing gradient calls
  df.reset();
  points.clear();
  points.push_back(point1);
  df.addPointsToField(points);
  bool first = true;
  for (int z=1; z<df.getZNumCells()-1; z++) {
    for (int x=1; x<df.getXNumCells()-1; x++) {
      for (int y=1; y<df.getYNumCells()-1; y++) {
        double dist = df.getDistance(x,y,z);
        double wx, wy, wz;
        df.gridToWorld(x,y,z,wx,wy,wz);
        Eigen::Vector3d grad(0.0,0.0,0.0);
        bool grad_in_bounds;
        double dist_grad = df.getDistanceGradient(wx, wy, wz,
                                                  grad.x(), grad.y(), grad.z(), grad_in_bounds);
        ASSERT_TRUE(grad_in_bounds) << x << " " << y << " " << z;
        ASSERT_NEAR(dist, dist_grad, .0001);
        if(dist > 0 &&
           dist < max_dist) {
          double xscale = grad.x()/grad.norm();
          double yscale = grad.y()/grad.norm();
          double zscale = grad.z()/grad.norm();

          double comp_x = wx-xscale*dist;
          double comp_y = wy-yscale*dist;
          double comp_z = wz-zscale*dist;
          if(first) {
            first = false;
            std::cout << "Dist " << dist << std::endl;
            std::cout << "Cell " << x << " " << y << " " << z << " " << wx << " " << wy << " " << wz << std::endl;
            std::cout << "Scale " << xscale << " " << yscale << " " << zscale << std::endl;
            std::cout << "Grad " << grad.x() << " " << grad.y() << " " << grad.z() << " comp " << comp_x << " " << comp_y << " " << comp_z << std::endl;
          }
          ASSERT_NEAR(comp_x, point1.x(), resolution) << dist << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() << " " << xscale << " " << yscale << " " << zscale << std::endl;
          ASSERT_NEAR(comp_y, point1.y(), resolution) << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() << " " << xscale << " " << yscale << " " << zscale << std::endl;
          ASSERT_NEAR(comp_z, point1.z(), resolution) << x << " " << y << " " << z << " " << grad.x() << " " << grad.y() << " " << grad.z() << " " << xscale << " " << yscale << " " << zscale << std::endl;
        }
      }
    }
  }
  ASSERT_FALSE(first);
}

TEST(TestSignedPropagationDistanceField, TestSignedAddRemovePoints)
{

  PropagationDistanceField df( width, height, depth, resolution, origin_x, origin_y, origin_z, max_dist, true);

  // Check size
  int numX = df.getXNumCells();
  int numY = df.getYNumCells();
  int numZ = df.getZNumCells();

  EXPECT_EQ( numX, (int)(width/resolution+0.5) );
  EXPECT_EQ( numY, (int)(height/resolution+0.5) );
  EXPECT_EQ( numZ, (int)(depth/resolution+0.5) );

  // Error checking
  //print(df, numX, numY, numZ);

  // TODO - check initial values
  //EXPECT_EQ( df.getCell(0,0,0).distance_square_, max_dist_sq_in_voxels );

  // Add points to the grid
  double lwx, lwy, lwz;
  double hwx, hwy, hwz;
  df.gridToWorld(1,1,1,lwx,lwy,lwz);
  df.gridToWorld(8,8,8,hwx,hwy,hwz);

  EigenSTL::vector_Vector3d points;
  for(double x = lwx; x <= hwx; x+= .1) {
    for(double y = lwy; y <= hwy; y+= .1) {
      for(double z = lwz; z <= hwz; z+= .1) {
        points.push_back(Eigen::Vector3d(x,y,z));
      }
    }
  }

  df.reset();
  logInform("Adding %u points", points.size());
  df.addPointsToField(points);
  //print(df, numX, numY, numZ);
  //printNeg(df, numX, numY, numZ);

  double cx, cy, cz;
  df.gridToWorld(5,5,5,cx,cy,cz);

  Eigen::Vector3d center_point(cx,cy,cz);

  EigenSTL::vector_Vector3d rem_points;
  rem_points.push_back(center_point);
  df.removePointsFromField(rem_points);
  //std::cout << "Pos "<< std::endl;
  //print(df, numX, numY, numZ);
  //std::cout << "Neg "<< std::endl;
  //printNeg(df, numX, numY, numZ);

  //testing equality with initial add of points without the center point
  PropagationDistanceField test_df( width, height, depth, resolution, origin_x, origin_y, origin_z, max_dist, true);
  EigenSTL::vector_Vector3d test_points;
  for(unsigned int i = 0; i < points.size(); i++) {
    if(points[i].x() != center_point.x() ||
       points[i].y() != center_point.y() ||
       points[i].z() != center_point.z())
    {
      test_points.push_back(points[i]);
    }
  }
  test_df.addPointsToField(test_points);
  ASSERT_TRUE(areDistanceFieldsDistancesEqual(df, test_df));

  PropagationDistanceField gradient_df( width, height, depth, resolution, origin_x, origin_y, origin_z, max_dist, true);

  shapes::Sphere sphere(.25);

  geometry_msgs::Pose p;
  p.orientation.w = 1.0;
  p.position.x = .5;
  p.position.y = .5;
  p.position.z = .5;

  gradient_df.addShapeToField(&sphere, p);
  printNeg(gradient_df, numX, numY, numZ);
  EXPECT_GT(gradient_df.getCell(5,5,5).negative_distance_square_, 1);
  //all negative cells should have gradients that point towards cells with distance 1
  for (int z=1; z<df.getZNumCells()-1; z++) {
    for (int x=1; x<df.getXNumCells()-1; x++) {
      for (int y=1; y<df.getYNumCells()-1; y++) {
        if(gradient_df.getCell(x,y,z).negative_distance_square_ > 0) {
          double dist = gradient_df.getDistance(x,y,z);
          ASSERT_LT(dist, 0) << "Pos " << gradient_df.getCell(x,y,z).distance_square_ << " " << gradient_df.getCell(x,y,z).negative_distance_square_;
          double wx, wy, wz;
          df.gridToWorld(x,y,z,wx,wy,wz);
          Eigen::Vector3d grad(0.0,0.0,0.0);
          bool grad_in_bounds;
          double dist_grad = gradient_df.getDistanceGradient(wx, wy, wz,
                                                             grad.x(), grad.y(), grad.z(), grad_in_bounds);
          ASSERT_TRUE(grad_in_bounds) << x << " " << y << " " << z;
          ASSERT_NEAR(dist, dist_grad, .0001);
          double xscale = grad.x()/grad.norm();
          double yscale = grad.y()/grad.norm();
          double zscale = grad.z()/grad.norm();

          double comp_x = wx-xscale*dist;
          double comp_y = wy-yscale*dist;
          double comp_z = wz-zscale*dist;

          int cell_x, cell_y, cell_z;
          gradient_df.worldToGrid(comp_x, comp_y, comp_z,
                                  cell_x, cell_y, cell_z);
          ASSERT_GE(gradient_df.getCell(cell_x, cell_y, cell_z).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;
        }
      }
    }
  }
}


TEST(TestSignedPropagationDistanceField, TestShape)
{

  PropagationDistanceField df( width, height, depth, resolution, origin_x, origin_y, origin_z, max_dist, true);

  int numX = df.getXNumCells();
  int numY = df.getYNumCells();
  int numZ = df.getZNumCells();

  shapes::Sphere sphere(.25);

  geometry_msgs::Pose p;
  p.orientation.w = 1.0;
  p.position.x = .5;
  p.position.y = .5;
  p.position.z = .5;

  geometry_msgs::Pose np;
  np.orientation.w = 1.0;
  np.position.x = .7;
  np.position.y = .7;
  np.position.z = .7;

  df.addShapeToField(&sphere, p);

  bodies::Body* body = bodies::createBodyFromShape(&sphere);
  Eigen::Affine3d pose_e;
  tf::poseMsgToEigen(p, pose_e);
  body->setPose(pose_e);
  EigenSTL::vector_Vector3d point_vec = distance_field::determineCollisionPoints(body, resolution);
  delete body;
  check_distance_field(df, point_vec,
                       numX, numY, numZ, true);

  // std::cout << "Shape pos "<< std::endl;
  // print(df, numX, numY, numZ);
  // std::cout << "Shape neg "<< std::endl;
  // printNeg(df, numX, numY, numZ);

  df.addShapeToField(&sphere, np);

  body = bodies::createBodyFromShape(&sphere);
  Eigen::Affine3d npose_e;
  tf::poseMsgToEigen(np, npose_e);
  body->setPose(npose_e);

  EigenSTL::vector_Vector3d point_vec_2 = distance_field::determineCollisionPoints(body, resolution);
  delete body;
  EigenSTL::vector_Vector3d point_vec_union = point_vec_2;
  point_vec_union.insert(point_vec_union.end(),
                         point_vec.begin(),
                         point_vec.end());
  check_distance_field(df, point_vec_union,
                       numX, numY, numZ, true);

  //should get rid of old pose
  df.moveShapeInField(&sphere, p, np);

  check_distance_field(df, point_vec_2,
                       numX, numY, numZ, true);

  //should be equivalent to just adding second shape
  PropagationDistanceField test_df( width, height, depth, resolution, origin_x, origin_y, origin_z, max_dist, true);
  test_df.addShapeToField(&sphere, np);
  ASSERT_TRUE(areDistanceFieldsDistancesEqual(df, test_df));
}

static const double PERF_WIDTH = 3.0;
static const double PERF_HEIGHT = 3.0;
static const double PERF_DEPTH = 4.0;
static const double PERF_RESOLUTION = 0.02;
static const double PERF_ORIGIN_X = 0.0;
static const double PERF_ORIGIN_Y = 0.0;
static const double PERF_ORIGIN_Z = 0.0;
static const double PERF_MAX_DIST = .25;
static const unsigned int UNIFORM_DISTANCE = 10;

TEST(TestSignedPropagationDistanceField, TestPerformance)
{
  std::cout << "Creating distance field with " <<
    (PERF_WIDTH/PERF_RESOLUTION)*(PERF_HEIGHT/PERF_RESOLUTION)*(PERF_DEPTH/PERF_RESOLUTION)
            << " entries" << std::endl;

  ros::WallTime dt = ros::WallTime::now();
  PropagationDistanceField df( PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
                               PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, false);
  std::cout << "Creating unsigned took " << (ros::WallTime::now()-dt).toSec() << std::endl;

  dt = ros::WallTime::now();
  PropagationDistanceField sdf( PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
                                PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, true);

  std::cout << "Creating signed took " << (ros::WallTime::now()-dt).toSec() << std::endl;

  shapes::Box big_table(2.0,2.0,.5);

  geometry_msgs::Pose p;
  p.orientation.w = 1.0;
  p.position.x = PERF_WIDTH/2.0;
  p.position.y = PERF_DEPTH/2.0;
  p.position.z = PERF_HEIGHT/2.0;

  geometry_msgs::Pose np;
  np.orientation.w = 1.0;
  np.position.x = p.position.x+.01;
  np.position.y = p.position.y;
  np.position.z = p.position.z;

  unsigned int big_num_points = ceil(2.0/PERF_RESOLUTION)*ceil(2.0/PERF_RESOLUTION)*ceil(.5/PERF_RESOLUTION);

  std::cout << "Adding " << big_num_points << " points" << std::endl;

  dt = ros::WallTime::now();
  df.addShapeToField(&big_table, p);
  std::cout << "Adding to unsigned took "
            << (ros::WallTime::now()-dt).toSec()
            << " avg " << (ros::WallTime::now()-dt).toSec()/(big_num_points*1.0)
            << std::endl;

  dt = ros::WallTime::now();
  df.addShapeToField(&big_table, p);
  std::cout << "Re-adding to unsigned took " << (ros::WallTime::now()-dt).toSec() << std::endl;

  dt = ros::WallTime::now();
  sdf.addShapeToField(&big_table, p);
  std::cout << "Adding to signed took "
            << (ros::WallTime::now()-dt).toSec()
            << " avg " << (ros::WallTime::now()-dt).toSec()/(big_num_points*1.0)
            << std::endl;

  dt = ros::WallTime::now();
  df.moveShapeInField(&big_table, p, np);
  std::cout << "Moving in unsigned took " << (ros::WallTime::now()-dt).toSec() << std::endl;

  dt = ros::WallTime::now();
  sdf.moveShapeInField(&big_table, p, np);
  std::cout << "Moving in signed took " << (ros::WallTime::now()-dt).toSec() << std::endl;

  dt = ros::WallTime::now();
  df.removeShapeFromField(&big_table, np);
  std::cout << "Removing from unsigned took " << (ros::WallTime::now()-dt).toSec() << std::endl;

  dt = ros::WallTime::now();
  sdf.removeShapeFromField(&big_table, np);
  std::cout << "Removing from signed took " << (ros::WallTime::now()-dt).toSec() << std::endl;

  dt = ros::WallTime::now();
  df.reset();

  shapes::Box small_table(.25,.25,.05);

  unsigned int small_num_points = (13)*(13)*(3);

  std::cout << "Adding " << small_num_points << " points" << std::endl;

  dt = ros::WallTime::now();
  df.addShapeToField(&small_table, p);
  std::cout << "Adding to unsigned took "
            << (ros::WallTime::now()-dt).toSec()
            << " avg " << (ros::WallTime::now()-dt).toSec()/(small_num_points*1.0)
            << std::endl;

  dt = ros::WallTime::now();
  sdf.addShapeToField(&small_table, p);
  std::cout << "Adding to signed took "
            << (ros::WallTime::now()-dt).toSec()
            << " avg " << (ros::WallTime::now()-dt).toSec()/(small_num_points*1.0)
            << std::endl;

  dt = ros::WallTime::now();
  df.moveShapeInField(&small_table, p, np);
  std::cout << "Moving in unsigned took " << (ros::WallTime::now()-dt).toSec() << std::endl;

  dt = ros::WallTime::now();
  sdf.moveShapeInField(&small_table, p, np);
  std::cout << "Moving in signed took " << (ros::WallTime::now()-dt).toSec() << std::endl;

  //uniformly spaced points - a worst case scenario
  PropagationDistanceField worstdfu(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
                                   PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, false);

  PropagationDistanceField worstdfs(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
                                   PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, true);



  EigenSTL::vector_Vector3d bad_vec;
  unsigned int count = 0;
  for(unsigned int z = UNIFORM_DISTANCE; z < worstdfu.getZNumCells()-UNIFORM_DISTANCE; z += UNIFORM_DISTANCE) {
    for(unsigned int x = UNIFORM_DISTANCE; x < worstdfu.getXNumCells()-UNIFORM_DISTANCE; x += UNIFORM_DISTANCE) {
      for(unsigned int y = UNIFORM_DISTANCE; y < worstdfu.getYNumCells()-UNIFORM_DISTANCE; y += UNIFORM_DISTANCE) {
        count++;
        Eigen::Vector3d loc;
        bool valid = worstdfu.gridToWorld(x,y,z,
                                          loc.x(), loc.y(), loc.z());

        if(!valid) {
          logWarn("Something wrong");
          continue;
        }
        bad_vec.push_back(loc);
      }
    }
  }

  dt = ros::WallTime::now();
  worstdfu.addPointsToField(bad_vec);
  ros::WallDuration wd = ros::WallTime::now()-dt;
  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));
  dt = ros::WallTime::now();
  worstdfs.addPointsToField(bad_vec);
  wd = ros::WallTime::now()-dt;
  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));

}

TEST(TestSignedPropagationDistanceField, TestOcTree)
{
  PropagationDistanceField df(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
                              PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, false);

  octomap::OcTree tree(.02);

  unsigned int count = 0;
  for(float x = 1.01; x < 1.5; x += .02) {
    for(float y = 1.01; y < 1.5; y += .02) {
      for(float z = 1.01; z < 1.5; z += .02, count++) {
        octomap::point3d point(x,y,z);
        tree.updateNode(point, true);
      }
    }
  }

  //more points at the border of the distance field
  for(float x = 2.51; x < 3.5; x += .02) {
    for(float y = 1.01; y < 3.5; y += .02) {
      for(float z = 1.01; z < 3.5; z += .02, count++) {
        octomap::point3d point(x,y,z);
        tree.updateNode(point, true);
      }
    }
  }

  std::cout << "OcTree nodes " << count << std::endl;
  df.addOcTreeToField(&tree);

  EXPECT_TRUE(checkOctomapVersusDistanceField(df, tree));

  //more cells
  for(float x = .01; x < .50; x += .02) {
    for(float y = .01; y < .50; y += .02) {
      for(float z = .01; z < .50; z += .02, count++) {
        octomap::point3d point(x,y,z);
        tree.updateNode(point, true);
      }
    }
  }
  df.addOcTreeToField(&tree);
  EXPECT_TRUE(checkOctomapVersusDistanceField(df, tree));

  PropagationDistanceField df_oct(tree,
                                 octomap::point3d(0.5,0.5,0.5),
                                 octomap::point3d(5.0,5.0,5.0),
                                 PERF_MAX_DIST,
                                 false);

  EXPECT_TRUE(checkOctomapVersusDistanceField(df_oct, tree));

  //now try different resolutions
  octomap::OcTree tree_lowres(.05);
  octomap::point3d point1(.5,.5,.5);
  octomap::point3d point2(.7,.5,.5);
  octomap::point3d point3(1.0,.5,.5);
  tree_lowres.updateNode(point1, true);
  tree_lowres.updateNode(point2, true);
  tree_lowres.updateNode(point3, true);
  ASSERT_EQ(countLeafNodes(tree_lowres), 3);

  PropagationDistanceField df_highres(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
                                      PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, false);

  df_highres.addOcTreeToField(&tree_lowres);
  EXPECT_EQ(countOccupiedCells(df_highres), 3*(6*6*6));
  std::cout << "Occupied cells " << countOccupiedCells(df_highres) << std::endl;

  //testing adding shape that happens to be octree
  boost::shared_ptr<octomap::OcTree> tree_shape(new octomap::OcTree(.05));
  octomap::point3d tpoint1(1.0,.5,1.0);
  octomap::point3d tpoint2(1.7,.5,.5);
  octomap::point3d tpoint3(1.8,.5,.5);
  tree_shape->updateNode(tpoint1, true);
  tree_shape->updateNode(tpoint2, true);
  tree_shape->updateNode(tpoint3, true);

  boost::shared_ptr<shapes::OcTree> shape_oc(new shapes::OcTree(tree_shape));

  PropagationDistanceField df_test_shape_1(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
                                           PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, false);

  PropagationDistanceField df_test_shape_2(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
                                           PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, false);

  df_test_shape_1.addOcTreeToField(tree_shape.get());
  df_test_shape_2.addShapeToField(shape_oc.get(), geometry_msgs::Pose());
  EXPECT_TRUE(areDistanceFieldsDistancesEqual(df_test_shape_1, df_test_shape_2));
}

TEST(TestSignedPropagationDistanceField, TestReadWrite)
{

  PropagationDistanceField small_df( width, height, depth, resolution, origin_x, origin_y, origin_z, max_dist);

  EigenSTL::vector_Vector3d points;
  points.push_back(point1);
  points.push_back(point2);
  points.push_back(point3);
  small_df.addPointsToField(points);

  std::ofstream sf("test_small.df", std::ios::out);

  small_df.writeToStream(sf);
  //must close to make sure that the buffer is written
  sf.close();

  std::ifstream si("test_small.df", std::ios::in | std::ios::binary);
  PropagationDistanceField small_df2(si, PERF_MAX_DIST, false);
  ASSERT_TRUE(areDistanceFieldsDistancesEqual(small_df, small_df2));

  PropagationDistanceField df(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION,
                              PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, false);

  shapes::Sphere sphere(.5);

  geometry_msgs::Pose p;
  p.orientation.w = 1.0;
  p.position.x = .5;
  p.position.y = .5;
  p.position.z = .5;

  df.addShapeToField(&sphere, p);

  std::ofstream f("test_big.df", std::ios::out);

  df.writeToStream(f);
  f.close();

  std::ifstream i("test_big.df", std::ios::in);
  PropagationDistanceField df2(i, PERF_MAX_DIST, false);
  EXPECT_TRUE(areDistanceFieldsDistancesEqual(df, df2));

  //we shouldn't segfault if we start with an old ifstream
  PropagationDistanceField dfx(i, PERF_MAX_DIST, false);

  std::ifstream i2("test_big.df", std::ios::in);
  ros::WallTime wt = ros::WallTime::now();
  PropagationDistanceField df3(i2, PERF_MAX_DIST+.02, false);
  std::cout << "Reconstruction for big file took " << (ros::WallTime::now()-wt).toSec() << std::endl;
  EXPECT_FALSE(areDistanceFieldsDistancesEqual(df, df3));
}

int main(int argc, char **argv){
  testing::InitGoogleTest(&argc, argv);

  return RUN_ALL_TESTS();
}