propagation_distance_field.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Mrinal Kalakrishnan, Ken Anderson */
36 
37 #ifndef MOVEIT_DISTANCE_FIELD_PROPAGATION_DISTANCE_FIELD_
38 #define MOVEIT_DISTANCE_FIELD_PROPAGATION_DISTANCE_FIELD_
39 
42 #include <vector>
43 #include <list>
44 #include <Eigen/Core>
45 #include <set>
46 #include <octomap/octomap.h>
47 
48 namespace distance_field
49 {
55 {
56  bool operator()(Eigen::Vector3i loc_1, Eigen::Vector3i loc_2) const
57  {
58  if (loc_1.z() != loc_2.z())
59  return (loc_1.z() < loc_2.z());
60  else if (loc_1.y() != loc_2.y())
61  return (loc_1.y() < loc_2.y());
62  else if (loc_1.x() != loc_2.x())
63  return (loc_1.x() < loc_2.x());
64  return false;
65  }
66 };
67 
73 {
80 
94  PropDistanceFieldVoxel(int distance_sq_positive, int distance_sq_negative);
95 
98  Eigen::Vector3i closest_point_;
99  Eigen::Vector3i closest_negative_point_;
104  static const int UNINITIALIZED = -1;
105 };
106 
134 {
135 public:
161  PropagationDistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x,
162  double origin_y, double origin_z, double max_distance,
163  bool propagate_negative_distances = false);
164 
187  PropagationDistanceField(const octomap::OcTree& octree, const octomap::point3d& bbx_min,
188  const octomap::point3d& bbx_max, double max_distance,
189  bool propagate_negative_distances = false);
190 
212  PropagationDistanceField(std::istream& stream, double max_distance, bool propagate_negative_distances = false);
219  {
220  }
221 
235  virtual void addPointsToField(const EigenSTL::vector_Vector3d& points);
236 
256  virtual void removePointsFromField(const EigenSTL::vector_Vector3d& points);
257 
280  virtual void updatePointsInField(const EigenSTL::vector_Vector3d& old_points,
281  const EigenSTL::vector_Vector3d& new_points);
282 
288  virtual void reset();
289 
305  virtual double getDistance(double x, double y, double z) const;
306 
322  virtual double getDistance(int x, int y, int z) const;
323 
324  virtual bool isCellValid(int x, int y, int z) const;
325  virtual int getXNumCells() const;
326  virtual int getYNumCells() const;
327  virtual int getZNumCells() const;
328  virtual bool gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const;
329  virtual bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const;
330 
347  virtual bool writeToStream(std::ostream& stream) const;
348 
365  virtual bool readFromStream(std::istream& stream);
366 
367  // passthrough docs to DistanceField
368  virtual double getUninitializedDistance() const
369  {
370  return max_distance_;
371  }
372 
384  const PropDistanceFieldVoxel& getCell(int x, int y, int z) const
385  {
386  return voxel_grid_->getCell(x, y, z);
387  }
388 
407  const PropDistanceFieldVoxel* getNearestCell(int x, int y, int z, double& dist, Eigen::Vector3i& pos) const
408  {
409  const PropDistanceFieldVoxel* cell = &voxel_grid_->getCell(x, y, z);
410  if (cell->distance_square_ > 0)
411  {
412  dist = sqrt_table_[cell->distance_square_];
413  pos = cell->closest_point_;
414  const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z());
415  return ncell == cell ? NULL : ncell;
416  }
417  if (cell->negative_distance_square_ > 0)
418  {
419  dist = -sqrt_table_[cell->negative_distance_square_];
420  pos = cell->closest_negative_point_;
421  const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z());
422  return ncell == cell ? NULL : ncell;
423  }
424  dist = 0.0;
425  pos.x() = x;
426  pos.y() = y;
427  pos.z() = z;
428  return NULL;
429  }
430 
440  {
441  return max_distance_sq_;
442  }
443 
444 private:
445  typedef std::set<Eigen::Vector3i, compareEigen_Vector3i> VoxelSet;
453  void initialize();
454 
460  void addNewObstacleVoxels(const std::vector<Eigen::Vector3i>& voxel_points);
461 
467  void removeObstacleVoxels(const std::vector<Eigen::Vector3i>& voxel_points);
468 
475  void propagatePositive();
476 
483  void propagateNegative();
484 
492  virtual double getDistance(const PropDistanceFieldVoxel& object) const;
493 
504  int getDirectionNumber(int dx, int dy, int dz) const;
505 
514  Eigen::Vector3i getLocationDifference(int directionNumber) const;
515 
521  void initNeighborhoods();
522 
528  void print(const VoxelSet& set);
529 
535  void print(const EigenSTL::vector_Vector3d& points);
536 
545  static int eucDistSq(Eigen::Vector3i point1, Eigen::Vector3i point2);
546 
551  std::vector<std::vector<Eigen::Vector3i> > bucket_queue_;
557  std::vector<std::vector<Eigen::Vector3i> > negative_bucket_queue_;
563  double max_distance_;
566  std::vector<double> sqrt_table_;
580  std::vector<std::vector<std::vector<Eigen::Vector3i> > > neighborhoods_;
581 
582  std::vector<Eigen::Vector3i> direction_number_to_direction_;
584 };
585 
587 
588 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel(int distance_square, int negative_distance_squared)
589  : distance_square_(distance_square), negative_distance_square_(negative_distance_squared)
590 {
597 }
598 
600 {
601 }
602 
604 {
605  return sqrt_table_[object.distance_square_] - sqrt_table_[object.negative_distance_square_];
606 }
607 }
608 
609 #endif
const PropDistanceFieldVoxel & getCell(int x, int y, int z) const
Gets full cell data given an index.
static const Eigen::Vector3d point2(0.0, 0.1, 0.2)
bool propagate_negative_
Whether or not to propagate negative distances.
std::set< Eigen::Vector3i, compareEigen_Vector3i > VoxelSet
Typedef for set of integer indices.
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
int distance_square_
Distance in cells to the closest obstacle, squared.
int update_direction_
Direction from which this voxel was updated for occupied distance propagation.
ROSCONSOLE_DECL void initialize()
Structure that holds voxel information for the DistanceField. Will be used in VoxelGrid.
static const double origin_y
std::vector< std::vector< Eigen::Vector3i > > negative_bucket_queue_
Data member that holds points from which to propagate in the negative, where each vector holds points...
VoxelGrid holds a dense 3D, axis-aligned set of data at a given resolution, where the data is supplie...
Definition: voxel_grid.h:62
Struct for sorting type Eigen::Vector3i for use in sorted std containers. Sorts in z order...
Eigen::Vector3i closest_point_
Closest occupied cell.
Eigen::Vector3i closest_negative_point_
Closest unoccupied cell.
static const int UNINITIALIZED
Value that represents an unitialized voxel.
double y
DistanceField is an abstract base class for computing distances from sets of 3D obstacle points...
static const double resolution
static const double origin_z
VoxelGrid< PropDistanceFieldVoxel >::Ptr voxel_grid_
Actual container for distance data.
double z
int getMaximumDistanceSquared() const
Gets the maximum distance squared value.
Namespace for holding classes that generate distance fields.
static const Eigen::Vector3d point1(0.1, 0.0, 0.0)
virtual double getDistance(double x, double y, double z) const
Get the distance value associated with the cell indicated by the world coordinate. If the cell is invalid, max_distance will be returned. If running without negative distances, all obstacle cells will have zero distance. If running with negative distances, the distance will be between -max_distance and max_distance, with no values having a 0 distance.
int negative_distance_square_
Distance in cells to the nearest unoccupied cell, squared.
virtual double getUninitializedDistance() const
Gets a distance value for an invalid cell.
std::vector< double > sqrt_table_
Precomputed square root table for faster distance lookups.
const PropDistanceFieldVoxel * getNearestCell(int x, int y, int z, double &dist, Eigen::Vector3i &pos) const
Gets nearest surface cell and returns distance to it.
A DistanceField implementation that uses a vector propagation method. Distances propagate outward fro...
std::vector< Eigen::Vector3i > direction_number_to_direction_
Holds conversion from direction number to integer changes.
std::vector< std::vector< std::vector< Eigen::Vector3i > > > neighborhoods_
Holds information on neighbor direction, with 27 different directions. Shows where to propagate given...
PropDistanceFieldVoxel()
Constructor. All fields left uninitialized.
bool operator()(Eigen::Vector3i loc_1, Eigen::Vector3i loc_2) const
int negative_update_direction_
Direction from which this voxel was updated for negative distance propagation.
double x
void print(PropagationDistanceField &pdf, int numX, int numY, int numZ)
int max_distance_sq_
Holds maximum distance squared in cells.
static const double origin_x


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jan 15 2018 03:50:44