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 {
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