distance_field.h
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 the 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 
00037 #ifndef DF_DISTANCE_FIELD_H_
00038 #define DF_DISTANCE_FIELD_H_
00039 
00040 #include <distance_field/voxel_grid.h>
00041 #include <tf/LinearMath/Vector3.h>
00042 #include <tf/LinearMath/Quaternion.h>
00043 #include <tf/LinearMath/Transform.h>
00044 #include <vector>
00045 #include <list>
00046 #include <ros/ros.h>
00047 #include <visualization_msgs/Marker.h>
00048 
00049 #include <arm_navigation_msgs/CollisionMap.h>
00050 
00051 namespace distance_field
00052 {
00053 
00055 enum PlaneVisualizationType
00056 {
00057  XYPlane,
00058  XZPlane,
00059  YZPlane
00060 };
00061 
00073 template <typename T>
00074 class DistanceField: public VoxelGrid<T>
00075 {
00076 public:
00077 
00090   DistanceField(double size_x, double size_y, double size_z, double resolution,
00091       double origin_x, double origin_y, double origin_z, T default_object);
00092 
00093   virtual ~DistanceField();
00094 
00095 
00103   virtual void addPointsToField(const std::vector<tf::Vector3> &points)=0;
00104 
00108   void addCollisionMapToField(const arm_navigation_msgs::CollisionMap &collision_map);
00109 
00113   virtual void reset()=0;
00114 
00118   double getDistance(double x, double y, double z) const;
00119 
00123   double getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z) const;
00124 
00128   double getDistanceFromCell(int x, int y, int z) const;
00129 
00137   void getIsoSurfaceMarkers(double min_radius, double max_radius,
00138                             const std::string & frame_id, const ros::Time stamp,
00139                             const tf::Transform& cur,
00140                             visualization_msgs::Marker& marker );
00141 
00142 
00149   void getGradientMarkers(double min_radius, double max_radius,
00150                           const std::string & frame_id, const ros::Time stamp,
00151                           std::vector<visualization_msgs::Marker>& markers );
00152 
00162   void getPlaneMarkers(PlaneVisualizationType type, double length, double width, double height, tf::Vector3 origin,
00163                        const std::string & frame_id, const ros::Time stamp,
00164                        visualization_msgs::Marker& marker );
00165 
00166 protected:
00167   virtual double getDistance(const T& object) const=0;
00168 
00169 private:
00170   int inv_twice_resolution_;
00171 };
00172 
00174 
00175 template <typename T>
00176 DistanceField<T>::~DistanceField()
00177 {
00178 
00179 }
00180 
00181 template <typename T>
00182 DistanceField<T>::DistanceField(double size_x, double size_y, double size_z, double resolution,
00183     double origin_x, double origin_y, double origin_z, T default_object):
00184       VoxelGrid<T>(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, default_object)
00185 {
00186   inv_twice_resolution_ = 1.0/(2.0*resolution);
00187 }
00188 
00189 template <typename T>
00190 double DistanceField<T>::getDistance(double x, double y, double z) const
00191 {
00192   return getDistance((*this)(x,y,z));
00193 }
00194 
00195 template <typename T>
00196 double DistanceField<T>::getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z) const
00197 {
00198   int gx, gy, gz;
00199 
00200   this->worldToGrid(x, y, z, gx, gy, gz);
00201 
00202   // if out of bounds, return 0 distance, and 0 gradient
00203   // we need extra padding of 1 to get gradients
00204   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)
00205   {
00206     gradient_x = 0.0;
00207     gradient_y = 0.0;
00208     gradient_z = 0.0;
00209     return 0;
00210   }
00211 
00212   gradient_x = (getDistanceFromCell(gx+1,gy,gz) - getDistanceFromCell(gx-1,gy,gz))*inv_twice_resolution_;
00213   gradient_y = (getDistanceFromCell(gx,gy+1,gz) - getDistanceFromCell(gx,gy-1,gz))*inv_twice_resolution_;
00214   gradient_z = (getDistanceFromCell(gx,gy,gz+1) - getDistanceFromCell(gx,gy,gz-1))*inv_twice_resolution_;
00215 
00216   return getDistanceFromCell(gx,gy,gz);
00217 
00218 }
00219 
00220 template <typename T>
00221 double DistanceField<T>::getDistanceFromCell(int x, int y, int z) const
00222 {
00223   return getDistance(this->getCell(x,y,z));
00224 }
00225 
00226 template <typename T>
00227 void DistanceField<T>::getIsoSurfaceMarkers(double min_radius, double max_radius,
00228                                             const std::string & frame_id, const ros::Time stamp,
00229                                             const tf::Transform& cur,
00230                                             visualization_msgs::Marker& inf_marker )
00231 {
00232   inf_marker.points.clear();
00233   inf_marker.header.frame_id = frame_id;
00234   inf_marker.header.stamp = stamp;
00235   inf_marker.ns = "distance_field";
00236   inf_marker.id = 1;
00237   inf_marker.type = visualization_msgs::Marker::CUBE_LIST;
00238   inf_marker.action = 0;
00239   inf_marker.scale.x = this->resolution_[VoxelGrid<T>::DIM_X];
00240   inf_marker.scale.y = this->resolution_[VoxelGrid<T>::DIM_Y];
00241   inf_marker.scale.z = this->resolution_[VoxelGrid<T>::DIM_Z];
00242   inf_marker.color.r = 1.0;
00243   inf_marker.color.g = 0.0;
00244   inf_marker.color.b = 0.0;
00245   inf_marker.color.a = 0.1;
00246   //inf_marker.lifetime = ros::Duration(30.0);
00247 
00248   inf_marker.points.reserve(100000);
00249   int num_total_cells =
00250     this->num_cells_[VoxelGrid<T>::DIM_X]*
00251     this->num_cells_[VoxelGrid<T>::DIM_Y]*
00252     this->num_cells_[VoxelGrid<T>::DIM_Z];
00253   for (int x = 0; x < this->num_cells_[VoxelGrid<T>::DIM_X]; ++x)
00254   {
00255     for (int y = 0; y < this->num_cells_[VoxelGrid<T>::DIM_Y]; ++y)
00256     {
00257       for (int z = 0; z < this->num_cells_[VoxelGrid<T>::DIM_Z]; ++z)
00258       {
00259         double dist = getDistanceFromCell(x,y,z);
00260         if (dist >= min_radius && dist <= max_radius)
00261         {
00262           int last = inf_marker.points.size();
00263           inf_marker.points.resize(last + 1);
00264           double nx, ny, nz;
00265           this->gridToWorld(x,y,z,
00266                             nx, ny, nz);
00267           tf::Vector3 vec(nx,ny,nz);
00268           vec = cur*vec;
00269           inf_marker.points[last].x = vec.x();
00270           inf_marker.points[last].y = vec.y();
00271           inf_marker.points[last].z = vec.z();
00272         }
00273       }
00274     }
00275   }
00276 }
00277 
00278 template <typename T>
00279 void DistanceField<T>::getGradientMarkers( double min_radius, double max_radius,
00280                                            const std::string & frame_id, const ros::Time stamp,
00281                                            std::vector<visualization_msgs::Marker>& markers )
00282 {
00283   tf::Vector3 unitX(1, 0, 0);
00284   tf::Vector3 unitY(0, 1, 0);
00285   tf::Vector3 unitZ(0, 0, 1);
00286 
00287   int id = 0;
00288 
00289   for (int x = 0; x < this->num_cells_[this->DIM_X]; ++x)
00290   {
00291     for (int y = 0; y < this->num_cells_[this->DIM_Y]; ++y)
00292     {
00293       for (int z = 0; z < this->num_cells_[this->DIM_Z]; ++z)
00294       {
00295         double worldX, worldY, worldZ;
00296         this->gridToWorld(x, y, z, worldX, worldY, worldZ);
00297 
00298         double gradientX, gradientY, gradientZ;
00299         double distance = getDistanceGradient(worldX, worldY, worldZ, gradientX, gradientY, gradientZ);
00300         tf::Vector3 gradient(gradientX, gradientY, gradientZ);
00301 
00302         if (distance >= min_radius && distance <= max_radius && gradient.length() > 0)
00303         {
00304           visualization_msgs::Marker marker;
00305 
00306           marker.header.frame_id = frame_id;
00307           marker.header.stamp = stamp;
00308 
00309           marker.ns = "distance_field_gradient";
00310           marker.id = id++;
00311           marker.type = visualization_msgs::Marker::ARROW;
00312           marker.action = visualization_msgs::Marker::ADD;
00313 
00314           marker.pose.position.x = worldX;
00315           marker.pose.position.y = worldY;
00316           marker.pose.position.z = worldZ;
00317 
00318           tf::Vector3 axis = gradient.cross(unitX).length() > 0 ? gradient.cross(unitX) : unitY;
00319           tfScalar angle = -gradient.angle(unitX);
00320           tf::Quaternion rotation = tf::Quaternion(axis, angle);
00321 
00322           marker.pose.orientation.x = rotation.x();
00323           marker.pose.orientation.y = rotation.y();
00324           marker.pose.orientation.z = rotation.z();
00325           marker.pose.orientation.w = rotation.w();
00326 
00327           marker.scale.x = this->resolution_[this->DIM_X];
00328           marker.scale.y = this->resolution_[this->DIM_Y];
00329           marker.scale.z = this->resolution_[this->DIM_Z];
00330 
00331           marker.color.r = 0.0;
00332           marker.color.g = 0.0;
00333           marker.color.b = 1.0;
00334           marker.color.a = 1.0;
00335 
00336           markers.push_back(marker);
00337         }
00338       }
00339     }
00340   }
00341 }
00342 
00343 template <typename T>
00344 void DistanceField<T>::addCollisionMapToField(const arm_navigation_msgs::CollisionMap &collision_map)
00345 {
00346   size_t num_boxes = collision_map.boxes.size();
00347   std::vector<tf::Vector3> points;
00348   points.reserve(num_boxes);
00349   for (size_t i=0; i<num_boxes; ++i)
00350   {
00351     points.push_back(tf::Vector3(
00352         collision_map.boxes[i].center.x,
00353         collision_map.boxes[i].center.y,
00354         collision_map.boxes[i].center.z
00355         ));
00356   }
00357   addPointsToField(points);
00358 }
00359 
00360 template <typename T>
00361 void DistanceField<T>::getPlaneMarkers(distance_field::PlaneVisualizationType type, double length, double width,
00362                                       double height, tf::Vector3 origin,
00363                                       const std::string & frame_id, const ros::Time stamp,
00364                                       visualization_msgs::Marker& plane_marker )
00365 {
00366   plane_marker.header.frame_id = frame_id;
00367   plane_marker.header.stamp = stamp;
00368   plane_marker.ns = "distance_field_plane";
00369   plane_marker.id = 1;
00370   plane_marker.type = visualization_msgs::Marker::CUBE_LIST;
00371   plane_marker.action = visualization_msgs::Marker::ADD;
00372   plane_marker.scale.x = this->resolution_[VoxelGrid<T>::DIM_X];
00373   plane_marker.scale.y = this->resolution_[VoxelGrid<T>::DIM_Y];
00374   plane_marker.scale.z = this->resolution_[VoxelGrid<T>::DIM_Z];
00375   //plane_marker.lifetime = ros::Duration(30.0);
00376 
00377   plane_marker.points.reserve(100000);
00378   plane_marker.colors.reserve(100000);
00379 
00380   double minX = 0;
00381   double maxX = 0;
00382   double minY = 0;
00383   double maxY = 0;
00384   double minZ = 0;
00385   double maxZ = 0;
00386 
00387   switch(type)
00388   {
00389     case XYPlane:
00390       minZ = height;
00391       maxZ = height;
00392 
00393       minX = -length/2.0;
00394       maxX = length/2.0;
00395       minY = -width/2.0;
00396       maxY = width/2.0;
00397       break;
00398     case XZPlane:
00399       minY = height;
00400       maxY = height;
00401 
00402       minX = -length/2.0;
00403       maxX = length/2.0;
00404       minZ = -width/2.0;
00405       maxZ = width/2.0;
00406       break;
00407     case YZPlane:
00408       minX = height;
00409       maxX = height;
00410 
00411       minY = -length/2.0;
00412       maxY = length/2.0;
00413       minZ = -width/2.0;
00414       maxZ = width/2.0;
00415       break;
00416   }
00417 
00418   minX += origin.getX();
00419   minY += origin.getY();
00420   minZ += origin.getZ();
00421 
00422   maxX += origin.getX();
00423   maxY += origin.getY();
00424   maxZ += origin.getZ();
00425 
00426   int minXCell = 0;
00427   int maxXCell = 0;
00428   int minYCell = 0;
00429   int maxYCell = 0;
00430   int minZCell = 0;
00431   int maxZCell = 0;
00432 
00433   this->worldToGrid(minX,minY,minZ, minXCell, minYCell, minZCell);
00434   this->worldToGrid(maxX,maxY,maxZ, maxXCell, maxYCell, maxZCell);
00435   plane_marker.color.a = 1.0;
00436   for(int x = minXCell; x <= maxXCell; ++x)
00437   {
00438     for(int y = minYCell; y <= maxYCell; ++y)
00439     {
00440       for(int z = minZCell; z <= maxZCell; ++z)
00441       {
00442         if(!this->isCellValid(x,y,z))
00443         {
00444           continue;
00445         }
00446         double dist = getDistanceFromCell(x, y, z);
00447         int last = plane_marker.points.size();
00448         plane_marker.points.resize(last + 1);
00449         plane_marker.colors.resize(last + 1);
00450         double nx, ny, nz;
00451         this->gridToWorld(x, y, z, nx, ny, nz);
00452         tf::Vector3 vec(nx, ny, nz);
00453         plane_marker.points[last].x = vec.x();
00454         plane_marker.points[last].y = vec.y();
00455         plane_marker.points[last].z = vec.z();
00456         if(dist < 0.0)
00457         {
00458           plane_marker.colors[last].r = fmax(fmin(0.1/fabs(dist), 1.0), 0.0);
00459           plane_marker.colors[last].g = fmax(fmin(0.05/fabs(dist), 1.0), 0.0);
00460           plane_marker.colors[last].b = fmax(fmin(0.01/fabs(dist), 1.0), 0.0);
00461 
00462         }
00463         else
00464         {
00465           plane_marker.colors[last].b = fmax(fmin(0.1/(dist+0.001), 1.0),0.0);
00466           plane_marker.colors[last].g = fmax(fmin(0.05/(dist+0.001), 1.0),0.0);
00467           plane_marker.colors[last].r = fmax(fmin(0.01/(dist+0.001), 1.0),0.0);
00468         }
00469       }
00470     }
00471   }
00472 }
00473 
00474 }
00475 #endif /* DF_DISTANCE_FIELD_H_ */


distance_field
Author(s): Mrinal Kalakrishnan / mail@mrinal.net
autogenerated on Fri Dec 6 2013 21:11:00