distance_field: distance_field: propagation_distance

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 <LinearMath/btVector3.h>
00043 #include <vector>
00044 #include <list>
00045 #include <ros/ros.h>
00046 
00047 namespace distance_field
00048 {
00049 
00053 struct PropDistanceFieldVoxel
00054 {
00055   PropDistanceFieldVoxel();
00056   PropDistanceFieldVoxel(int distance_sq);
00057 
00058   int distance_square_;         
00059   int location_[3];             
00060   int closest_point_[3];        
00061   int update_direction_;        
00063   static const int UNINITIALIZED=-1;
00064 };
00065 
00066 struct SignedPropDistanceFieldVoxel : public PropDistanceFieldVoxel
00067 {
00068     SignedPropDistanceFieldVoxel();
00069     SignedPropDistanceFieldVoxel(int distance_sq_positive, int distance_sq_negative);
00070     int positive_distance_square_;
00071     int negative_distance_square_;
00072     int closest_positive_point_[3];
00073     int closest_negative_point_[3];
00074 
00075     static const int UNINITIALIZED=-999;
00076 };
00077 
00086 class PropagationDistanceField: public DistanceField<PropDistanceFieldVoxel>
00087 {
00088 public:
00089 
00090 
00094   PropagationDistanceField(double size_x, double size_y, double size_z, double resolution,
00095       double origin_x, double origin_y, double origin_z, double max_distance);
00096 
00097   virtual ~PropagationDistanceField();
00098 
00102   virtual void addPointsToField(const std::vector<btVector3> points);
00103 
00107   virtual void reset();
00108 
00109 private:
00110   std::vector<std::vector<PropDistanceFieldVoxel*> > bucket_queue_;
00111   double max_distance_;
00112   int max_distance_sq_;
00113 
00114   std::vector<double> sqrt_table_;
00115 
00116   // neighborhoods:
00117   // [0] - for expansion of d=0
00118   // [1] - for expansion of d>=1
00119   // Under this, we have the 27 directions
00120   // Then, a list of neighborhoods for each direction
00121   std::vector<std::vector<std::vector<std::vector<int> > > > neighborhoods_;
00122 
00123   std::vector<std::vector<int> > direction_number_to_direction_;
00124 
00125   virtual double getDistance(const PropDistanceFieldVoxel& object) const;
00126   int getDirectionNumber(int dx, int dy, int dz) const;
00127   void initNeighborhoods();
00128   static int eucDistSq(int* point1, int* point2);
00129 };
00130 
00132 
00133 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel(int distance_sq):
00134   distance_square_(distance_sq)
00135 {
00136     for (int i=0; i<3; i++)
00137       closest_point_[i] = PropDistanceFieldVoxel::UNINITIALIZED;
00138 }
00139 
00140 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel()
00141 {
00142 }
00143 
00144 inline double PropagationDistanceField::getDistance(const PropDistanceFieldVoxel& object) const
00145 {
00146   return sqrt_table_[object.distance_square_];
00147 }
00148 
00149 
00150 class SignedPropagationDistanceField : public DistanceField<SignedPropDistanceFieldVoxel>
00151 {
00152   public:
00153 
00154     SignedPropagationDistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x,
00155                                    double origin_y, double origin_z, double max_distance);
00156     virtual ~SignedPropagationDistanceField();
00157 
00158     virtual void addPointsToField(const std::vector<btVector3> points);
00159 
00160     virtual void reset();
00161 
00162   private:
00163     std::vector<std::vector<SignedPropDistanceFieldVoxel*> > positive_bucket_queue_;
00164     std::vector<std::vector<SignedPropDistanceFieldVoxel*> > negative_bucket_queue_;
00165     double max_distance_;
00166     int max_distance_sq_;
00167 
00168     std::vector<double> sqrt_table_;
00169 
00170     // [0] - for expansion of d=0
00171      // [1] - for expansion of d>=1
00172      // Under this, we have the 27 directions
00173      // Then, a list of neighborhoods for each direction
00174      std::vector<std::vector<std::vector<std::vector<int> > > > neighborhoods_;
00175 
00176      std::vector<std::vector<int> > direction_number_to_direction_;
00177 
00178      virtual double getDistance(const SignedPropDistanceFieldVoxel& object) const;
00179      int getDirectionNumber(int dx, int dy, int dz) const;
00180      void initNeighborhoods();
00181      static int eucDistSq(int* point1, int* point2);
00182 };
00183 
00184 
00185 
00186 inline SignedPropDistanceFieldVoxel::SignedPropDistanceFieldVoxel(int distance_sq_positive, int distance_sq_negative):
00187   positive_distance_square_(distance_sq_positive),
00188   negative_distance_square_(distance_sq_negative)
00189 {
00190     for (int i=0; i<3; i++)
00191     {
00192       closest_positive_point_[i] = SignedPropDistanceFieldVoxel::UNINITIALIZED;
00193       closest_negative_point_[i] = SignedPropDistanceFieldVoxel::UNINITIALIZED;
00194     }
00195 }
00196 
00197 inline SignedPropDistanceFieldVoxel::SignedPropDistanceFieldVoxel()
00198 {
00199 }
00200 
00201 inline double SignedPropagationDistanceField::getDistance(const SignedPropDistanceFieldVoxel& object) const
00202 {
00203   if(object.negative_distance_square_ != 0)
00204   {
00205     //ROS_INFO("Positive Distance %d, Negative Distance %d",object.positive_distance_square_,object.negative_distance_square_ );
00206   }
00207   return sqrt_table_[object.positive_distance_square_] - sqrt_table_[object.negative_distance_square_];
00208 }
00209 
00210 }
00211 
00212 #endif /* DF_PROPAGATION_DISTANCE_FIELD_H_ */
propagation_distance_field.h
