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 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 
00035 /* Author: Mrinal Kalakrishnan, Ken Anderson */
00036 
00037 #ifndef MOVEIT_DISTANCE_FIELD_PROPAGATION_DISTANCE_FIELD_
00038 #define MOVEIT_DISTANCE_FIELD_PROPAGATION_DISTANCE_FIELD_
00039 
00040 #include <moveit/distance_field/voxel_grid.h>
00041 #include <moveit/distance_field/distance_field.h>
00042 #include <vector>
00043 #include <list>
00044 #include <Eigen/Core>
00045 #include <set>
00046 #include <octomap/octomap.h>
00047 
00048 namespace distance_field
00049 {
00050 
00055 struct compareEigen_Vector3i
00056 {
00057   bool operator()(Eigen::Vector3i loc_1, Eigen::Vector3i loc_2) const
00058   {
00059     if( loc_1.z() != loc_2.z() )
00060       return ( loc_1.z() < loc_2.z() );
00061     else if( loc_1.y() != loc_2.y() )
00062       return ( loc_1.y() < loc_2.y() );
00063     else if( loc_1.x() != loc_2.x() )
00064       return ( loc_1.x() < loc_2.x() );
00065     return false;
00066   }
00067 };
00068 
00073 struct PropDistanceFieldVoxel
00074 {
00075 
00081   PropDistanceFieldVoxel();
00082 
00096   PropDistanceFieldVoxel(int distance_sq_positive, int distance_sq_negative);
00097 
00098   int distance_square_;         
00099   int negative_distance_square_; 
00100   Eigen::Vector3i closest_point_; 
00101   Eigen::Vector3i closest_negative_point_; 
00102   int update_direction_;        
00103   int negative_update_direction_;        
00105   static const int UNINITIALIZED=-1; 
00106 };
00107 
00134 class PropagationDistanceField: public DistanceField
00135 {
00136 public:
00137 
00138 
00164   PropagationDistanceField(double size_x,
00165                            double size_y,
00166                            double size_z,
00167                            double resolution,
00168                            double origin_x, double origin_y, double origin_z,
00169                            double max_distance,
00170                            bool propagate_negative_distances=false);
00171 
00194   PropagationDistanceField(const octomap::OcTree& octree,
00195                            const octomap::point3d& bbx_min,
00196                            const octomap::point3d& bbx_max,
00197                            double max_distance,
00198                            bool propagate_negative_distances=false);
00199 
00221   PropagationDistanceField(std::istream& stream,
00222                            double max_distance,
00223                            bool propagate_negative_distances=false);
00229   virtual ~PropagationDistanceField(){}
00230 
00244   virtual void addPointsToField(const EigenSTL::vector_Vector3d& points);
00245 
00265   virtual void removePointsFromField(const EigenSTL::vector_Vector3d& points);
00266 
00289   virtual void updatePointsInField(const EigenSTL::vector_Vector3d& old_points,
00290                                    const EigenSTL::vector_Vector3d& new_points);
00291 
00297   virtual void reset();
00298 
00314   virtual double getDistance(double x, double y, double z) const;
00315 
00331   virtual double getDistance(int x, int y, int z) const;
00332 
00333   virtual bool isCellValid(int x, int y, int z) const;
00334   virtual int getXNumCells() const;
00335   virtual int getYNumCells() const;
00336   virtual int getZNumCells() const;
00337   virtual bool gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const;
00338   virtual bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const;
00339 
00356   virtual bool writeToStream(std::ostream& stream) const;
00357 
00374   virtual bool readFromStream(std::istream& stream);
00375 
00376   //passthrough docs to DistanceField
00377   virtual double getUninitializedDistance() const
00378   {
00379     return max_distance_;
00380   }
00381 
00393   const PropDistanceFieldVoxel& getCell(int x, int y, int z) const 
00394   {
00395     return voxel_grid_->getCell(x, y, z);
00396   }
00397 
00416   const PropDistanceFieldVoxel* getNearestCell(int x, int y, int z, double& dist, Eigen::Vector3i& pos) const
00417   {
00418     const PropDistanceFieldVoxel* cell = &voxel_grid_->getCell(x, y, z);
00419     if (cell->distance_square_ > 0)
00420     {
00421       dist = sqrt_table_[cell->distance_square_];
00422       pos = cell->closest_point_;
00423       const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z());
00424       return ncell == cell ? NULL : ncell;
00425     }
00426     if (cell->negative_distance_square_ > 0)
00427     {
00428       dist = -sqrt_table_[cell->negative_distance_square_];
00429       pos = cell->closest_negative_point_;
00430       const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z());
00431       return ncell == cell ? NULL : ncell;
00432     }
00433     dist = 0.0;
00434     pos.x() = x;
00435     pos.y() = y;
00436     pos.z() = z;
00437     return NULL;
00438   }
00439 
00448   int getMaximumDistanceSquared() const
00449   {
00450     return max_distance_sq_;
00451   }
00452 
00453 private:
00454 
00455   typedef std::set<Eigen::Vector3i, compareEigen_Vector3i> VoxelSet; 
00463   void initialize();
00464 
00470   void addNewObstacleVoxels(const std::vector<Eigen::Vector3i>& voxel_points);
00471 
00477   void removeObstacleVoxels(const std::vector<Eigen::Vector3i>& voxel_points);
00478 
00485   void propagatePositive();
00486 
00493   void propagateNegative();
00494 
00502   virtual double getDistance(const PropDistanceFieldVoxel& object) const;
00503 
00514   int getDirectionNumber(int dx, int dy, int dz) const;
00515 
00524   Eigen::Vector3i getLocationDifference(int directionNumber) const;
00525 
00531   void initNeighborhoods();
00532 
00538   void print(const VoxelSet & set);
00539 
00545   void print(const EigenSTL::vector_Vector3d& points);
00546 
00555   static int eucDistSq(Eigen::Vector3i point1, Eigen::Vector3i point2);
00556 
00557   bool propagate_negative_;     
00559   boost::shared_ptr<VoxelGrid<PropDistanceFieldVoxel> > voxel_grid_; 
00561 
00562   std::vector<std::vector<Eigen::Vector3i> > bucket_queue_; 
00564   std::vector<std::vector<Eigen::Vector3i> > negative_bucket_queue_;
00566   double max_distance_;         
00567   int max_distance_sq_;         
00569   std::vector<double> sqrt_table_; 
00583   std::vector<std::vector<std::vector<Eigen::Vector3i > > > neighborhoods_;
00584 
00585   std::vector<Eigen::Vector3i > direction_number_to_direction_; 
00586 };
00587 
00589 
00590 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel(int distance_square, int negative_distance_squared):
00591   distance_square_(distance_square),
00592   negative_distance_square_(negative_distance_squared)
00593 {
00594   closest_point_.x() = PropDistanceFieldVoxel::UNINITIALIZED;
00595   closest_point_.y() = PropDistanceFieldVoxel::UNINITIALIZED;
00596   closest_point_.z() = PropDistanceFieldVoxel::UNINITIALIZED;
00597   closest_negative_point_.x() = PropDistanceFieldVoxel::UNINITIALIZED;
00598   closest_negative_point_.y() = PropDistanceFieldVoxel::UNINITIALIZED;
00599   closest_negative_point_.z() = PropDistanceFieldVoxel::UNINITIALIZED;
00600 }
00601 
00602 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel()
00603 {
00604 }
00605 
00606 inline double PropagationDistanceField::getDistance(const PropDistanceFieldVoxel& object) const
00607 {
00608   return sqrt_table_[object.distance_square_]-sqrt_table_[object.negative_distance_square_];
00609 }
00610 
00611 }
00612 
00613 #endif


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:53