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 {
00054 struct compareEigen_Vector3i
00055 {
00056   bool operator()(Eigen::Vector3i loc_1, Eigen::Vector3i loc_2) const
00057   {
00058     if (loc_1.z() != loc_2.z())
00059       return (loc_1.z() < loc_2.z());
00060     else if (loc_1.y() != loc_2.y())
00061       return (loc_1.y() < loc_2.y());
00062     else if (loc_1.x() != loc_2.x())
00063       return (loc_1.x() < loc_2.x());
00064     return false;
00065   }
00066 };
00067 
00072 struct PropDistanceFieldVoxel
00073 {
00079   PropDistanceFieldVoxel();
00080 
00094   PropDistanceFieldVoxel(int distance_sq_positive, int distance_sq_negative);
00095 
00096   int distance_square_;                    
00097   int negative_distance_square_;           
00098   Eigen::Vector3i closest_point_;          
00099   Eigen::Vector3i closest_negative_point_; 
00100   int update_direction_; 
00101   int negative_update_direction_; 
00104   static const int UNINITIALIZED = -1; 
00105 };
00106 
00133 class PropagationDistanceField : public DistanceField
00134 {
00135 public:
00161   PropagationDistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x,
00162                            double origin_y, double origin_z, double max_distance,
00163                            bool propagate_negative_distances = false);
00164 
00187   PropagationDistanceField(const octomap::OcTree& octree, const octomap::point3d& bbx_min,
00188                            const octomap::point3d& bbx_max, double max_distance,
00189                            bool propagate_negative_distances = false);
00190 
00212   PropagationDistanceField(std::istream& stream, double max_distance, bool propagate_negative_distances = false);
00218   virtual ~PropagationDistanceField()
00219   {
00220   }
00221 
00235   virtual void addPointsToField(const EigenSTL::vector_Vector3d& points);
00236 
00256   virtual void removePointsFromField(const EigenSTL::vector_Vector3d& points);
00257 
00280   virtual void updatePointsInField(const EigenSTL::vector_Vector3d& old_points,
00281                                    const EigenSTL::vector_Vector3d& new_points);
00282 
00288   virtual void reset();
00289 
00305   virtual double getDistance(double x, double y, double z) const;
00306 
00322   virtual double getDistance(int x, int y, int z) const;
00323 
00324   virtual bool isCellValid(int x, int y, int z) const;
00325   virtual int getXNumCells() const;
00326   virtual int getYNumCells() const;
00327   virtual int getZNumCells() const;
00328   virtual bool gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const;
00329   virtual bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const;
00330 
00347   virtual bool writeToStream(std::ostream& stream) const;
00348 
00365   virtual bool readFromStream(std::istream& stream);
00366 
00367   // passthrough docs to DistanceField
00368   virtual double getUninitializedDistance() const
00369   {
00370     return max_distance_;
00371   }
00372 
00384   const PropDistanceFieldVoxel& getCell(int x, int y, int z) const
00385   {
00386     return voxel_grid_->getCell(x, y, z);
00387   }
00388 
00407   const PropDistanceFieldVoxel* getNearestCell(int x, int y, int z, double& dist, Eigen::Vector3i& pos) const
00408   {
00409     const PropDistanceFieldVoxel* cell = &voxel_grid_->getCell(x, y, z);
00410     if (cell->distance_square_ > 0)
00411     {
00412       dist = sqrt_table_[cell->distance_square_];
00413       pos = cell->closest_point_;
00414       const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z());
00415       return ncell == cell ? NULL : ncell;
00416     }
00417     if (cell->negative_distance_square_ > 0)
00418     {
00419       dist = -sqrt_table_[cell->negative_distance_square_];
00420       pos = cell->closest_negative_point_;
00421       const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z());
00422       return ncell == cell ? NULL : ncell;
00423     }
00424     dist = 0.0;
00425     pos.x() = x;
00426     pos.y() = y;
00427     pos.z() = z;
00428     return NULL;
00429   }
00430 
00439   int getMaximumDistanceSquared() const
00440   {
00441     return max_distance_sq_;
00442   }
00443 
00444 private:
00445   typedef std::set<Eigen::Vector3i, compareEigen_Vector3i> VoxelSet; 
00453   void initialize();
00454 
00460   void addNewObstacleVoxels(const std::vector<Eigen::Vector3i>& voxel_points);
00461 
00467   void removeObstacleVoxels(const std::vector<Eigen::Vector3i>& voxel_points);
00468 
00475   void propagatePositive();
00476 
00483   void propagateNegative();
00484 
00492   virtual double getDistance(const PropDistanceFieldVoxel& object) const;
00493 
00504   int getDirectionNumber(int dx, int dy, int dz) const;
00505 
00514   Eigen::Vector3i getLocationDifference(int directionNumber) const;
00515 
00521   void initNeighborhoods();
00522 
00528   void print(const VoxelSet& set);
00529 
00535   void print(const EigenSTL::vector_Vector3d& points);
00536 
00545   static int eucDistSq(Eigen::Vector3i point1, Eigen::Vector3i point2);
00546 
00547   bool propagate_negative_; 
00549   VoxelGrid<PropDistanceFieldVoxel>::Ptr voxel_grid_; 
00551 
00552   std::vector<std::vector<Eigen::Vector3i> > bucket_queue_; 
00557   std::vector<std::vector<Eigen::Vector3i> > negative_bucket_queue_; 
00563   double max_distance_; 
00564   int max_distance_sq_; 
00566   std::vector<double> sqrt_table_; 
00580   std::vector<std::vector<std::vector<Eigen::Vector3i> > > neighborhoods_;
00581 
00582   std::vector<Eigen::Vector3i> direction_number_to_direction_; 
00584 };
00585 
00587 
00588 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel(int distance_square, int negative_distance_squared)
00589   : distance_square_(distance_square), negative_distance_square_(negative_distance_squared)
00590 {
00591   closest_point_.x() = PropDistanceFieldVoxel::UNINITIALIZED;
00592   closest_point_.y() = PropDistanceFieldVoxel::UNINITIALIZED;
00593   closest_point_.z() = PropDistanceFieldVoxel::UNINITIALIZED;
00594   closest_negative_point_.x() = PropDistanceFieldVoxel::UNINITIALIZED;
00595   closest_negative_point_.y() = PropDistanceFieldVoxel::UNINITIALIZED;
00596   closest_negative_point_.z() = PropDistanceFieldVoxel::UNINITIALIZED;
00597 }
00598 
00599 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel()
00600 {
00601 }
00602 
00603 inline double PropagationDistanceField::getDistance(const PropDistanceFieldVoxel& object) const
00604 {
00605   return sqrt_table_[object.distance_square_] - sqrt_table_[object.negative_distance_square_];
00606 }
00607 }
00608 
00609 #endif


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Apr 23 2018 10:24:45