propagation_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_PROPAGATION_DISTANCE_FIELD_H_
00038 #define DF_PROPAGATION_DISTANCE_FIELD_H_
00039 
00040 #include <distance_field/voxel_grid.h>
00041 #include <distance_field/distance_field.h>
00042 #include <tf/LinearMath/Vector3.h>
00043 #include <vector>
00044 #include <list>
00045 #include <ros/ros.h>
00046 #include <eigen3/Eigen/Core>
00047 #include <set>
00048 
00049 namespace distance_field
00050 {
00051 
00052 // TODO: Move to voxel_grid.h
00054 typedef Eigen::Vector3i int3;
00055 
00056 // less-than Comparison
00057 struct compareInt3
00058 {
00059   bool operator()(int3 loc_1, int3 loc_2) const
00060   {
00061     if( loc_1.z() != loc_2.z() )
00062       return ( loc_1.z() < loc_2.z() );
00063     else if( loc_1.y() != loc_2.y() )
00064       return ( loc_1.y() < loc_2.y() );
00065     else if( loc_1.x() != loc_2.x() )
00066       return ( loc_1.x() < loc_2.x() );
00067     return false;
00068   }
00069 };
00070 
00071 
00075 struct PropDistanceFieldVoxel
00076 {
00077   PropDistanceFieldVoxel();
00078   PropDistanceFieldVoxel(int distance_sq);
00079 
00080   int distance_square_;         
00081   int3 location_;               
00082   int3 closest_point_;          
00083   int update_direction_;        
00085   static const int UNINITIALIZED=-1;
00086 };
00087 
00088 struct SignedPropDistanceFieldVoxel : public PropDistanceFieldVoxel
00089 {
00090     SignedPropDistanceFieldVoxel();
00091     SignedPropDistanceFieldVoxel(int distance_sq_positive, int distance_sq_negative);
00092     int positive_distance_square_;
00093     int negative_distance_square_;
00094     int3 closest_positive_point_;
00095     int3 closest_negative_point_;
00096 
00097     static const int UNINITIALIZED=-999;
00098 };
00099 
00108 class PropagationDistanceField: public DistanceField<PropDistanceFieldVoxel>
00109 {
00110 public:
00111 
00112 
00116   PropagationDistanceField(double size_x, double size_y, double size_z, double resolution,
00117       double origin_x, double origin_y, double origin_z, double max_distance);
00118 
00119   virtual ~PropagationDistanceField();
00120 
00126   virtual void updatePointsInField(const std::vector<tf::Vector3>& points, const bool iterative=true);
00127 
00131   virtual void addPointsToField(const std::vector<tf::Vector3>& points);
00132 
00136   virtual void reset();
00137 
00142   void getOccupiedVoxelMarkers(const std::string & frame_id, 
00143                                const ros::Time stamp,
00144                                const tf::Transform& cur,
00145                                visualization_msgs::Marker& marker);
00146 
00147 private:
00149   typedef std::set<int3, compareInt3> VoxelSet;
00150   VoxelSet object_voxel_locations_;
00151 
00153   std::vector<std::vector<PropDistanceFieldVoxel*> > bucket_queue_;
00154   double max_distance_;
00155   int max_distance_sq_;
00156 
00157   std::vector<double> sqrt_table_;
00158 
00159   // neighborhoods:
00160   // [0] - for expansion of d=0
00161   // [1] - for expansion of d>=1
00162   // Under this, we have the 27 directions
00163   // Then, a list of neighborhoods for each direction
00164   std::vector<std::vector<std::vector<int3 > > > neighborhoods_;
00165 
00166   std::vector<int3 > direction_number_to_direction_;
00167 
00168   void addNewObstacleVoxels(const VoxelSet& points);
00169   void removeObstacleVoxels(const VoxelSet& points);
00170   // starting with the voxels on the queue, propogate values to neighbors up to a certain distance.
00171   void propogate();
00172   virtual double getDistance(const PropDistanceFieldVoxel& object) const;
00173   int getDirectionNumber(int dx, int dy, int dz) const;
00174   int3 getLocationDifference(int directionNumber) const;        // TODO- separate out neighborhoods
00175   void initNeighborhoods();
00176   static int eucDistSq(int3 point1, int3 point2);
00177 
00178 };
00179 
00181 
00182 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel(int distance_sq):
00183   distance_square_(distance_sq)
00184 {
00185   closest_point_.x() = PropDistanceFieldVoxel::UNINITIALIZED;
00186   closest_point_.y() = PropDistanceFieldVoxel::UNINITIALIZED;
00187   closest_point_.z() = PropDistanceFieldVoxel::UNINITIALIZED;
00188 }
00189 
00190 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel()
00191 {
00192 }
00193 
00194 inline double PropagationDistanceField::getDistance(const PropDistanceFieldVoxel& object) const
00195 {
00196   return sqrt_table_[object.distance_square_];
00197 }
00198 
00199 
00200 class SignedPropagationDistanceField : public DistanceField<SignedPropDistanceFieldVoxel>
00201 {
00202   public:
00203 
00204     SignedPropagationDistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x,
00205                                    double origin_y, double origin_z, double max_distance);
00206     virtual ~SignedPropagationDistanceField();
00207 
00208     virtual void addPointsToField(const std::vector<tf::Vector3> &points);
00209 
00210     virtual void reset();
00211 
00212   private:
00213     std::vector<std::vector<SignedPropDistanceFieldVoxel*> > positive_bucket_queue_;
00214     std::vector<std::vector<SignedPropDistanceFieldVoxel*> > negative_bucket_queue_;
00215     double max_distance_;
00216     int max_distance_sq_;
00217 
00218     std::vector<double> sqrt_table_;
00219 
00220     // [0] - for expansion of d=0
00221      // [1] - for expansion of d>=1
00222      // Under this, we have the 27 directions
00223      // Then, a list of neighborhoods for each direction
00224      std::vector<std::vector<std::vector<int3 > > > neighborhoods_;
00225 
00226      std::vector<int3 > direction_number_to_direction_;
00227 
00228      virtual double getDistance(const SignedPropDistanceFieldVoxel& object) const;
00229      int getDirectionNumber(int dx, int dy, int dz) const;
00230      void initNeighborhoods();
00231      static int eucDistSq(int3 point1, int3 point2);
00232 };
00233 
00234 
00235 
00236 inline SignedPropDistanceFieldVoxel::SignedPropDistanceFieldVoxel(int distance_sq_positive, int distance_sq_negative):
00237   positive_distance_square_(distance_sq_positive),
00238   negative_distance_square_(distance_sq_negative),
00239   closest_positive_point_(SignedPropDistanceFieldVoxel::UNINITIALIZED),
00240   closest_negative_point_(SignedPropDistanceFieldVoxel::UNINITIALIZED)
00241 {
00242 }
00243 
00244 inline SignedPropDistanceFieldVoxel::SignedPropDistanceFieldVoxel()
00245 {
00246 }
00247 
00248 inline double SignedPropagationDistanceField::getDistance(const SignedPropDistanceFieldVoxel& object) const
00249 {
00250   if(object.negative_distance_square_ != 0)
00251   {
00252     //ROS_INFO("Positive Distance %d, Negative Distance %d",object.positive_distance_square_,object.negative_distance_square_ );
00253   }
00254   return sqrt_table_[object.positive_distance_square_] - sqrt_table_[object.negative_distance_square_];
00255 }
00256 
00257 }
00258 
00259 #endif /* DF_PROPAGATION_DISTANCE_FIELD_H_ */


distance_field
Author(s): Mrinal Kalakrishnan / mail@mrinal.net
autogenerated on Thu Dec 12 2013 11:08:36