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 EigenSTL
49 {
50 typedef std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>> vector_Vector3i;
51 }
52 
53 namespace distance_field
54 {
60 {
61  bool operator()(const Eigen::Vector3i& loc_1, const Eigen::Vector3i& loc_2) const
62  {
63  if (loc_1.z() != loc_2.z())
64  return (loc_1.z() < loc_2.z());
65  else if (loc_1.y() != loc_2.y())
66  return (loc_1.y() < loc_2.y());
67  else if (loc_1.x() != loc_2.x())
68  return (loc_1.x() < loc_2.x());
69  return false;
70  }
71 };
72 
78 {
85 
99  PropDistanceFieldVoxel(int distance_sq_positive, int distance_sq_negative);
100 
103  Eigen::Vector3i closest_point_;
104  Eigen::Vector3i closest_negative_point_;
109  static const int UNINITIALIZED = -1;
111 };
112 
140 {
141 public:
167  PropagationDistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x,
168  double origin_y, double origin_z, double max_distance,
169  bool propagate_negative_distances = false);
170 
193  PropagationDistanceField(const octomap::OcTree& octree, const octomap::point3d& bbx_min,
194  const octomap::point3d& bbx_max, double max_distance,
195  bool propagate_negative_distances = false);
196 
218  PropagationDistanceField(std::istream& stream, double max_distance, bool propagate_negative_distances = false);
225  {
226  }
227 
241  virtual void addPointsToField(const EigenSTL::vector_Vector3d& points);
242 
262  virtual void removePointsFromField(const EigenSTL::vector_Vector3d& points);
263 
286  virtual void updatePointsInField(const EigenSTL::vector_Vector3d& old_points,
287  const EigenSTL::vector_Vector3d& new_points);
288 
294  virtual void reset();
295 
311  virtual double getDistance(double x, double y, double z) const;
312 
328  virtual double getDistance(int x, int y, int z) const;
329 
330  virtual bool isCellValid(int x, int y, int z) const;
331  virtual int getXNumCells() const;
332  virtual int getYNumCells() const;
333  virtual int getZNumCells() const;
334  virtual bool gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const;
335  virtual bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const;
336 
353  virtual bool writeToStream(std::ostream& stream) const;
354 
371  virtual bool readFromStream(std::istream& stream);
372 
373  // passthrough docs to DistanceField
374  virtual double getUninitializedDistance() const
375  {
376  return max_distance_;
377  }
378 
390  const PropDistanceFieldVoxel& getCell(int x, int y, int z) const
391  {
392  return voxel_grid_->getCell(x, y, z);
393  }
394 
413  const PropDistanceFieldVoxel* getNearestCell(int x, int y, int z, double& dist, Eigen::Vector3i& pos) const
414  {
415  const PropDistanceFieldVoxel* cell = &voxel_grid_->getCell(x, y, z);
416  if (cell->distance_square_ > 0)
417  {
418  dist = sqrt_table_[cell->distance_square_];
419  pos = cell->closest_point_;
420  const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z());
421  return ncell == cell ? NULL : ncell;
422  }
423  if (cell->negative_distance_square_ > 0)
424  {
425  dist = -sqrt_table_[cell->negative_distance_square_];
426  pos = cell->closest_negative_point_;
427  const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z());
428  return ncell == cell ? NULL : ncell;
429  }
430  dist = 0.0;
431  pos.x() = x;
432  pos.y() = y;
433  pos.z() = z;
434  return NULL;
435  }
436 
446  {
447  return max_distance_sq_;
448  }
449 
450 private:
452  typedef std::set<Eigen::Vector3i, compareEigen_Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>> VoxelSet;
459  void initialize();
460 
466  void addNewObstacleVoxels(const EigenSTL::vector_Vector3i& voxel_points);
467 
473  void removeObstacleVoxels(const EigenSTL::vector_Vector3i& voxel_points);
474 
481  void propagatePositive();
482 
489  void propagateNegative();
490 
498  virtual double getDistance(const PropDistanceFieldVoxel& object) const;
499 
510  int getDirectionNumber(int dx, int dy, int dz) const;
511 
520  Eigen::Vector3i getLocationDifference(int directionNumber) const;
521 
527  void initNeighborhoods();
528 
534  void print(const VoxelSet& set);
535 
541  void print(const EigenSTL::vector_Vector3d& points);
542 
551  static int eucDistSq(Eigen::Vector3i point1, Eigen::Vector3i point2);
552 
557  std::vector<EigenSTL::vector_Vector3i> bucket_queue_;
563  std::vector<EigenSTL::vector_Vector3i> negative_bucket_queue_;
569  double max_distance_;
572  std::vector<double> sqrt_table_;
586  std::vector<std::vector<EigenSTL::vector_Vector3i>> neighborhoods_;
587 
590 };
591 
593 
594 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel(int distance_square, int negative_distance_squared)
595  : distance_square_(distance_square), negative_distance_square_(negative_distance_squared)
596 {
603 }
604 
606 {
607 }
608 
610 {
611  return sqrt_table_[object.distance_square_] - sqrt_table_[object.negative_distance_square_];
612 }
613 }
614 
615 #endif
bool operator()(const Eigen::Vector3i &loc_1, const Eigen::Vector3i &loc_2) const
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::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
int distance_square_
Distance in cells to the closest obstacle, squared.
std::vector< std::vector< EigenSTL::vector_Vector3i > > neighborhoods_
Holds information on neighbor direction, with 27 different directions. Shows where to propagate given...
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
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
std::vector< EigenSTL::vector_Vector3i > negative_bucket_queue_
Data member that holds points from which to propagate in the negative, where each vector holds points...
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.
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
double z
std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > vector_Vector3i
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.
EigenSTL::vector_Vector3i direction_number_to_direction_
Holds conversion from direction number to integer changes.
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.
std::set< Eigen::Vector3i, compareEigen_Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > VoxelSet
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...
PropDistanceFieldVoxel()
Constructor. All fields left uninitialized.
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 Sun Oct 18 2020 13:16:33