A DistanceField implementation that uses a vector propagation method. More...
#include <propagation_distance_field.h>
Public Member Functions | |
virtual void | addPointsToField (const std::vector< tf::Vector3 > &points) |
Add (and expand) a set of points to the distance field. | |
void | getOccupiedVoxelMarkers (const std::string &frame_id, const ros::Time stamp, const tf::Transform &cur, visualization_msgs::Marker &marker) |
Get visualization markers for the set of occupied cells. | |
PropagationDistanceField (double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, double origin_z, double max_distance) | |
Constructor for the DistanceField. | |
virtual void | reset () |
Resets the distance field to the max_distance. | |
virtual void | updatePointsInField (const std::vector< tf::Vector3 > &points, const bool iterative=true) |
Change the set of obstacle points and recalculate the distance field (if there are any changes). | |
virtual | ~PropagationDistanceField () |
Private Types | |
typedef std::set< int3, compareInt3 > | VoxelSet |
The set of all the obstacle voxels. | |
Private Member Functions | |
void | addNewObstacleVoxels (const VoxelSet &points) |
int | getDirectionNumber (int dx, int dy, int dz) const |
virtual double | getDistance (const PropDistanceFieldVoxel &object) const |
int3 | getLocationDifference (int directionNumber) const |
void | initNeighborhoods () |
void | propogate () |
void | removeObstacleVoxels (const VoxelSet &points) |
Static Private Member Functions | |
static int | eucDistSq (int3 point1, int3 point2) |
Private Attributes | |
std::vector< std::vector < PropDistanceFieldVoxel * > > | bucket_queue_ |
Structure used to hold propogation frontier. | |
std::vector< int3 > | direction_number_to_direction_ |
double | max_distance_ |
int | max_distance_sq_ |
std::vector< std::vector < std::vector< int3 > > > | neighborhoods_ |
VoxelSet | object_voxel_locations_ |
std::vector< double > | sqrt_table_ |
A DistanceField implementation that uses a vector propagation method.
It computes the distance transform of the input points, and stores the distance to the closest obstacle in each voxel. Also available is the location of the closest point, and the gradient of the field at a point. Expansion of obstacles is performed upto a given radius.
Definition at line 108 of file propagation_distance_field.h.
typedef std::set<int3, compareInt3> distance_field::PropagationDistanceField::VoxelSet [private] |
The set of all the obstacle voxels.
Definition at line 149 of file propagation_distance_field.h.
distance_field::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 | ||
) |
Constructor for the DistanceField.
Definition at line 47 of file propagation_distance_field.cpp.
Definition at line 43 of file propagation_distance_field.cpp.
void distance_field::PropagationDistanceField::addNewObstacleVoxels | ( | const VoxelSet & | points | ) | [private] |
Definition at line 167 of file propagation_distance_field.cpp.
void distance_field::PropagationDistanceField::addPointsToField | ( | const std::vector< tf::Vector3 > & | points | ) | [virtual] |
Add (and expand) a set of points to the distance field.
Implements distance_field::DistanceField< PropDistanceFieldVoxel >.
Definition at line 139 of file propagation_distance_field.cpp.
int distance_field::PropagationDistanceField::eucDistSq | ( | int3 | point1, |
int3 | point2 | ||
) | [static, private] |
Definition at line 64 of file propagation_distance_field.cpp.
int distance_field::PropagationDistanceField::getDirectionNumber | ( | int | dx, |
int | dy, | ||
int | dz | ||
) | const [private] |
Definition at line 434 of file propagation_distance_field.cpp.
double distance_field::PropagationDistanceField::getDistance | ( | const PropDistanceFieldVoxel & | object | ) | const [inline, private, virtual] |
Implements distance_field::DistanceField< PropDistanceFieldVoxel >.
Definition at line 194 of file propagation_distance_field.h.
int3 distance_field::PropagationDistanceField::getLocationDifference | ( | int | directionNumber | ) | const [private] |
Definition at line 439 of file propagation_distance_field.cpp.
void distance_field::PropagationDistanceField::getOccupiedVoxelMarkers | ( | const std::string & | frame_id, |
const ros::Time | stamp, | ||
const tf::Transform & | cur, | ||
visualization_msgs::Marker & | marker | ||
) |
Get visualization markers for the set of occupied cells.
marker | the marker to be published |
Definition at line 329 of file propagation_distance_field.cpp.
void distance_field::PropagationDistanceField::initNeighborhoods | ( | ) | [private] |
Definition at line 376 of file propagation_distance_field.cpp.
void distance_field::PropagationDistanceField::propogate | ( | ) | [private] |
Definition at line 257 of file propagation_distance_field.cpp.
void distance_field::PropagationDistanceField::removeObstacleVoxels | ( | const VoxelSet & | points | ) | [private] |
Definition at line 194 of file propagation_distance_field.cpp.
void distance_field::PropagationDistanceField::reset | ( | ) | [virtual] |
Resets the distance field to the max_distance.
Implements distance_field::DistanceField< PropDistanceFieldVoxel >.
Definition at line 371 of file propagation_distance_field.cpp.
void distance_field::PropagationDistanceField::updatePointsInField | ( | const std::vector< tf::Vector3 > & | points, |
const bool | iterative = true |
||
) | [virtual] |
Change the set of obstacle points and recalculate the distance field (if there are any changes).
iterative | Calculate the changes in the object voxels, and propogate the changes outward. Otherwise, clear the distance map and recalculate the entire voxel map. |
Definition at line 72 of file propagation_distance_field.cpp.
std::vector<std::vector<PropDistanceFieldVoxel*> > distance_field::PropagationDistanceField::bucket_queue_ [private] |
Structure used to hold propogation frontier.
Definition at line 153 of file propagation_distance_field.h.
std::vector<int3 > distance_field::PropagationDistanceField::direction_number_to_direction_ [private] |
Definition at line 166 of file propagation_distance_field.h.
double distance_field::PropagationDistanceField::max_distance_ [private] |
Definition at line 154 of file propagation_distance_field.h.
Definition at line 155 of file propagation_distance_field.h.
std::vector<std::vector<std::vector<int3 > > > distance_field::PropagationDistanceField::neighborhoods_ [private] |
Definition at line 164 of file propagation_distance_field.h.
Definition at line 150 of file propagation_distance_field.h.
std::vector<double> distance_field::PropagationDistanceField::sqrt_table_ [private] |
Definition at line 157 of file propagation_distance_field.h.