37 #ifndef MOVEIT_DISTANCE_FIELD_PROPAGATION_DISTANCE_FIELD_ 38 #define MOVEIT_DISTANCE_FIELD_PROPAGATION_DISTANCE_FIELD_ 49 typedef std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>>
vector_Vector3i;
60 bool operator()(
const Eigen::Vector3i& loc_1,
const Eigen::Vector3i& loc_2)
const 62 if (loc_1.z() != loc_2.z())
63 return (loc_1.z() < loc_2.z());
64 else if (loc_1.y() != loc_2.y())
65 return (loc_1.y() < loc_2.y());
66 else if (loc_1.x() != loc_2.x())
67 return (loc_1.x() < loc_2.x());
108 static const int UNINITIALIZED = -1;
167 double origin_y,
double origin_z,
double max_distance,
168 bool propagate_negative_distances =
false);
194 bool propagate_negative_distances =
false);
293 void reset()
override;
310 double getDistance(
double x,
double y,
double z)
const override;
327 double getDistance(
int x,
int y,
int z)
const override;
329 bool isCellValid(
int x,
int y,
int z)
const override;
330 int getXNumCells()
const override;
331 int getYNumCells()
const override;
332 int getZNumCells()
const override;
333 bool gridToWorld(
int x,
int y,
int z,
double& world_x,
double& world_y,
double& world_z)
const override;
334 bool worldToGrid(
double world_x,
double world_y,
double world_z,
int& x,
int& y,
int& z)
const override;
352 bool writeToStream(std::ostream& stream)
const override;
370 bool readFromStream(std::istream& stream)
override;
375 return max_distance_;
391 return voxel_grid_->getCell(x, y, z);
420 return ncell == cell ? NULL : ncell;
427 return ncell == cell ? NULL : ncell;
446 return max_distance_sq_;
451 typedef std::set<Eigen::Vector3i, compareEigen_Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>>
VoxelSet;
480 void propagatePositive();
488 void propagateNegative();
509 int getDirectionNumber(
int dx,
int dy,
int dz)
const;
519 Eigen::Vector3i getLocationDifference(
int directionNumber)
const;
526 void initNeighborhoods();
533 void print(
const VoxelSet&
set);
546 std::vector<EigenSTL::vector_Vector3i> bucket_queue_;
583 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel(
int distance_square,
int negative_distance_squared)
584 : distance_square_(distance_square), negative_distance_square_(negative_distance_squared)
600 return sqrt_table_[
object.distance_square_] - sqrt_table_[
object.negative_distance_square_];
bool propagate_negative_
Whether or not to propagate negative distances.
bool operator()(const Eigen::Vector3i &loc_1, const Eigen::Vector3i &loc_2) const
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
const PropDistanceFieldVoxel & getCell(int x, int y, int z) const
Gets full cell data given an index.
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.
VoxelGrid holds a dense 3D, axis-aligned set of data at a given resolution, where the data is supplie...
Struct for sorting type Eigen::Vector3i for use in sorted std containers. Sorts in z order...
const PropDistanceFieldVoxel * getNearestCell(int x, int y, int z, double &dist, Eigen::Vector3i &pos) const
Gets nearest surface cell and returns distance to it.
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.
~PropagationDistanceField() override
Empty destructor.
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...
VoxelGrid< PropDistanceFieldVoxel >::Ptr voxel_grid_
Actual container for distance data.
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > vector_Vector3i
double getUninitializedDistance() const override
Gets a distance value for an invalid cell.
int getMaximumDistanceSquared() const
Gets the maximum distance squared value.
Namespace for holding classes that generate distance fields.
double max_distance_
Holds maximum distance.
EigenSTL::vector_Vector3i direction_number_to_direction_
Holds conversion from direction number to integer changes.
double getDistance(double x, double y, double z) const override
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.
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
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.
void print(PropagationDistanceField &pdf, int numX, int numY, int numZ)
int max_distance_sq_
Holds maximum distance squared in cells.