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 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.
 *********************************************************************/
 
/* Author: Mrinal Kalakrishnan, Ken Anderson */
 
#include <gtest/gtest.h>
 
#include <moveit/distance_field/voxel_grid.h>
#include <moveit/distance_field/propagation_distance_field.h>
#include <moveit/distance_field/find_internal_points.h>
#include <geometric_shapes/body_operations.h>
#include <tf2_eigen/tf2_eigen.hpp>
#include <octomap/octomap.h>
#include <memory>
 
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 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 distanceSequence(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 y = 0; y < numY; ++y)
    {
      for (int x = 0; x < numX; ++x)
      {
        std::cout << pdf.getCell(x, y, z).distance_square_ << ' ';
      }
      std::cout << '\n';
    }
    std::cout << '\n';
  }
  for (int z = 0; z < numZ; ++z)
  {
    for (int y = 0; y < numY; ++y)
    {
      for (int x = 0; x < numX; ++x)
      {
        if (pdf.getCell(x, y, z).distance_square_ == 0)
        {
          // RCLCPP_INFO("distance_field", "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 y = 0; y < numY; ++y)
    {
      for (int x = 0; x < numX; ++x)
      {
        std::cout << pdf.getCell(x, y, z).negative_distance_square_ << ' ';
      }
      std::cout << '\n';
    }
    std::cout << '\n';
  }
}
 
void printPointCoords(const Eigen::Vector3i& p)
{
  if (p.x() < 0)
  {
    std::cout << '-';
  }
  else
  {
    std::cout << p.x();
  }
 
  if (p.y() < 0)
  {
    std::cout << '-';
  }
  else
  {
    std::cout << p.y();
  }
 
  if (p.z() < 0)
  {
    std::cout << '-';
  }
  else
  {
    std::cout << p.z();
  }
 
  std::cout << ' ';
}
 
void printBoth(PropagationDistanceField& pdf, int numX, int numY, int numZ)
{
  std::cout << "Positive distance square ... negative distance square\n";
  for (int z = 0; z < numZ; ++z)
  {
    std::cout << "Z=" << z << '\n';
    for (int y = 0; y < numY; ++y)
    {
      for (int x = 0; x < numX; ++x)
      {
        std::cout << pdf.getCell(x, y, z).distance_square_ << ' ';
      }
      std::cout << "   ";
      for (int x = 0; x < numX; ++x)
      {
        std::cout << pdf.getCell(x, y, z).negative_distance_square_ << ' ';
      }
      std::cout << "     ";
      for (int x = 0; x < numX; ++x)
      {
        printPointCoords(pdf.getCell(x, y, z).closest_point_);
      }
      std::cout << "   ";
      for (int x = 0; x < numX; ++x)
      {
        printPointCoords(pdf.getCell(x, y, z).closest_negative_point_);
      }
      std::cout << '\n';
    }
    std::cout << '\n';
  }
}
 
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()
                        << '\n';
              return false;
            }
          }
          if (!octree.isNodeOccupied(result))
          {
            std::cout << "Disagreement at " << qx << ' ' << qy << ' ' << qz << '\n';
            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() << '\n';
      count++;
    }
  }
  return count;
}
 
// points should contain all occupied points
void checkDistanceField(const PropagationDistanceField& df, const EigenSTL::vector_Vector3d& points, int numX, int numY,
                        int numZ, bool do_negs)
{
  EigenSTL::vector_Vector3i points_ind(points.size());
  for (unsigned int i = 0; i < points.size(); ++i)
  {
    Eigen::Vector3i loc;
    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 (Eigen::Vector3i& point : points_ind)
          {
            if (point.x() == x && point.y() == y && point.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 num_x = df.getXNumCells();
  int num_y = df.getYNumCells();
  int num_z = df.getZNumCells();
 
  EXPECT_EQ(num_x, static_cast<int>(WIDTH / RESOLUTION + 0.5));
  EXPECT_EQ(num_y, static_cast<int>(HEIGHT / RESOLUTION + 0.5));
  EXPECT_EQ(num_z, static_cast<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);
  // RCLCPP_INFO("distance_field", "Adding %zu 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" << '\n';
  // print(df, numX, numY, numZ);
  // printNeg(df, numX, numY, numZ);
  checkDistanceField(df, points, num_x, num_y, num_z, false);
 
  // Remove
  points.clear();
  points.push_back(POINT2);
  df.removePointsFromField(points);
  points.clear();
  points.push_back(POINT3);
  checkDistanceField(df, points, num_x, num_y, num_z, 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 << '\n';
            std::cout << "Cell " << x << ' ' << y << ' ' << z << ' ' << wx << ' ' << wy << ' ' << wz << '\n';
            std::cout << "Scale " << xscale << ' ' << yscale << ' ' << zscale << '\n';
            std::cout << "Grad " << grad.x() << ' ' << grad.y() << ' ' << grad.z() << " comp " << comp_x << ' '
                      << comp_y << ' ' << comp_z << '\n';
          }
          ASSERT_NEAR(comp_x, POINT1.x(), RESOLUTION)
              << dist << x << ' ' << y << ' ' << z << ' ' << grad.x() << ' ' << grad.y() << ' ' << grad.z() << ' '
              << xscale << ' ' << yscale << ' ' << zscale << '\n';
          ASSERT_NEAR(comp_y, POINT1.y(), RESOLUTION)
              << x << ' ' << y << ' ' << z << ' ' << grad.x() << ' ' << grad.y() << ' ' << grad.z() << ' ' << xscale
              << ' ' << yscale << ' ' << zscale << '\n';
          ASSERT_NEAR(comp_z, POINT1.z(), RESOLUTION)
              << x << ' ' << y << ' ' << z << ' ' << grad.x() << ' ' << grad.y() << ' ' << grad.z() << ' ' << xscale
              << ' ' << yscale << ' ' << zscale << '\n';
        }
      }
    }
  }
  ASSERT_FALSE(first);
}
 
TEST(TestSignedPropagationDistanceField, TestSignedAddRemovePoints)
{
  PropagationDistanceField df(WIDTH, HEIGHT, DEPTH, RESOLUTION, ORIGIN_X, ORIGIN_Y, ORIGIN_Z, MAX_DIST, true);
 
  // Check size
  int num_x = df.getXNumCells();
  int num_y = df.getYNumCells();
  int num_z = df.getZNumCells();
 
  EXPECT_EQ(num_x, static_cast<int>(WIDTH / RESOLUTION + 0.5));
  EXPECT_EQ(num_y, static_cast<int>(HEIGHT / RESOLUTION + 0.5));
  EXPECT_EQ(num_z, static_cast<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_distanceSequence_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();
  // RCLCPP_INFO("distance_field", "Adding %zu 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 "<< '\n';
  // print(df, numX, numY, numZ);
  // std::cout << "Neg "<< '\n';
  // 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 (const Eigen::Vector3d& point : points)
  {
    if (point.x() != center_point.x() || point.y() != center_point.y() || point.z() != center_point.z())
    {
      test_points.push_back(point);
    }
  }
  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::msg::Pose p;
  p.orientation.w = 1.0;
  p.position.x = .5;
  p.position.y = .5;
  p.position.z = .5;
 
  Eigen::Isometry3d p_eigen;
  tf2::fromMsg(p, p_eigen);
 
  gradient_df.addShapeToField(&sphere, p_eigen);
  // printBoth(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)
      {
        double dist = gradient_df.getDistance(x, y, z);
        double ncell_dist;
        Eigen::Vector3i ncell_pos;
        const PropDistanceFieldVoxel* ncell = gradient_df.getNearestCell(x, y, z, ncell_dist, ncell_pos);
 
        EXPECT_EQ(ncell_dist, dist);
 
        if (ncell == nullptr)
        {
          if (ncell_dist > 0)
          {
            EXPECT_GE(ncell_dist, gradient_df.getUninitializedDistance())
                << "dist=" << dist << " xyz=" << x << ' ' << y << ' ' << z << " ncell=" << ncell_pos.x() << ' '
                << ncell_pos.y() << ' ' << ncell_pos.z() << '\n';
          }
          else if (ncell_dist < 0)
          {
            EXPECT_LE(ncell_dist, -gradient_df.getUninitializedDistance())
                << "dist=" << dist << " xyz=" << x << ' ' << y << ' ' << z << " ncell=" << ncell_pos.x() << ' '
                << ncell_pos.y() << ' ' << ncell_pos.z() << '\n';
          }
        }
 
        if (gradient_df.getCell(x, y, z).negative_distance_square_ > 0)
        {
          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);
 
          if (!ncell)
            continue;
 
          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() << '\n';
 
          double grad_size_sq = grad.squaredNorm();
          if (grad_size_sq < std::numeric_limits<double>::epsilon())
            continue;
 
          double oo_grad_size = 1.0 / sqrt(grad_size_sq);
          double xscale = grad.x() * oo_grad_size;
          double yscale = grad.y() * oo_grad_size;
          double zscale = grad.z() * oo_grad_size;
 
          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;
          bool cell_in_bounds = gradient_df.worldToGrid(comp_x, comp_y, comp_z, cell_x, cell_y, cell_z);
 
          ASSERT_EQ(cell_in_bounds, true);
          const PropDistanceFieldVoxel* cell = &gradient_df.getCell(cell_x, cell_y, cell_z);
 
#if 0
          EXPECT_EQ(ncell_pos.x(), cell_x);
          EXPECT_EQ(ncell_pos.y(), cell_y);
          EXPECT_EQ(ncell_pos.z(), cell_z);
          EXPECT_EQ(ncell, cell);
#endif
          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
              << '\n';
        }
      }
    }
  }
}
 
TEST(TestSignedPropagationDistanceField, TestShape)
{
  PropagationDistanceField df(WIDTH, HEIGHT, DEPTH, RESOLUTION, ORIGIN_X, ORIGIN_Y, ORIGIN_Z, MAX_DIST, true);
 
  int num_x = df.getXNumCells();
  int num_y = df.getYNumCells();
  int num_z = df.getZNumCells();
 
  shapes::Sphere sphere(.25);
 
  Eigen::Isometry3d p = Eigen::Translation3d(0.5, 0.5, 0.5) * Eigen::Quaterniond(0.0, 0.0, 0.0, 1.0);
  Eigen::Isometry3d np = Eigen::Translation3d(0.7, 0.7, 0.7) * Eigen::Quaterniond(0.0, 0.0, 0.0, 1.0);
 
  df.addShapeToField(&sphere, p);
 
  bodies::Body* body = bodies::createBodyFromShape(&sphere);
  body->setPose(p);
  EigenSTL::vector_Vector3d point_vec;
  findInternalPointsConvex(*body, RESOLUTION, point_vec);
  delete body;
  checkDistanceField(df, point_vec, num_x, num_y, num_z, true);
 
  // std::cout << "Shape pos "<< '\n';
  // print(df, numX, numY, numZ);
  // std::cout << "Shape neg "<< '\n';
  // printNeg(df, numX, numY, numZ);
 
  df.addShapeToField(&sphere, np);
 
  body = bodies::createBodyFromShape(&sphere);
  body->setPose(np);
 
  EigenSTL::vector_Vector3d point_vec_2;
  findInternalPointsConvex(*body, RESOLUTION, point_vec_2);
  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());
  checkDistanceField(df, point_vec_union, num_x, num_y, num_z, true);
 
  // should get rid of old pose
  df.moveShapeInField(&sphere, p, np);
 
  checkDistanceField(df, point_vec_2, num_x, num_y, num_z, 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" << '\n';
 
  auto dt = std::chrono::system_clock::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 "
      << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - dt).count() / 1000.0
      << " secs" << '\n';
 
  dt = std::chrono::system_clock::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 "
      << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - dt).count() / 1000.0
      << " secs" << '\n';
 
  shapes::Box big_table(2.0, 2.0, .5);
 
  Eigen::Isometry3d p = Eigen::Translation3d(PERF_WIDTH / 2.0, PERF_DEPTH / 2.0, PERF_HEIGHT / 2.0) *
                        Eigen::Quaterniond(0.0, 0.0, 0.0, 1.0);
  Eigen::Isometry3d np = Eigen::Translation3d(PERF_WIDTH / 2.0 + .01, PERF_DEPTH / 2.0, PERF_HEIGHT / 2.0) *
                         Eigen::Quaterniond(0.0, 0.0, 0.0, 1.0);
 
  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" << '\n';
 
  dt = std::chrono::system_clock::now();
  df.addShapeToField(&big_table, p);
  std::cout
      << "Adding to unsigned took "
      << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - dt).count() / 1000.0
      << " secs"
      << " avg "
      << (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - dt).count() /
          1000.0) /
             (big_num_points * 1.0)
      << '\n';
 
  dt = std::chrono::system_clock::now();
  df.addShapeToField(&big_table, p);
  std::cout
      << "Re-adding to unsigned took "
      << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - dt).count() / 1000.0
      << " secs" << '\n';
 
  dt = std::chrono::system_clock::now();
  sdf.addShapeToField(&big_table, p);
  std::cout
      << "Adding to signed took "
      << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - dt).count() / 1000.0
      << " secs"
      << " avg "
      << (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - dt).count() /
          1000.0) /
             (big_num_points * 1.0)
      << '\n';
 
  dt = std::chrono::system_clock::now();
  df.moveShapeInField(&big_table, p, np);
  std::cout
      << "Moving in unsigned took "
      << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - dt).count() / 1000.0
      << " secs" << '\n';
 
  dt = std::chrono::system_clock::now();
  sdf.moveShapeInField(&big_table, p, np);
  std::cout
      << "Moving in signed took "
      << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - dt).count() / 1000.0
      << " secs" << '\n';
 
  dt = std::chrono::system_clock::now();
  df.removeShapeFromField(&big_table, np);
  std::cout
      << "Removing from unsigned took "
      << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - dt).count() / 1000.0
      << " secs" << '\n';
 
  dt = std::chrono::system_clock::now();
  sdf.removeShapeFromField(&big_table, np);
  std::cout
      << "Removing from signed took "
      << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - dt).count() / 1000.0
      << " secs" << '\n';
 
  dt = std::chrono::system_clock::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" << '\n';
 
  dt = std::chrono::system_clock::now();
  df.addShapeToField(&small_table, p);
  std::cout
      << "Adding to unsigned took "
      << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - dt).count() / 1000.0
      << " secs"
      << " avg " << (std::chrono::system_clock::now() - dt).count() / (small_num_points * 1.0) << '\n';
 
  dt = std::chrono::system_clock::now();
  sdf.addShapeToField(&small_table, p);
  std::cout
      << "Adding to signed took "
      << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - dt).count() / 1000.0
      << " secs"
      << " avg " << (std::chrono::system_clock::now() - dt).count() / (small_num_points * 1.0) << '\n';
 
  dt = std::chrono::system_clock::now();
  df.moveShapeInField(&small_table, p, np);
  std::cout
      << "Moving in unsigned took "
      << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - dt).count() / 1000.0
      << " secs" << '\n';
 
  dt = std::chrono::system_clock::now();
  sdf.moveShapeInField(&small_table, p, np);
  std::cout
      << "Moving in signed took "
      << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - dt).count() / 1000.0
      << " secs" << '\n';
 
  // 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)
        {
          // RCLCPP_WARN("distance_field", "Something wrong");
          continue;
        }
        bad_vec.push_back(loc);
      }
    }
  }
 
  dt = std::chrono::system_clock::now();
  worstdfu.addPointsToField(bad_vec);
  std::chrono::duration<double> wd = std::chrono::system_clock::now() - dt;
  printf("Time for unsigned adding %u uniform points is %g average %g\n", static_cast<unsigned int>(bad_vec.size()),
         wd.count(), wd.count() / (bad_vec.size() * 1.0));
  dt = std::chrono::system_clock::now();
  worstdfs.addPointsToField(bad_vec);
  wd = std::chrono::system_clock::now() - dt;
  printf("Time for signed adding %u uniform points is %g average %g\n", static_cast<unsigned int>(bad_vec.size()),
         wd.count(), wd.count() / (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 << '\n';
  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), 3u);
 
  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), 3u * (4u * 4u * 4u));
  std::cout << "Occupied cells " << countOccupiedCells(df_highres) << '\n';
 
  // testing adding shape that happens to be octree
  auto tree_shape = std::make_shared<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);
 
  auto shape_oc = std::make_shared<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(), Eigen::Isometry3d());
  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);
 
  Eigen::Isometry3d p = Eigen::Translation3d(0.5, 0.5, 0.5) * Eigen::Quaterniond(0.0, 0.0, 0.0, 1.0);
 
  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);
  auto wt = std::chrono::system_clock::now();
  PropagationDistanceField df3(i2, PERF_MAX_DIST + .02, false);
  std::cout << "Reconstruction for big file took " << (std::chrono::system_clock::now() - wt).count() << '\n';
  EXPECT_FALSE(areDistanceFieldsDistancesEqual(df, df3));
}
 
int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}