37 #ifndef MOVEIT_DISTANCE_FIELD_PROPAGATION_DISTANCE_FIELD_ 38 #define MOVEIT_DISTANCE_FIELD_PROPAGATION_DISTANCE_FIELD_ 50 typedef std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>>
vector_Vector3i;
61 bool operator()(
const Eigen::Vector3i& loc_1,
const Eigen::Vector3i& loc_2)
const 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());
109 static const int UNINITIALIZED = -1;
169 bool propagate_negative_distances =
false);
195 bool propagate_negative_distances =
false);
294 virtual void reset();
311 virtual double getDistance(
double x,
double y,
double z)
const;
328 virtual double getDistance(
int x,
int y,
int z)
const;
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;
353 virtual bool writeToStream(std::ostream& stream)
const;
371 virtual bool readFromStream(std::istream& stream);
376 return max_distance_;
392 return voxel_grid_->getCell(x, y, z);
421 return ncell == cell ? NULL : ncell;
428 return ncell == cell ? NULL : ncell;
447 return max_distance_sq_;
452 typedef std::set<Eigen::Vector3i, compareEigen_Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>>
VoxelSet;
481 void propagatePositive();
489 void propagateNegative();
510 int getDirectionNumber(
int dx,
int dy,
int dz)
const;
520 Eigen::Vector3i getLocationDifference(
int directionNumber)
const;
527 void initNeighborhoods();
534 void print(
const VoxelSet&
set);
551 static int eucDistSq(Eigen::Vector3i
point1, Eigen::Vector3i
point2);
557 std::vector<EigenSTL::vector_Vector3i> bucket_queue_;
594 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel(
int distance_square,
int negative_distance_squared)
595 : distance_square_(distance_square), negative_distance_square_(negative_distance_squared)
611 return sqrt_table_[
object.distance_square_] - sqrt_table_[
object.negative_distance_square_];
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.
virtual ~PropagationDistanceField()
Empty destructor.
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...
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.
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.
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)
double max_distance_
Holds maximum distance.
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.
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