distance_field.h

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.
 *********************************************************************/

#ifndef DF_DISTANCE_FIELD_H_
#define DF_DISTANCE_FIELD_H_

#include <distance_field/voxel_grid.h>
#include <LinearMath/btVector3.h>
#include <LinearMath/btQuaternion.h>
#include <LinearMath/btTransform.h>
#include <vector>
#include <list>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <mapping_msgs/CollisionMap.h>

namespace distance_field
{

template <typename T>
class DistanceField: public VoxelGrid<T>
{
public:
  DistanceField(double size_x, double size_y, double size_z, double resolution,
      double origin_x, double origin_y, double origin_z, T default_object);

  virtual ~DistanceField();



  virtual void addPointsToField(const std::vector<btVector3> points)=0;

  void addCollisionMapToField(const mapping_msgs::CollisionMap &collision_map);

  virtual void reset()=0;

  double getDistance(double x, double y, double z) const;

  double getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z) const;

  double getDistanceFromCell(int x, int y, int z) const;

  void visualize(double min_radius, double max_radius, std::string frame_id, const btTransform& cur, ros::Time stamp);

  void visualizeGradient(double min_radius, double max_radius, std::string frame_id, ros::Time stamp);

protected:
  virtual double getDistance(const T& object) const=0;

private:
  ros::Publisher pub_viz_;
  int inv_twice_resolution_;
};


template <typename T>
DistanceField<T>::~DistanceField()
{

}

template <typename T>
DistanceField<T>::DistanceField(double size_x, double size_y, double size_z, double resolution,
    double origin_x, double origin_y, double origin_z, T default_object):
      VoxelGrid<T>(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, default_object)
{
  ros::NodeHandle node;
  pub_viz_ = node.advertise<visualization_msgs::Marker>("visualization_marker", 3);
  inv_twice_resolution_ = 1.0/(2.0*resolution);
}

template <typename T>
double DistanceField<T>::getDistance(double x, double y, double z) const
{
  return getDistance((*this)(x,y,z));
}

template <typename T>
double DistanceField<T>::getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z) const
{
  int gx, gy, gz;

  this->worldToGrid(x, y, z, gx, gy, gz);

  // if out of bounds, return 0 distance, and 0 gradient
  // we need extra padding of 1 to get gradients
  if (gx<1 || gy<1 || gz<1 || gx>=this->num_cells_[this->DIM_X]-1 || gy>=this->num_cells_[this->DIM_Y]-1 || gz>=this->num_cells_[this->DIM_Z]-1)
  {
    gradient_x = 0.0;
    gradient_y = 0.0;
    gradient_z = 0.0;
    return 0;
  }

  gradient_x = (getDistanceFromCell(gx+1,gy,gz) - getDistanceFromCell(gx-1,gy,gz))*inv_twice_resolution_;
  gradient_y = (getDistanceFromCell(gx,gy+1,gz) - getDistanceFromCell(gx,gy-1,gz))*inv_twice_resolution_;
  gradient_z = (getDistanceFromCell(gx,gy,gz+1) - getDistanceFromCell(gx,gy,gz-1))*inv_twice_resolution_;

  return getDistanceFromCell(gx,gy,gz);

}

template <typename T>
double DistanceField<T>::getDistanceFromCell(int x, int y, int z) const
{
  return getDistance(this->getCell(x,y,z));
}

template <typename T>
void DistanceField<T>::visualize(double min_radius, double max_radius, std::string frame_id, 
                                 const btTransform& cur, ros::Time stamp)
{
  visualization_msgs::Marker inf_marker; // Marker for the inflation
  inf_marker.header.frame_id = frame_id;
  inf_marker.header.stamp = stamp;
  inf_marker.ns = "distance_field";
  inf_marker.id = 1;
  inf_marker.type = visualization_msgs::Marker::CUBE_LIST;
  inf_marker.action = 0;
  inf_marker.scale.x = this->resolution_[VoxelGrid<T>::DIM_X];
  inf_marker.scale.y = this->resolution_[VoxelGrid<T>::DIM_Y];
  inf_marker.scale.z = this->resolution_[VoxelGrid<T>::DIM_Z];
  inf_marker.color.r = 1.0;
  inf_marker.color.g = 0.0;
  inf_marker.color.b = 0.0;
  inf_marker.color.a = 0.1;
  //inf_marker.lifetime = ros::Duration(30.0);

  inf_marker.points.reserve(100000);
  int num_total_cells =
    this->num_cells_[VoxelGrid<T>::DIM_X]*
    this->num_cells_[VoxelGrid<T>::DIM_Y]*
    this->num_cells_[VoxelGrid<T>::DIM_Z];
  for (int x = 0; x < this->num_cells_[VoxelGrid<T>::DIM_X]; ++x)
  {
    for (int y = 0; y < this->num_cells_[VoxelGrid<T>::DIM_Y]; ++y)
    {
      for (int z = 0; z < this->num_cells_[VoxelGrid<T>::DIM_Z]; ++z)
      {
        double dist = getDistanceFromCell(x,y,z);
        if (dist >= min_radius && dist <= max_radius)
        {
          int last = inf_marker.points.size();
          inf_marker.points.resize(last + 1);
          double nx, ny, nz;
          this->gridToWorld(x,y,z,
                            nx, ny, nz);
          btVector3 vec(nx,ny,nz);
          vec = cur*vec;
          inf_marker.points[last].x = vec.x();
          inf_marker.points[last].y = vec.y();
          inf_marker.points[last].z = vec.z();
        }
      }
    }
  }
  ROS_DEBUG("Publishing markers: %u/%d inflated", (unsigned int) inf_marker.points.size(), num_total_cells);
  pub_viz_.publish(inf_marker);
}

template <typename T>
void DistanceField<T>::visualizeGradient(double min_radius, double max_radius, std::string frame_id, ros::Time stamp)
{
  btVector3 unitX(1, 0, 0);
  btVector3 unitY(0, 1, 0);
  btVector3 unitZ(0, 0, 1);

  int id = 0;

  for (int x = 0; x < this->num_cells_[this->DIM_X]; ++x)
  {
    for (int y = 0; y < this->num_cells_[this->DIM_Y]; ++y)
    {
      for (int z = 0; z < this->num_cells_[this->DIM_Z]; ++z)
      {
        double worldX, worldY, worldZ;
        this->gridToWorld(x, y, z, worldX, worldY, worldZ);

        double gradientX, gradientY, gradientZ;
        double distance = getDistanceGradient(worldX, worldY, worldZ, gradientX, gradientY, gradientZ);
        btVector3 gradient(gradientX, gradientY, gradientZ);

        if (distance >= min_radius && distance <= max_radius && gradient.length() > 0)
        {
          visualization_msgs::Marker marker;

          marker.header.frame_id = frame_id;
          marker.header.stamp = stamp;

          marker.ns = "distance_field_gradient";
          marker.id = id++;
          marker.type = visualization_msgs::Marker::ARROW;
          marker.action = visualization_msgs::Marker::ADD;

          marker.pose.position.x = worldX;
          marker.pose.position.y = worldY;
          marker.pose.position.z = worldZ;

          btVector3 axis = gradient.cross(unitX).length() > 0 ? gradient.cross(unitX) : unitY;
          btScalar angle = -gradient.angle(unitX);
          btQuaternion rotation = btQuaternion(axis, angle);

          marker.pose.orientation.x = rotation.x();
          marker.pose.orientation.y = rotation.y();
          marker.pose.orientation.z = rotation.z();
          marker.pose.orientation.w = rotation.w();

          marker.scale.x = this->resolution_[this->DIM_X];
          marker.scale.y = this->resolution_[this->DIM_Y];
          marker.scale.z = this->resolution_[this->DIM_Z];

          marker.color.r = 0.0;
          marker.color.g = 0.0;
          marker.color.b = 1.0;
          marker.color.a = 1.0;

          //marker.lifetime = ros::Duration(30.0);

          pub_viz_.publish(marker);
        }
      }
    }
  }
}

template <typename T>
void DistanceField<T>::addCollisionMapToField(const mapping_msgs::CollisionMap &collision_map)
{
  size_t num_boxes = collision_map.boxes.size();
  std::vector<btVector3> points;
  points.reserve(num_boxes);
  for (size_t i=0; i<num_boxes; ++i)
  {
    points.push_back(btVector3(
        collision_map.boxes[i].center.x,
        collision_map.boxes[i].center.y,
        collision_map.boxes[i].center.z
        ));
  }
  addPointsToField(points);
}


}
#endif /* DF_DISTANCE_FIELD_H_ */

---

distance_field
Author(s): Mrinal Kalakrishnan / mail@mrinal.net
autogenerated on Fri Jan 11 09:54:12 2013