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