38 #include <visualization_msgs/Marker.h> 39 #include <console_bridge/console.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(),
69 bool propagate_negative_distances)
96 int dx = point1.x() - point2.x();
97 int dy = point1.y() - point2.y();
98 int dz = point1.z() - point2.z();
99 return dx * dx + dy * dy + dz * dz;
105 VoxelSet::const_iterator it;
106 for (it =
set.begin(); it !=
set.end(); ++it)
108 Eigen::Vector3i loc1 = *it;
117 EigenSTL::vector_Vector3d::const_iterator it;
118 for (it = points.begin(); it != points.end(); ++it)
120 Eigen::Vector3d loc1 = *it;
130 for (
unsigned int i = 0; i < old_points.size(); i++)
132 Eigen::Vector3i voxel_loc;
133 bool valid =
worldToGrid(old_points[i].
x(), old_points[i].
y(), old_points[i].
z(), voxel_loc.x(), voxel_loc.y(),
137 old_point_set.insert(voxel_loc);
142 for (
unsigned int i = 0; i < new_points.size(); i++)
144 Eigen::Vector3i voxel_loc;
145 bool valid =
worldToGrid(new_points[i].
x(), new_points[i].
y(), new_points[i].
z(), voxel_loc.x(), voxel_loc.y(),
149 new_point_set.insert(voxel_loc);
154 std::vector<Eigen::Vector3i> old_not_new;
155 std::set_difference(old_point_set.begin(), old_point_set.end(), new_point_set.begin(), new_point_set.end(),
156 std::inserter(old_not_new, old_not_new.end()), comp);
158 std::vector<Eigen::Vector3i> new_not_old;
159 std::set_difference(new_point_set.begin(), new_point_set.end(), old_point_set.begin(), old_point_set.end(),
160 std::inserter(new_not_old, new_not_old.end()), comp);
162 std::vector<Eigen::Vector3i> new_not_in_current;
163 for (
unsigned int i = 0; i < new_not_old.size(); i++)
165 if (
voxel_grid_->getCell(new_not_old[i].x(), new_not_old[i].y(), new_not_old[i].z()).distance_square_ != 0)
167 new_not_in_current.push_back(new_not_old[i]);
186 std::vector<Eigen::Vector3i> voxel_points;
188 for (
unsigned int i = 0; i < points.size(); i++)
191 Eigen::Vector3i voxel_loc;
192 bool valid =
worldToGrid(points[i].
x(), points[i].
y(), points[i].
z(), voxel_loc.x(), voxel_loc.y(), voxel_loc.z());
196 if (
voxel_grid_->getCell(voxel_loc.x(), voxel_loc.y(), voxel_loc.z()).distance_square_ > 0)
198 voxel_points.push_back(voxel_loc);
207 std::vector<Eigen::Vector3i> voxel_points;
210 for (
unsigned int i = 0; i < points.size(); i++)
213 Eigen::Vector3i voxel_loc;
214 bool valid =
worldToGrid(points[i].
x(), points[i].
y(), points[i].
z(), voxel_loc.x(), voxel_loc.y(), voxel_loc.z());
218 voxel_points.push_back(voxel_loc);
232 std::vector<Eigen::Vector3i> negative_stack;
239 for (
unsigned int i = 0; i < voxel_points.size(); i++)
242 const Eigen::Vector3i& loc = voxel_points[i];
253 negative_stack.push_back(loc);
260 while (!negative_stack.empty())
262 Eigen::Vector3i loc = negative_stack.back();
263 negative_stack.pop_back();
265 for (
int neighbor = 0; neighbor < 27; neighbor++)
268 Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
274 if (!
isCellValid(close_point.x(), close_point.y(), close_point.z()))
279 voxel_grid_->getCell(close_point.x(), close_point.y(), close_point.z());
294 negative_stack.push_back(nloc);
313 std::vector<Eigen::Vector3i> stack;
314 std::vector<Eigen::Vector3i> negative_stack;
333 for (
unsigned int i = 0; i < voxel_points.size(); i++)
339 stack.push_back(voxel_points[i]);
350 while (!stack.empty())
352 Eigen::Vector3i loc = stack.back();
355 for (
int neighbor = 0; neighbor < 27; neighbor++)
358 Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
364 if (!
isCellValid(close_point.x(), close_point.y(), close_point.z()))
369 voxel_grid_->getCell(close_point.x(), close_point.y(), close_point.z());
378 stack.push_back(nloc);
402 std::vector<Eigen::Vector3i>::iterator list_it =
bucket_queue_[i].begin();
403 std::vector<Eigen::Vector3i>::iterator list_end =
bucket_queue_[i].end();
404 for (; list_it != list_end; ++list_it)
406 const Eigen::Vector3i& loc = *list_it;
410 std::vector<Eigen::Vector3i>* neighborhood;
424 for (
unsigned int n = 0; n < neighborhood->size(); n++)
426 Eigen::Vector3i
diff = (*neighborhood)[n];
427 Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
438 if (new_distance_sq < neighbor->distance_square_)
461 for (; list_it != list_end; ++list_it)
463 const Eigen::Vector3i& loc = *list_it;
467 std::vector<Eigen::Vector3i>* neighborhood;
482 for (
unsigned int n = 0; n < neighborhood->size(); n++)
484 Eigen::Vector3i
diff = (*neighborhood)[n];
485 Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
497 if (new_distance_sq < neighbor->negative_distance_square_)
539 for (
int dx = -1; dx <= 1; ++dx)
541 for (
int dy = -1; dy <= 1; ++dy)
543 for (
int dz = -1; dz <= 1; ++dz)
546 Eigen::Vector3i n_point(dx, dy, dz);
553 for (
int n = 0; n < 2; n++)
557 for (
int dx = -1; dx <= 1; ++dx)
559 for (
int dy = -1; dy <= 1; ++dy)
561 for (
int dz = -1; dz <= 1; ++dz)
565 for (
int tdx = -1; tdx <= 1; ++tdx)
567 for (
int tdy = -1; tdy <= 1; ++tdy)
569 for (
int tdz = -1; tdz <= 1; ++tdz)
571 if (tdx == 0 && tdy == 0 && tdz == 0)
575 if ((
abs(tdx) +
abs(tdy) +
abs(tdz)) != 1)
577 if (dx * tdx < 0 || dy * tdy < 0 || dz * tdz < 0)
580 Eigen::Vector3i n_point(tdx, tdy, tdz);
595 return (dx + 1) * 9 + (dy + 1) * 3 + dz + 1;
635 voxel_grid_->gridToWorld(x, y, z, world_x, world_y, world_z);
641 return voxel_grid_->worldToGrid(world_x, world_y, world_z, x, y, z);
647 os <<
"size_x: " <<
size_x_ << std::endl;
648 os <<
"size_y: " <<
size_y_ << std::endl;
649 os <<
"size_z: " <<
size_z_ << std::endl;
650 os <<
"origin_x: " <<
origin_x_ << std::endl;
651 os <<
"origin_y: " <<
origin_y_ << std::endl;
652 os <<
"origin_z: " <<
origin_z_ << std::endl;
656 boost::iostreams::filtering_ostream out;
657 out.push(boost::iostreams::zlib_compressor());
660 for (
unsigned int x = 0; x < static_cast<unsigned int>(
getXNumCells());
x++)
662 for (
unsigned int y = 0; y < static_cast<unsigned int>(
getYNumCells());
y++)
664 for (
unsigned int z = 0; z < static_cast<unsigned int>(
getZNumCells());
z += 8)
666 std::bitset<8> bs(0);
667 unsigned int zv = std::min((
unsigned int)8,
getZNumCells() - z);
668 for (
unsigned int zi = 0; zi < zv; zi++)
670 if (
getCell(
x,
y, z + zi).distance_square_ == 0)
676 out.write((
char*)&bs,
sizeof(
char));
692 if (temp !=
"resolution:")
697 if (temp !=
"size_x:")
702 if (temp !=
"size_y:")
707 if (temp !=
"size_z:")
712 if (temp !=
"origin_x:")
717 if (temp !=
"origin_y:")
722 if (temp !=
"origin_z:")
735 boost::iostreams::filtering_istream in;
736 in.push(boost::iostreams::zlib_decompressor());
741 std::vector<Eigen::Vector3i> obs_points;
742 for (
unsigned int x = 0; x < static_cast<unsigned int>(
getXNumCells());
x++)
744 for (
unsigned int y = 0; y < static_cast<unsigned int>(
getYNumCells());
y++)
746 for (
unsigned int z = 0; z < static_cast<unsigned int>(
getZNumCells());
z += 8)
754 std::bitset<8> inbit((
unsigned long long)inchar);
755 unsigned int zv = std::min((
unsigned int)8,
getZNumCells() - z);
756 for (
unsigned int zi = 0; zi < zv; zi++)
761 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::set< Eigen::Vector3i, compareEigen_Vector3i > VoxelSet
Typedef for set of integer indices.
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
int distance_square_
Distance in cells to the closest obstacle, squared.
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.
std::vector< std::vector< Eigen::Vector3i > > negative_bucket_queue_
Data member that holds points from which to propagate in the negative, where each vector holds points...
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...
#define CONSOLE_BRIDGE_logDebug(fmt,...)
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 CONSOLE_BRIDGE_logDebug.
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.
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.
std::vector< std::vector< Eigen::Vector3i > > bucket_queue_
Structure used to hold propagation frontier.
double origin_z_
Z origin of the distance field.
void addNewObstacleVoxels(const std::vector< Eigen::Vector3i > &voxel_points)
Adds a valid set of integer points to the voxel grid.
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.
#define CONSOLE_BRIDGE_logError(fmt,...)
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)
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.
std::vector< double > sqrt_table_
Precomputed square root table for faster distance lookups.
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...
std::vector< Eigen::Vector3i > direction_number_to_direction_
Holds conversion from direction number to integer changes.
std::vector< std::vector< std::vector< Eigen::Vector3i > > > neighborhoods_
Holds information on neighbor direction, with 27 different directions. Shows where to propagate given...
void removeObstacleVoxels(const std::vector< Eigen::Vector3i > &voxel_points)
Removes a valid set of integer points from the voxel grid.
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.
static const double origin_x
void initNeighborhoods()
Helper function for computing location and neighborhood information in 27 connected voxel grid...