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 
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     return voxel_grid_->getCell(x, y, z);
00395   }
00396 
00405   int getMaximumDistanceSquared() const
00406   {
00407     return max_distance_sq_;
00408   }
00409 
00410 private:
00411 
00412   typedef std::set<Eigen::Vector3i, compareEigen_Vector3i> VoxelSet; 
00420   void initialize();
00421 
00427   void addNewObstacleVoxels(const std::vector<Eigen::Vector3i>& voxel_points);
00428 
00434   void removeObstacleVoxels(const std::vector<Eigen::Vector3i>& voxel_points);
00435 
00442   void propagatePositive();
00443 
00450   void propagateNegative();
00451 
00459   virtual double getDistance(const PropDistanceFieldVoxel& object) const;
00460 
00471   int getDirectionNumber(int dx, int dy, int dz) const;
00472 
00481   Eigen::Vector3i getLocationDifference(int directionNumber) const;
00482 
00488   void initNeighborhoods();
00489 
00495   void print(const VoxelSet & set);
00496 
00502   void print(const EigenSTL::vector_Vector3d& points);
00503 
00512   static int eucDistSq(Eigen::Vector3i point1, Eigen::Vector3i point2);
00513 
00514   bool propagate_negative_;     
00516   boost::shared_ptr<VoxelGrid<PropDistanceFieldVoxel> > voxel_grid_; 
00518 
00519   std::vector<std::vector<Eigen::Vector3i> > bucket_queue_; 
00521   std::vector<std::vector<Eigen::Vector3i> > negative_bucket_queue_;
00523   double max_distance_;         
00524   int max_distance_sq_;         
00526   std::vector<double> sqrt_table_; 
00540   std::vector<std::vector<std::vector<Eigen::Vector3i > > > neighborhoods_;
00541 
00542   std::vector<Eigen::Vector3i > direction_number_to_direction_; 
00543 };
00544 
00546 
00547 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel(int distance_square, int negative_distance_squared):
00548   distance_square_(distance_square),
00549   negative_distance_square_(negative_distance_squared)
00550 {
00551   closest_point_.x() = PropDistanceFieldVoxel::UNINITIALIZED;
00552   closest_point_.y() = PropDistanceFieldVoxel::UNINITIALIZED;
00553   closest_point_.z() = PropDistanceFieldVoxel::UNINITIALIZED;
00554   closest_negative_point_.x() = PropDistanceFieldVoxel::UNINITIALIZED;
00555   closest_negative_point_.y() = PropDistanceFieldVoxel::UNINITIALIZED;
00556   closest_negative_point_.z() = PropDistanceFieldVoxel::UNINITIALIZED;
00557 }
00558 
00559 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel()
00560 {
00561 }
00562 
00563 inline double PropagationDistanceField::getDistance(const PropDistanceFieldVoxel& object) const
00564 {
00565   return sqrt_table_[object.distance_square_]-sqrt_table_[object.negative_distance_square_];
00566 }
00567 
00568 }
00569 
00570 #endif


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Oct 6 2014 02:24:47