38 #include <visualization_msgs/Marker.h> 40 #include <boost/iostreams/filtering_stream.hpp> 41 #include <boost/iostreams/copy.hpp> 42 #include <boost/iostreams/filter/zlib.hpp> 48 double max_distance,
bool propagate_negative)
49 :
DistanceField(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z)
50 , propagate_negative_(propagate_negative)
51 , max_distance_(max_distance)
58 bool propagate_negative_distances)
59 :
DistanceField(bbx_max.
x() - bbx_min.
x(), bbx_max.
y() - bbx_min.
y(), bbx_max.
z() - bbx_min.
z(),
70 bool propagate_negative_distances)
97 int dx = point1.x() - point2.x();
98 int dy = point1.y() - point2.y();
99 int dz = point1.z() - point2.z();
100 return dx * dx + dy * dy + dz * dz;
106 VoxelSet::const_iterator it;
107 for (it =
set.begin(); it !=
set.end(); ++it)
109 Eigen::Vector3i loc1 = *it;
110 ROS_DEBUG_NAMED(
"distance_field",
"%d, %d, %d ", loc1.x(), loc1.y(), loc1.z());
112 ROS_DEBUG_NAMED(
"distance_field",
"] size=%u\n", (
unsigned int)
set.size());
118 EigenSTL::vector_Vector3d::const_iterator it;
119 for (it = points.begin(); it != points.end(); ++it)
121 Eigen::Vector3d loc1 = *it;
122 ROS_DEBUG_NAMED(
"distance_field",
"%g, %g, %g ", loc1.x(), loc1.y(), loc1.z());
124 ROS_DEBUG_NAMED(
"distance_field",
"] size=%u\n", (
unsigned int)points.size());
131 for (
unsigned int i = 0; i < old_points.size(); i++)
133 Eigen::Vector3i voxel_loc;
134 bool valid =
worldToGrid(old_points[i].
x(), old_points[i].
y(), old_points[i].
z(), voxel_loc.x(), voxel_loc.y(),
138 old_point_set.insert(voxel_loc);
143 for (
unsigned int i = 0; i < new_points.size(); i++)
145 Eigen::Vector3i voxel_loc;
146 bool valid =
worldToGrid(new_points[i].
x(), new_points[i].
y(), new_points[i].
z(), voxel_loc.x(), voxel_loc.y(),
150 new_point_set.insert(voxel_loc);
156 std::set_difference(old_point_set.begin(), old_point_set.end(), new_point_set.begin(), new_point_set.end(),
157 std::inserter(old_not_new, old_not_new.end()), comp);
160 std::set_difference(new_point_set.begin(), new_point_set.end(), old_point_set.begin(), old_point_set.end(),
161 std::inserter(new_not_old, new_not_old.end()), comp);
164 for (
unsigned int i = 0; i < new_not_old.size(); i++)
166 if (
voxel_grid_->getCell(new_not_old[i].x(), new_not_old[i].y(), new_not_old[i].z()).distance_square_ != 0)
168 new_not_in_current.push_back(new_not_old[i]);
189 for (
unsigned int i = 0; i < points.size(); i++)
192 Eigen::Vector3i voxel_loc;
193 bool valid =
worldToGrid(points[i].
x(), points[i].
y(), points[i].
z(), voxel_loc.x(), voxel_loc.y(), voxel_loc.z());
197 if (
voxel_grid_->getCell(voxel_loc.x(), voxel_loc.y(), voxel_loc.z()).distance_square_ > 0)
199 voxel_points.push_back(voxel_loc);
211 for (
unsigned int i = 0; i < points.size(); i++)
214 Eigen::Vector3i voxel_loc;
215 bool valid =
worldToGrid(points[i].
x(), points[i].
y(), points[i].
z(), voxel_loc.x(), voxel_loc.y(), voxel_loc.z());
219 voxel_points.push_back(voxel_loc);
240 for (
unsigned int i = 0; i < voxel_points.size(); i++)
243 const Eigen::Vector3i& loc = voxel_points[i];
254 negative_stack.push_back(loc);
261 while (!negative_stack.empty())
263 Eigen::Vector3i loc = negative_stack.back();
264 negative_stack.pop_back();
266 for (
int neighbor = 0; neighbor < 27; neighbor++)
269 Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
275 if (!
isCellValid(close_point.x(), close_point.y(), close_point.z()))
280 voxel_grid_->getCell(close_point.x(), close_point.y(), close_point.z());
295 negative_stack.push_back(nloc);
334 for (
unsigned int i = 0; i < voxel_points.size(); i++)
340 stack.push_back(voxel_points[i]);
351 while (!stack.empty())
353 Eigen::Vector3i loc = stack.back();
356 for (
int neighbor = 0; neighbor < 27; neighbor++)
359 Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
365 if (!
isCellValid(close_point.x(), close_point.y(), close_point.z()))
370 voxel_grid_->getCell(close_point.x(), close_point.y(), close_point.z());
379 stack.push_back(nloc);
403 EigenSTL::vector_Vector3i::iterator list_it =
bucket_queue_[i].begin();
404 EigenSTL::vector_Vector3i::iterator list_end =
bucket_queue_[i].end();
405 for (; list_it != list_end; ++list_it)
407 const Eigen::Vector3i& loc = *list_it;
419 ROS_ERROR_NAMED(
"distance_field",
"PROGRAMMING ERROR: Invalid update direction detected: %d",
426 for (
unsigned int n = 0; n < neighborhood->size(); n++)
428 Eigen::Vector3i
diff = (*neighborhood)[n];
429 Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
440 if (new_distance_sq < neighbor->distance_square_)
463 for (; list_it != list_end; ++list_it)
465 const Eigen::Vector3i& loc = *list_it;
478 ROS_ERROR_NAMED(
"distance_field",
"PROGRAMMING ERROR: Invalid update direction detected: %d",
485 for (
unsigned int n = 0; n < neighborhood->size(); n++)
487 Eigen::Vector3i
diff = (*neighborhood)[n];
488 Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
500 if (new_distance_sq < neighbor->negative_distance_square_)
542 for (
int dx = -1; dx <= 1; ++dx)
544 for (
int dy = -1; dy <= 1; ++dy)
546 for (
int dz = -1; dz <= 1; ++dz)
549 Eigen::Vector3i n_point(dx, dy, dz);
556 for (
int n = 0; n < 2; n++)
560 for (
int dx = -1; dx <= 1; ++dx)
562 for (
int dy = -1; dy <= 1; ++dy)
564 for (
int dz = -1; dz <= 1; ++dz)
568 for (
int tdx = -1; tdx <= 1; ++tdx)
570 for (
int tdy = -1; tdy <= 1; ++tdy)
572 for (
int tdz = -1; tdz <= 1; ++tdz)
574 if (tdx == 0 && tdy == 0 && tdz == 0)
578 if ((
abs(tdx) +
abs(tdy) +
abs(tdz)) != 1)
580 if (dx * tdx < 0 || dy * tdy < 0 || dz * tdz < 0)
583 Eigen::Vector3i n_point(tdx, tdy, tdz);
598 return (dx + 1) * 9 + (dy + 1) * 3 + dz + 1;
638 voxel_grid_->gridToWorld(x, y, z, world_x, world_y, world_z);
644 return voxel_grid_->worldToGrid(world_x, world_y, world_z, x, y, z);
650 os <<
"size_x: " <<
size_x_ << std::endl;
651 os <<
"size_y: " <<
size_y_ << std::endl;
652 os <<
"size_z: " <<
size_z_ << std::endl;
653 os <<
"origin_x: " <<
origin_x_ << std::endl;
654 os <<
"origin_y: " <<
origin_y_ << std::endl;
655 os <<
"origin_z: " <<
origin_z_ << std::endl;
659 boost::iostreams::filtering_ostream out;
660 out.push(boost::iostreams::zlib_compressor());
663 for (
unsigned int x = 0; x < static_cast<unsigned int>(
getXNumCells());
x++)
665 for (
unsigned int y = 0; y < static_cast<unsigned int>(
getYNumCells());
y++)
667 for (
unsigned int z = 0; z < static_cast<unsigned int>(
getZNumCells());
z += 8)
669 std::bitset<8> bs(0);
670 unsigned int zv = std::min((
unsigned int)8,
getZNumCells() - z);
671 for (
unsigned int zi = 0; zi < zv; zi++)
673 if (
getCell(
x,
y, z + zi).distance_square_ == 0)
679 out.write((
char*)&bs,
sizeof(
char));
695 if (temp !=
"resolution:")
700 if (temp !=
"size_x:")
705 if (temp !=
"size_y:")
710 if (temp !=
"size_z:")
715 if (temp !=
"origin_x:")
720 if (temp !=
"origin_y:")
725 if (temp !=
"origin_z:")
738 boost::iostreams::filtering_istream in;
739 in.push(boost::iostreams::zlib_decompressor());
745 for (
unsigned int x = 0; x < static_cast<unsigned int>(
getXNumCells());
x++)
747 for (
unsigned int y = 0; y < static_cast<unsigned int>(
getYNumCells());
y++)
749 for (
unsigned int z = 0; z < static_cast<unsigned int>(
getZNumCells());
z += 8)
757 std::bitset<8> inbit((
unsigned long long)inchar);
758 unsigned int zv = std::min((
unsigned int)8,
getZNumCells() - z);
759 for (
unsigned int zi = 0; zi < zv; zi++)
764 obs_points.push_back(Eigen::Vector3i(
x,
y, z + zi));
virtual void reset()
Resets the entire distance field to max_distance for positive values and zero for negative values...
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)
virtual int getXNumCells() const
Gets the number of cells along the X axis.
bool propagate_negative_
Whether or not to propagate negative distances.
virtual void updatePointsInField(const EigenSTL::vector_Vector3d &old_points, const EigenSTL::vector_Vector3d &new_points)
This function will remove any obstacle points that are in the old point set but not the new point set...
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
int distance_square_
Distance in cells to the closest obstacle, squared.
std::vector< EigenSTL::vector_Vector3i > bucket_queue_
Structure used to hold propagation frontier.
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.
virtual bool writeToStream(std::ostream &stream) const
Writes the contents of the distance field to the supplied stream.
Structure that holds voxel information for the DistanceField. Will be used in VoxelGrid.
double size_x_
X size of the distance field.
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
static const double origin_y
double getResolution() const
Gets the resolution of the distance field in meters.
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...
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...
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.
void print(const VoxelSet &set)
Debug function that prints all voxels in a set to ROS_DEBUG_NAMED.
double origin_y_
Y origin of the distance field.
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, and dz values.
#define ROS_DEBUG_NAMED(name,...)
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...
void propagateNegative()
Propagates inward to a maximum distance given the contents of the negative_bucket_queue_, and clears the negative_bucket_queue_.
virtual bool readFromStream(std::istream &stream)
Reads, parameterizes, and populates the distance field based on the supplied stream.
virtual bool worldToGrid(double world_x, double world_y, double world_z, int &x, int &y, int &z) const
Converts from a world location to a set of integer indices. Should return false if the world location...
double size_z_
Z size of the distance field.
static const double resolution
virtual int getYNumCells() const
Gets the number of cells along the Y axis.
static const double origin_z
VoxelGrid< PropDistanceFieldVoxel >::Ptr voxel_grid_
Actual container for distance data.
double origin_z_
Z origin of the distance field.
std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > vector_Vector3i
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
void propagatePositive()
Propagates outward to the maximum distance given the contents of the bucket_queue_, and clears the bucket_queue_.
virtual void removePointsFromField(const EigenSTL::vector_Vector3d &points)
Remove a set of obstacle points from the distance field, updating distance values accordingly...
Namespace for holding classes that generate distance fields.
double origin_x_
X origin of the distance field.
static const Eigen::Vector3d point1(0.1, 0.0, 0.0)
double max_distance_
Holds maximum distance.
virtual int getZNumCells() const
Gets the number of cells along the Z axis.
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.
virtual bool isCellValid(int x, int y, int z) const
Determines whether or not the cell associated with the supplied indices is valid for this distance fi...
Eigen::Vector3i getLocationDifference(int directionNumber) const
Helper function that gets change values given single number representing update direction.
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
EigenSTL::vector_Vector3i direction_number_to_direction_
Holds conversion from direction number to integer changes.
void initialize()
Initializes the field, resetting the voxel grid and building a sqrt lookup table for efficiency based...
int negative_distance_square_
Distance in cells to the nearest unoccupied cell, squared.
virtual void addPointsToField(const EigenSTL::vector_Vector3d &points)
Add a set of obstacle points to the distance field, updating distance values accordingly. The distance field may already contain obstacle cells.
#define ROS_ERROR_NAMED(name,...)
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
void removeObstacleVoxels(const EigenSTL::vector_Vector3i &voxel_points)
Removes a valid set of integer points from the voxel grid.
void addOcTreeToField(const octomap::OcTree *octree)
Adds an octree to the distance field. Cells that are occupied in the octree that lie within the voxel...
static int eucDistSq(Eigen::Vector3i point1, Eigen::Vector3i point2)
Computes squared distance between two 3D integer points.
int negative_update_direction_
Direction from which this voxel was updated for negative distance propagation.
double resolution_
Resolution of the distance field.
double size_y_
Y size of the distance field.
virtual bool gridToWorld(int x, int y, int z, double &world_x, double &world_y, double &world_z) const
Converts from an set of integer indices to a world location given the origin and resolution parameter...
int max_distance_sq_
Holds maximum distance squared in cells.
void addNewObstacleVoxels(const EigenSTL::vector_Vector3i &voxel_points)
Adds a valid set of integer points to the voxel grid.
static const double origin_x
void initNeighborhoods()
Helper function for computing location and neighborhood information in 27 connected voxel grid...