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 #pragma once
38 
41 #include <vector>
42 #include <Eigen/Core>
43 #include <set>
44 #include <octomap/octomap.h>
45 
46 namespace EigenSTL
47 {
48 typedef std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>> vector_Vector3i;
49 }
50 
51 namespace distance_field
52 {
58 {
59  bool operator()(const Eigen::Vector3i& loc_1, const Eigen::Vector3i& loc_2) const
60  {
61  if (loc_1.z() != loc_2.z())
62  return (loc_1.z() < loc_2.z());
63  else if (loc_1.y() != loc_2.y())
64  return (loc_1.y() < loc_2.y());
65  else if (loc_1.x() != loc_2.x())
66  return (loc_1.x() < loc_2.x());
67  return false;
68  }
69 };
70 
76 {
83 
97  PropDistanceFieldVoxel(int distance_sq_positive, int distance_sq_negative);
98 
101  Eigen::Vector3i closest_point_;
102  Eigen::Vector3i closest_negative_point_;
107  static const int UNINITIALIZED = -1;
109 };
110 
138 {
139 public:
165  PropagationDistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x,
166  double origin_y, double origin_z, double max_distance,
167  bool propagate_negative_distances = false);
168 
191  PropagationDistanceField(const octomap::OcTree& octree, const octomap::point3d& bbx_min,
192  const octomap::point3d& bbx_max, double max_distance,
193  bool propagate_negative_distances = false);
194 
216  PropagationDistanceField(std::istream& stream, double max_distance, bool propagate_negative_distances = false);
223  {
224  }
225 
239  void addPointsToField(const EigenSTL::vector_Vector3d& points) override;
240 
260  void removePointsFromField(const EigenSTL::vector_Vector3d& points) override;
261 
284  void updatePointsInField(const EigenSTL::vector_Vector3d& old_points,
285  const EigenSTL::vector_Vector3d& new_points) override;
286 
292  void reset() override;
293 
309  double getDistance(double x, double y, double z) const override;
310 
326  double getDistance(int x, int y, int z) const override;
327 
328  bool isCellValid(int x, int y, int z) const override;
329  int getXNumCells() const override;
330  int getYNumCells() const override;
331  int getZNumCells() const override;
332  bool gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const override;
333  bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const override;
334 
351  bool writeToStream(std::ostream& stream) const override;
352 
369  bool readFromStream(std::istream& stream) override;
370 
371  // passthrough docs to DistanceField
372  double getUninitializedDistance() const override
373  {
374  return max_distance_;
375  }
376 
388  const PropDistanceFieldVoxel& getCell(int x, int y, int z) const
389  {
390  return voxel_grid_->getCell(x, y, z);
391  }
392 
411  const PropDistanceFieldVoxel* getNearestCell(int x, int y, int z, double& dist, Eigen::Vector3i& pos) const
412  {
413  const PropDistanceFieldVoxel* cell = &voxel_grid_->getCell(x, y, z);
414  if (cell->distance_square_ > 0)
415  {
416  dist = sqrt_table_[cell->distance_square_];
417  pos = cell->closest_point_;
418  const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z());
419  return ncell == cell ? nullptr : ncell;
420  }
421  if (cell->negative_distance_square_ > 0)
422  {
423  dist = -sqrt_table_[cell->negative_distance_square_];
424  pos = cell->closest_negative_point_;
425  const PropDistanceFieldVoxel* ncell = &voxel_grid_->getCell(pos.x(), pos.y(), pos.z());
426  return ncell == cell ? nullptr : ncell;
427  }
428  dist = 0.0;
429  pos.x() = x;
430  pos.y() = y;
431  pos.z() = z;
432  return nullptr;
433  }
434 
444  {
445  return max_distance_sq_;
446  }
447 
448 private:
450  typedef std::set<Eigen::Vector3i, CompareEigenVector3i, Eigen::aligned_allocator<Eigen::Vector3i>> VoxelSet;
457  void initialize();
458 
464  void addNewObstacleVoxels(const EigenSTL::vector_Vector3i& voxel_points);
465 
471  void removeObstacleVoxels(const EigenSTL::vector_Vector3i& voxel_points);
472 
479  void propagatePositive();
480 
487  void propagateNegative();
488 
496  virtual double getDistance(const PropDistanceFieldVoxel& object) const;
497 
508  int getDirectionNumber(int dx, int dy, int dz) const;
509 
518  Eigen::Vector3i getLocationDifference(int directionNumber) const;
519 
525  void initNeighborhoods();
526 
532  void print(const VoxelSet& set);
533 
539  void print(const EigenSTL::vector_Vector3d& points);
540 
545  std::vector<EigenSTL::vector_Vector3i> bucket_queue_;
551  std::vector<EigenSTL::vector_Vector3i> negative_bucket_queue_;
557  double max_distance_;
560  std::vector<double> sqrt_table_;
574  std::vector<std::vector<EigenSTL::vector_Vector3i>> neighborhoods_;
575 
578 };
579 
581 
582 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel(int distance_square, int negative_distance_squared)
583  : distance_square_(distance_square), negative_distance_square_(negative_distance_squared)
584 {
591 }
592 
594 {
595 }
596 
598 {
599  return sqrt_table_[object.distance_square_] - sqrt_table_[object.negative_distance_square_];
600 }
601 } // namespace distance_field
distance_field::PropagationDistanceField::max_distance_sq_
int max_distance_sq_
Holds maximum distance squared in cells.
Definition: propagation_distance_field.h:558
distance_field::PropagationDistanceField::addNewObstacleVoxels
void addNewObstacleVoxels(const EigenSTL::vector_Vector3i &voxel_points)
Adds a valid set of integer points to the voxel grid.
Definition: propagation_distance_field.cpp:252
distance_field::PropagationDistanceField::print
void print(const VoxelSet &set)
Debug function that prints all voxels in a set to ROS_DEBUG_NAMED.
Definition: propagation_distance_field.cpp:128
distance_field::PropagationDistanceField::sqrt_table_
std::vector< double > sqrt_table_
Precomputed square root table for faster distance lookups.
Definition: propagation_distance_field.h:560
distance_field::PropagationDistanceField::PropagationDistanceField
PropagationDistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, double origin_z, double max_distance, bool propagate_negative_distances=false)
Constructor that initializes entire distance field to empty - all cells will be assigned maximum dist...
Definition: propagation_distance_field.cpp:78
distance_field::PropagationDistanceField::getLocationDifference
Eigen::Vector3i getLocationDifference(int directionNumber) const
Helper function that gets change values given single number representing update direction.
Definition: propagation_distance_field.cpp:622
distance_field::PropagationDistanceField::gridToWorld
bool gridToWorld(int x, int y, int z, double &world_x, double &world_y, double &world_z) const override
Converts from an set of integer indices to a world location given the origin and resolution parameter...
Definition: propagation_distance_field.cpp:657
distance_field::PropagationDistanceField::VoxelSet
std::set< Eigen::Vector3i, CompareEigenVector3i, Eigen::aligned_allocator< Eigen::Vector3i > > VoxelSet
Definition: propagation_distance_field.h:450
distance_field::PropagationDistanceField
A DistanceField implementation that uses a vector propagation method. Distances propagate outward fro...
Definition: propagation_distance_field.h:137
distance_field::PropagationDistanceField::getMaximumDistanceSquared
int getMaximumDistanceSquared() const
Gets the maximum distance squared value.
Definition: propagation_distance_field.h:443
distance_field::PropagationDistanceField::~PropagationDistanceField
~PropagationDistanceField() override
Empty destructor.
Definition: propagation_distance_field.h:222
distance_field::PropagationDistanceField::bucket_queue_
std::vector< EigenSTL::vector_Vector3i > bucket_queue_
Structure used to hold propagation frontier.
Definition: propagation_distance_field.h:546
distance_field::PropDistanceFieldVoxel::closest_point_
Eigen::Vector3i closest_point_
Closest occupied cell.
Definition: propagation_distance_field.h:101
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
distance_field::PropagationDistanceField::reset
void reset() override
Resets the entire distance field to max_distance for positive values and zero for negative values.
Definition: propagation_distance_field.cpp:539
distance_field::PropagationDistanceField::addPointsToField
void addPointsToField(const EigenSTL::vector_Vector3d &points) override
Add a set of obstacle points to the distance field, updating distance values accordingly....
Definition: propagation_distance_field.cpp:208
distance_field::PropDistanceFieldVoxel::closest_negative_point_
Eigen::Vector3i closest_negative_point_
Closest unoccupied cell.
Definition: propagation_distance_field.h:102
distance_field::PropagationDistanceField::isCellValid
bool isCellValid(int x, int y, int z) const override
Determines whether or not the cell associated with the supplied indices is valid for this distance fi...
Definition: propagation_distance_field.cpp:637
distance_field::PropagationDistanceField::propagate_negative_
bool propagate_negative_
Whether or not to propagate negative distances.
Definition: propagation_distance_field.h:541
distance_field::PropagationDistanceField::voxel_grid_
VoxelGrid< PropDistanceFieldVoxel >::Ptr voxel_grid_
Actual container for distance data.
Definition: propagation_distance_field.h:543
distance_field::PropagationDistanceField::worldToGrid
bool worldToGrid(double world_x, double world_y, double world_z, int &x, int &y, int &z) const override
Converts from a world location to a set of integer indices. Should return false if the world location...
Definition: propagation_distance_field.cpp:663
distance_field::CompareEigenVector3i
Struct for sorting type Eigen::Vector3i for use in sorted std containers. Sorts in z order,...
Definition: propagation_distance_field.h:57
distance_field::PropagationDistanceField::removeObstacleVoxels
void removeObstacleVoxels(const EigenSTL::vector_Vector3i &voxel_points)
Removes a valid set of integer points from the voxel grid.
Definition: propagation_distance_field.cpp:334
EigenSTL
Definition: propagation_distance_field.h:46
distance_field::VoxelGrid
VoxelGrid holds a dense 3D, axis-aligned set of data at a given resolution, where the data is supplie...
Definition: voxel_grid.h:93
octomath::Vector3
distance_field::PropagationDistanceField::negative_bucket_queue_
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...
Definition: propagation_distance_field.h:551
distance_field::CompareEigenVector3i::operator()
bool operator()(const Eigen::Vector3i &loc_1, const Eigen::Vector3i &loc_2) const
Definition: propagation_distance_field.h:59
octomap::OcTree
EigenSTL::vector_Vector3i
std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > vector_Vector3i
Definition: propagation_distance_field.h:80
distance_field::DistanceField
DistanceField is an abstract base class for computing distances from sets of 3D obstacle points....
Definition: distance_field.h:91
distance_field::PropDistanceFieldVoxel::update_direction_
int update_direction_
Direction from which this voxel was updated for occupied distance propagation.
Definition: propagation_distance_field.h:103
y
double y
distance_field::PropagationDistanceField::updatePointsInField
void updatePointsInField(const EigenSTL::vector_Vector3d &old_points, const EigenSTL::vector_Vector3d &new_points) override
This function will remove any obstacle points that are in the old point set but not the new point set...
Definition: propagation_distance_field.cpp:152
distance_field::PropDistanceFieldVoxel::UNINITIALIZED
static const int UNINITIALIZED
Value that represents an unitialized voxel.
Definition: propagation_distance_field.h:107
distance_field::PropagationDistanceField::direction_number_to_direction_
EigenSTL::vector_Vector3i direction_number_to_direction_
Holds conversion from direction number to integer changes.
Definition: propagation_distance_field.h:576
distance_field::PropagationDistanceField::getDistance
double getDistance(double x, double y, double z) const override
Get the distance value associated with the cell indicated by the world coordinate....
Definition: propagation_distance_field.cpp:627
distance_field::PropagationDistanceField::getDirectionNumber
int getDirectionNumber(int dx, int dy, int dz) const
Helper function to get a single number in a 27 connected 3D voxel grid given dx, dy,...
Definition: propagation_distance_field.cpp:617
distance_field::PropagationDistanceField::initNeighborhoods
void initNeighborhoods()
Helper function for computing location and neighborhood information in 27 connected voxel grid.
Definition: propagation_distance_field.cpp:559
distance_field::PropagationDistanceField::max_distance_
double max_distance_
Holds maximum distance
Definition: propagation_distance_field.h:557
distance_field::PropagationDistanceField::writeToStream
bool writeToStream(std::ostream &stream) const override
Writes the contents of the distance field to the supplied stream.
Definition: propagation_distance_field.cpp:668
distance_field.h
distance_field
Namespace for holding classes that generate distance fields.
Definition: distance_field.h:64
distance_field::PropagationDistanceField::getYNumCells
int getYNumCells() const override
Gets the number of cells along the Y axis.
Definition: propagation_distance_field.cpp:647
distance_field::PropagationDistanceField::neighborhoods_
std::vector< std::vector< EigenSTL::vector_Vector3i > > neighborhoods_
Holds information on neighbor direction, with 27 different directions. Shows where to propagate given...
Definition: propagation_distance_field.h:574
distance_field::PropDistanceFieldVoxel::distance_square_
int distance_square_
Distance in cells to the closest obstacle, squared.
Definition: propagation_distance_field.h:99
distance_field::PropagationDistanceField::initialize
void initialize()
Initializes the field, resetting the voxel grid and building a sqrt lookup table for efficiency based...
Definition: propagation_distance_field.cpp:108
distance_field::PropDistanceFieldVoxel::negative_distance_square_
int negative_distance_square_
Distance in cells to the nearest unoccupied cell, squared.
Definition: propagation_distance_field.h:100
distance_field::PropagationDistanceField::removePointsFromField
void removePointsFromField(const EigenSTL::vector_Vector3d &points) override
Remove a set of obstacle points from the distance field, updating distance values accordingly.
Definition: propagation_distance_field.cpp:229
distance_field::PropagationDistanceField::propagateNegative
void propagateNegative()
Propagates inward to a maximum distance given the contents of the negative_bucket_queue_,...
Definition: propagation_distance_field.cpp:478
octomap.h
voxel_grid.h
x
double x
distance_field::PropDistanceFieldVoxel::negative_update_direction_
int negative_update_direction_
Direction from which this voxel was updated for negative distance propagation.
Definition: propagation_distance_field.h:104
distance_field::PropagationDistanceField::getUninitializedDistance
double getUninitializedDistance() const override
Gets a distance value for an invalid cell.
Definition: propagation_distance_field.h:372
distance_field::PropagationDistanceField::getZNumCells
int getZNumCells() const override
Gets the number of cells along the Z axis.
Definition: propagation_distance_field.cpp:652
distance_field::PropagationDistanceField::readFromStream
bool readFromStream(std::istream &stream) override
Reads, parameterizes, and populates the distance field based on the supplied stream.
Definition: propagation_distance_field.cpp:708
distance_field::PropagationDistanceField::getCell
const PropDistanceFieldVoxel & getCell(int x, int y, int z) const
Gets full cell data given an index.
Definition: propagation_distance_field.h:388
distance_field::PropagationDistanceField::getNearestCell
const PropDistanceFieldVoxel * getNearestCell(int x, int y, int z, double &dist, Eigen::Vector3i &pos) const
Gets nearest surface cell and returns distance to it.
Definition: propagation_distance_field.h:411
z
double z
distance_field::PropDistanceFieldVoxel::PropDistanceFieldVoxel
PropDistanceFieldVoxel()
Constructor. All fields left uninitialized.
Definition: propagation_distance_field.h:593
distance_field::PropagationDistanceField::propagatePositive
void propagatePositive()
Propagates outward to the maximum distance given the contents of the bucket_queue_,...
Definition: propagation_distance_field.cpp:421
distance_field::PropagationDistanceField::getXNumCells
int getXNumCells() const override
Gets the number of cells along the X axis.
Definition: propagation_distance_field.cpp:642
distance_field::PropDistanceFieldVoxel
Structure that holds voxel information for the DistanceField. Will be used in VoxelGrid.
Definition: propagation_distance_field.h:75


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Dec 3 2021 03:23:21