Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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