propagation_distance_field.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Mrinal Kalakrishnan, Ken Anderson */
36 
38 #include <visualization_msgs/Marker.h>
39 #include <ros/console.h>
40 #include <boost/iostreams/filtering_stream.hpp>
41 #include <boost/iostreams/copy.hpp>
42 #include <boost/iostreams/filter/zlib.hpp>
43 
44 namespace distance_field
45 {
46 PropagationDistanceField::PropagationDistanceField(double size_x, double size_y, double size_z, double resolution,
47  double origin_x, double origin_y, double origin_z,
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)
52 {
53  initialize();
54 }
55 
56 PropagationDistanceField::PropagationDistanceField(const octomap::OcTree& octree, const octomap::point3d& bbx_min,
57  const octomap::point3d& bbx_max, double 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(),
60  octree.getResolution(), bbx_min.x(), bbx_min.y(), bbx_min.z())
61  , propagate_negative_(propagate_negative_distances)
62  , max_distance_(max_distance)
63  , max_distance_sq_(0) // avoid gcc warning about uninitialized value
64 {
65  initialize();
66  addOcTreeToField(&octree);
67 }
68 
69 PropagationDistanceField::PropagationDistanceField(std::istream& is, double max_distance,
70  bool propagate_negative_distances)
71  : DistanceField(0, 0, 0, 0, 0, 0, 0), propagate_negative_(propagate_negative_distances), max_distance_(max_distance)
72 {
73  readFromStream(is);
74 }
75 
76 void PropagationDistanceField::initialize()
77 {
78  max_distance_sq_ = ceil(max_distance_ / resolution_) * ceil(max_distance_ / resolution_);
79  voxel_grid_ =
80  std::make_shared<VoxelGrid<PropDistanceFieldVoxel>>(size_x_, size_y_, size_z_, resolution_, origin_x_, origin_y_,
81  origin_z_, PropDistanceFieldVoxel(max_distance_sq_, 0));
82 
83  initNeighborhoods();
84 
85  bucket_queue_.resize(max_distance_sq_ + 1);
86  negative_bucket_queue_.resize(max_distance_sq_ + 1);
87 
88  // create a sqrt table:
89  sqrt_table_.resize(max_distance_sq_ + 1);
90  for (int i = 0; i <= max_distance_sq_; ++i)
91  sqrt_table_[i] = sqrt(double(i)) * resolution_;
92 
93  reset();
94 }
95 
96 void PropagationDistanceField::print(const VoxelSet& set)
97 {
98  ROS_DEBUG_NAMED("distance_field", "[");
99  VoxelSet::const_iterator it;
100  for (it = set.begin(); it != set.end(); ++it)
101  {
102  Eigen::Vector3i loc1 = *it;
103  ROS_DEBUG_NAMED("distance_field", "%d, %d, %d ", loc1.x(), loc1.y(), loc1.z());
104  }
105  ROS_DEBUG_NAMED("distance_field", "] size=%u\n", (unsigned int)set.size());
106 }
107 
108 void PropagationDistanceField::print(const EigenSTL::vector_Vector3d& points)
109 {
110  ROS_DEBUG_NAMED("distance_field", "[");
111  EigenSTL::vector_Vector3d::const_iterator it;
112  for (it = points.begin(); it != points.end(); ++it)
113  {
114  Eigen::Vector3d loc1 = *it;
115  ROS_DEBUG_NAMED("distance_field", "%g, %g, %g ", loc1.x(), loc1.y(), loc1.z());
116  }
117  ROS_DEBUG_NAMED("distance_field", "] size=%u\n", (unsigned int)points.size());
118 }
119 
120 void PropagationDistanceField::updatePointsInField(const EigenSTL::vector_Vector3d& old_points,
121  const EigenSTL::vector_Vector3d& new_points)
122 {
123  VoxelSet old_point_set;
124  for (const Eigen::Vector3d& old_point : old_points)
125  {
126  Eigen::Vector3i voxel_loc;
127  bool valid = worldToGrid(old_point.x(), old_point.y(), old_point.z(), voxel_loc.x(), voxel_loc.y(), voxel_loc.z());
128  if (valid)
129  {
130  old_point_set.insert(voxel_loc);
131  }
132  }
133 
134  VoxelSet new_point_set;
135  for (const Eigen::Vector3d& new_point : new_points)
136  {
137  Eigen::Vector3i voxel_loc;
138  bool valid = worldToGrid(new_point.x(), new_point.y(), new_point.z(), voxel_loc.x(), voxel_loc.y(), voxel_loc.z());
139  if (valid)
140  {
141  new_point_set.insert(voxel_loc);
142  }
143  }
144  CompareEigenVector3i comp;
145 
146  EigenSTL::vector_Vector3i old_not_new;
147  std::set_difference(old_point_set.begin(), old_point_set.end(), new_point_set.begin(), new_point_set.end(),
148  std::inserter(old_not_new, old_not_new.end()), comp);
149 
150  EigenSTL::vector_Vector3i new_not_old;
151  std::set_difference(new_point_set.begin(), new_point_set.end(), old_point_set.begin(), old_point_set.end(),
152  std::inserter(new_not_old, new_not_old.end()), comp);
153 
154  EigenSTL::vector_Vector3i new_not_in_current;
155  for (Eigen::Vector3i& voxel_loc : new_not_old)
156  {
157  if (voxel_grid_->getCell(voxel_loc.x(), voxel_loc.y(), voxel_loc.z()).distance_square_ != 0)
158  {
159  new_not_in_current.push_back(voxel_loc);
160  }
161  // ROS_INFO_NAMED("distance_field", "Adding obstacle voxel %d %d %d", (*it).x(), (*it).y(), (*it).z());
162  }
163 
164  removeObstacleVoxels(old_not_new);
165  addNewObstacleVoxels(new_not_in_current);
166 
167  // ROS_DEBUG_NAMED("distance_field", "new=" );
168  // print(points_added);
169  // ROS_DEBUG_NAMED("distance_field", "removed=" );
170  // print(points_removed);
171  // ROS_DEBUG_NAMED("distance_field", "obstacle_voxel_locations_=" );
172  // print(object_voxel_locations_);
173  // ROS_DEBUG_NAMED("distance_field", "");
174 }
175 
176 void PropagationDistanceField::addPointsToField(const EigenSTL::vector_Vector3d& points)
177 {
178  EigenSTL::vector_Vector3i voxel_points;
179 
180  for (const Eigen::Vector3d& point : points)
181  {
182  // Convert to voxel coordinates
183  Eigen::Vector3i voxel_loc;
184  bool valid = worldToGrid(point.x(), point.y(), point.z(), voxel_loc.x(), voxel_loc.y(), voxel_loc.z());
185 
186  if (valid)
187  {
188  if (voxel_grid_->getCell(voxel_loc.x(), voxel_loc.y(), voxel_loc.z()).distance_square_ > 0)
189  {
190  voxel_points.push_back(voxel_loc);
191  }
192  }
193  }
194  addNewObstacleVoxels(voxel_points);
195 }
196 
197 void PropagationDistanceField::removePointsFromField(const EigenSTL::vector_Vector3d& points)
198 {
199  EigenSTL::vector_Vector3i voxel_points;
200  // VoxelSet voxel_locs;
201 
202  for (const Eigen::Vector3d& point : points)
203  {
204  // Convert to voxel coordinates
205  Eigen::Vector3i voxel_loc;
206  bool valid = worldToGrid(point.x(), point.y(), point.z(), voxel_loc.x(), voxel_loc.y(), voxel_loc.z());
207 
208  if (valid)
209  {
210  voxel_points.push_back(voxel_loc);
211  // if(voxel_grid_->getCell(voxel_loc.x(),voxel_loc.y(),voxel_loc.z()).distance_square_ == 0) {
212  // voxel_locs.insert(voxel_loc);
213  //}
214  }
215  }
216 
217  removeObstacleVoxels(voxel_points);
218 }
219 
220 void PropagationDistanceField::addNewObstacleVoxels(const EigenSTL::vector_Vector3i& voxel_points)
221 {
222  int initial_update_direction = getDirectionNumber(0, 0, 0);
223  bucket_queue_[0].reserve(voxel_points.size());
224  EigenSTL::vector_Vector3i negative_stack;
225  if (propagate_negative_)
226  {
227  negative_stack.reserve(getXNumCells() * getYNumCells() * getZNumCells());
228  negative_bucket_queue_[0].reserve(voxel_points.size());
229  }
230 
231  for (const Eigen::Vector3i& voxel_point : voxel_points)
232  {
233  PropDistanceFieldVoxel& voxel = voxel_grid_->getCell(voxel_point.x(), voxel_point.y(), voxel_point.z());
234  const Eigen::Vector3i& loc = voxel_point;
235  voxel.distance_square_ = 0;
236  voxel.closest_point_ = loc;
237  voxel.update_direction_ = initial_update_direction;
238  bucket_queue_[0].push_back(loc);
239  if (propagate_negative_)
240  {
241  voxel.negative_distance_square_ = max_distance_sq_;
242  voxel.closest_negative_point_.x() = PropDistanceFieldVoxel::UNINITIALIZED;
243  voxel.closest_negative_point_.y() = PropDistanceFieldVoxel::UNINITIALIZED;
244  voxel.closest_negative_point_.z() = PropDistanceFieldVoxel::UNINITIALIZED;
245  negative_stack.push_back(loc);
246  }
247  }
248  propagatePositive();
249 
250  if (propagate_negative_)
251  {
252  while (!negative_stack.empty())
253  {
254  Eigen::Vector3i loc = negative_stack.back();
255  negative_stack.pop_back();
256 
257  for (int neighbor = 0; neighbor < 27; neighbor++)
258  {
259  Eigen::Vector3i diff = getLocationDifference(neighbor);
260  Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
261 
262  if (isCellValid(nloc.x(), nloc.y(), nloc.z()))
263  {
264  PropDistanceFieldVoxel& nvoxel = voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z());
265  Eigen::Vector3i& close_point = nvoxel.closest_negative_point_;
266  if (!isCellValid(close_point.x(), close_point.y(), close_point.z()))
267  {
268  close_point = nloc;
269  }
270  PropDistanceFieldVoxel& closest_point_voxel =
271  voxel_grid_->getCell(close_point.x(), close_point.y(), close_point.z());
272 
273  // our closest non-obstacle cell has become an obstacle
274  if (closest_point_voxel.negative_distance_square_ != 0)
275  {
276  // find all neigbors inside pre-existing obstacles whose
277  // closest_negative_point_ is now an obstacle. These must all be
278  // set to max_distance_sq_ so they will be re-propogated with a new
279  // closest_negative_point_ that is outside the obstacle.
280  if (nvoxel.negative_distance_square_ != max_distance_sq_)
281  {
282  nvoxel.negative_distance_square_ = max_distance_sq_;
283  nvoxel.closest_negative_point_.x() = PropDistanceFieldVoxel::UNINITIALIZED;
284  nvoxel.closest_negative_point_.y() = PropDistanceFieldVoxel::UNINITIALIZED;
285  nvoxel.closest_negative_point_.z() = PropDistanceFieldVoxel::UNINITIALIZED;
286  negative_stack.push_back(nloc);
287  }
288  }
289  else
290  {
291  // this cell still has a valid non-obstacle cell, so we need to propogate from it
292  nvoxel.negative_update_direction_ = initial_update_direction;
293  negative_bucket_queue_[0].push_back(nloc);
294  }
295  }
296  }
297  }
298  propagateNegative();
299  }
300 }
301 
302 void PropagationDistanceField::removeObstacleVoxels(const EigenSTL::vector_Vector3i& voxel_points)
303 // const VoxelSet& locations )
304 {
306  EigenSTL::vector_Vector3i negative_stack;
307  int initial_update_direction = getDirectionNumber(0, 0, 0);
308 
309  stack.reserve(getXNumCells() * getYNumCells() * getZNumCells());
310  bucket_queue_[0].reserve(voxel_points.size());
311  if (propagate_negative_)
312  {
313  negative_stack.reserve(getXNumCells() * getYNumCells() * getZNumCells());
314  negative_bucket_queue_[0].reserve(voxel_points.size());
315  }
316 
317  // First reset the obstacle voxels,
318  // VoxelSet::const_iterator it = locations.begin();
319  // for( it=locations.begin(); it!=locations.end(); ++it)
320  // {
321  // Eigen::Vector3i loc = *it;
322  // bool valid = isCellValid( loc.x(), loc.y(), loc.z());
323  // if (!valid)
324  // continue;
325  for (const Eigen::Vector3i& voxel_point : voxel_points)
326  {
327  PropDistanceFieldVoxel& voxel = voxel_grid_->getCell(voxel_point.x(), voxel_point.y(), voxel_point.z());
328  voxel.distance_square_ = max_distance_sq_;
329  voxel.closest_point_ = voxel_point;
330  voxel.update_direction_ = initial_update_direction; // not needed?
331  stack.push_back(voxel_point);
332  if (propagate_negative_)
333  {
334  voxel.negative_distance_square_ = 0.0;
335  voxel.closest_negative_point_ = voxel_point;
336  voxel.negative_update_direction_ = initial_update_direction;
337  negative_bucket_queue_[0].push_back(voxel_point);
338  }
339  }
340 
341  // Reset all neighbors who's closest point is now gone.
342  while (!stack.empty())
343  {
344  Eigen::Vector3i loc = stack.back();
345  stack.pop_back();
346 
347  for (int neighbor = 0; neighbor < 27; neighbor++)
348  {
349  Eigen::Vector3i diff = getLocationDifference(neighbor);
350  Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
351 
352  if (isCellValid(nloc.x(), nloc.y(), nloc.z()))
353  {
354  PropDistanceFieldVoxel& nvoxel = voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z());
355  Eigen::Vector3i& close_point = nvoxel.closest_point_;
356  if (!isCellValid(close_point.x(), close_point.y(), close_point.z()))
357  {
358  close_point = nloc;
359  }
360  PropDistanceFieldVoxel& closest_point_voxel =
361  voxel_grid_->getCell(close_point.x(), close_point.y(), close_point.z());
362 
363  if (closest_point_voxel.distance_square_ != 0)
364  { // closest point no longer exists
365  if (nvoxel.distance_square_ != max_distance_sq_)
366  {
367  nvoxel.distance_square_ = max_distance_sq_;
368  nvoxel.closest_point_ = nloc;
369  nvoxel.update_direction_ = initial_update_direction; // not needed?
370  stack.push_back(nloc);
371  }
372  }
373  else
374  { // add to queue so we can propagate the values
375  nvoxel.update_direction_ = initial_update_direction;
376  bucket_queue_[0].push_back(nloc);
377  }
378  }
379  }
380  }
381  propagatePositive();
382 
383  if (propagate_negative_)
384  {
385  propagateNegative();
386  }
387 }
388 
389 void PropagationDistanceField::propagatePositive()
390 {
391  // now process the queue:
392  for (unsigned int i = 0; i < bucket_queue_.size(); ++i)
393  {
394  EigenSTL::vector_Vector3i::iterator list_it = bucket_queue_[i].begin();
395  EigenSTL::vector_Vector3i::iterator list_end = bucket_queue_[i].end();
396  for (; list_it != list_end; ++list_it)
397  {
398  const Eigen::Vector3i& loc = *list_it;
399  PropDistanceFieldVoxel* vptr = &voxel_grid_->getCell(loc.x(), loc.y(), loc.z());
400 
401  // select the neighborhood list based on the update direction:
402  EigenSTL::vector_Vector3i* neighborhood;
403  int d = i;
404  if (d > 1)
405  d = 1;
406 
407  // This will never happen. update_direction_ is always set before voxel is added to bucket queue.
408  if (vptr->update_direction_ < 0 || vptr->update_direction_ > 26)
409  {
410  ROS_ERROR_NAMED("distance_field", "PROGRAMMING ERROR: Invalid update direction detected: %d",
411  vptr->update_direction_);
412  continue;
413  }
414 
415  neighborhood = &neighborhoods_[d][vptr->update_direction_];
416 
417  for (const Eigen::Vector3i& diff : *neighborhood)
418  {
419  Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
420  if (!isCellValid(nloc.x(), nloc.y(), nloc.z()))
421  continue;
422 
423  // the real update code:
424  // calculate the neighbor's new distance based on my closest filled voxel:
425  PropDistanceFieldVoxel* neighbor = &voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z());
426  int new_distance_sq = (vptr->closest_point_ - nloc).squaredNorm();
427  if (new_distance_sq > max_distance_sq_)
428  continue;
429 
430  if (new_distance_sq < neighbor->distance_square_)
431  {
432  // update the neighboring voxel
433  neighbor->distance_square_ = new_distance_sq;
434  neighbor->closest_point_ = vptr->closest_point_;
435  neighbor->update_direction_ = getDirectionNumber(diff.x(), diff.y(), diff.z());
436 
437  // and put it in the queue:
438  bucket_queue_[new_distance_sq].push_back(nloc);
439  }
440  }
441  }
442  bucket_queue_[i].clear();
443  }
444 }
445 
446 void PropagationDistanceField::propagateNegative()
447 {
448  // now process the queue:
449  for (unsigned int i = 0; i < negative_bucket_queue_.size(); ++i)
450  {
451  EigenSTL::vector_Vector3i::iterator list_it = negative_bucket_queue_[i].begin();
452  EigenSTL::vector_Vector3i::iterator list_end = negative_bucket_queue_[i].end();
453  for (; list_it != list_end; ++list_it)
454  {
455  const Eigen::Vector3i& loc = *list_it;
456  PropDistanceFieldVoxel* vptr = &voxel_grid_->getCell(loc.x(), loc.y(), loc.z());
457 
458  // select the neighborhood list based on the update direction:
459  EigenSTL::vector_Vector3i* neighborhood;
460  int d = i;
461  if (d > 1)
462  d = 1;
463 
464  // This will never happen. negative_update_direction_ is always set before voxel is added to
465  // negative_bucket_queue_.
466  if (vptr->negative_update_direction_ < 0 || vptr->negative_update_direction_ > 26)
467  {
468  ROS_ERROR_NAMED("distance_field", "PROGRAMMING ERROR: Invalid update direction detected: %d",
469  vptr->update_direction_);
470  continue;
471  }
472 
473  neighborhood = &neighborhoods_[d][vptr->negative_update_direction_];
474 
475  for (const Eigen::Vector3i& diff : *neighborhood)
476  {
477  Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
478  if (!isCellValid(nloc.x(), nloc.y(), nloc.z()))
479  continue;
480 
481  // the real update code:
482  // calculate the neighbor's new distance based on my closest filled voxel:
483  PropDistanceFieldVoxel* neighbor = &voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z());
484  int new_distance_sq = (vptr->closest_negative_point_ - nloc).squaredNorm();
485  if (new_distance_sq > max_distance_sq_)
486  continue;
487  // std::cout << "Looking at " << nloc.x() << " " << nloc.y() << " " << nloc.z() << " " << new_distance_sq << " "
488  // << neighbor->negative_distance_square_ << std::endl;
489  if (new_distance_sq < neighbor->negative_distance_square_)
490  {
491  // std::cout << "Updating " << nloc.x() << " " << nloc.y() << " " << nloc.z() << " " << new_distance_sq <<
492  // std::endl;
493  // update the neighboring voxel
494  neighbor->negative_distance_square_ = new_distance_sq;
495  neighbor->closest_negative_point_ = vptr->closest_negative_point_;
496  neighbor->negative_update_direction_ = getDirectionNumber(diff.x(), diff.y(), diff.z());
497 
498  // and put it in the queue:
499  negative_bucket_queue_[new_distance_sq].push_back(nloc);
500  }
501  }
502  }
503  negative_bucket_queue_[i].clear();
504  }
505 }
506 
507 void PropagationDistanceField::reset()
508 {
509  voxel_grid_->reset(PropDistanceFieldVoxel(max_distance_sq_, 0));
510  for (int x = 0; x < getXNumCells(); x++)
511  {
512  for (int y = 0; y < getYNumCells(); y++)
513  {
514  for (int z = 0; z < getZNumCells(); z++)
515  {
516  PropDistanceFieldVoxel& voxel = voxel_grid_->getCell(x, y, z);
517  voxel.closest_negative_point_.x() = x;
518  voxel.closest_negative_point_.y() = y;
519  voxel.closest_negative_point_.z() = z;
520  voxel.negative_distance_square_ = 0;
521  }
522  }
523  }
524  // object_voxel_locations_.clear();
525 }
526 
527 void PropagationDistanceField::initNeighborhoods()
528 {
529  // first initialize the direction number mapping:
530  direction_number_to_direction_.resize(27);
531  for (int dx = -1; dx <= 1; ++dx)
532  {
533  for (int dy = -1; dy <= 1; ++dy)
534  {
535  for (int dz = -1; dz <= 1; ++dz)
536  {
537  int direction_number = getDirectionNumber(dx, dy, dz);
538  Eigen::Vector3i n_point(dx, dy, dz);
539  direction_number_to_direction_[direction_number] = n_point;
540  }
541  }
542  }
543 
544  neighborhoods_.resize(2);
545  for (int n = 0; n < 2; n++)
546  {
547  neighborhoods_[n].resize(27);
548  // source directions
549  for (int dx = -1; dx <= 1; ++dx)
550  {
551  for (int dy = -1; dy <= 1; ++dy)
552  {
553  for (int dz = -1; dz <= 1; ++dz)
554  {
555  int direction_number = getDirectionNumber(dx, dy, dz);
556  // target directions:
557  for (int tdx = -1; tdx <= 1; ++tdx)
558  {
559  for (int tdy = -1; tdy <= 1; ++tdy)
560  {
561  for (int tdz = -1; tdz <= 1; ++tdz)
562  {
563  if (tdx == 0 && tdy == 0 && tdz == 0)
564  continue;
565  if (n >= 1)
566  {
567  if ((abs(tdx) + abs(tdy) + abs(tdz)) != 1)
568  continue;
569  if (dx * tdx < 0 || dy * tdy < 0 || dz * tdz < 0)
570  continue;
571  }
572  Eigen::Vector3i n_point(tdx, tdy, tdz);
573  neighborhoods_[n][direction_number].push_back(n_point);
574  }
575  }
576  }
577  // printf("n=%d, dx=%d, dy=%d, dz=%d, neighbors = %d\n", n, dx, dy, dz,
578  // neighborhoods_[n][direction_number].size());
579  }
580  }
581  }
582  }
583 }
584 
585 int PropagationDistanceField::getDirectionNumber(int dx, int dy, int dz) const
586 {
587  return (dx + 1) * 9 + (dy + 1) * 3 + dz + 1;
588 }
589 
590 Eigen::Vector3i PropagationDistanceField::getLocationDifference(int directionNumber) const
591 {
592  return direction_number_to_direction_[directionNumber];
593 }
594 
595 double PropagationDistanceField::getDistance(double x, double y, double z) const
596 {
597  return getDistance((*voxel_grid_.get())(x, y, z));
598 }
599 
600 double PropagationDistanceField::getDistance(int x, int y, int z) const
601 {
602  return getDistance(voxel_grid_->getCell(x, y, z));
603 }
604 
605 bool PropagationDistanceField::isCellValid(int x, int y, int z) const
606 {
607  return voxel_grid_->isCellValid(x, y, z);
608 }
609 
610 int PropagationDistanceField::getXNumCells() const
611 {
612  return voxel_grid_->getNumCells(DIM_X);
613 }
614 
615 int PropagationDistanceField::getYNumCells() const
616 {
617  return voxel_grid_->getNumCells(DIM_Y);
618 }
619 
620 int PropagationDistanceField::getZNumCells() const
621 {
622  return voxel_grid_->getNumCells(DIM_Z);
623 }
624 
625 bool PropagationDistanceField::gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const
626 {
627  voxel_grid_->gridToWorld(x, y, z, world_x, world_y, world_z);
628  return true;
629 }
630 
631 bool PropagationDistanceField::worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const
632 {
633  return voxel_grid_->worldToGrid(world_x, world_y, world_z, x, y, z);
634 }
635 
636 bool PropagationDistanceField::writeToStream(std::ostream& os) const
637 {
638  os << "resolution: " << resolution_ << std::endl;
639  os << "size_x: " << size_x_ << std::endl;
640  os << "size_y: " << size_y_ << std::endl;
641  os << "size_z: " << size_z_ << std::endl;
642  os << "origin_x: " << origin_x_ << std::endl;
643  os << "origin_y: " << origin_y_ << std::endl;
644  os << "origin_z: " << origin_z_ << std::endl;
645  // now the binary stuff
646 
647  // first writing to zlib compressed buffer
648  boost::iostreams::filtering_ostream out;
649  out.push(boost::iostreams::zlib_compressor());
650  out.push(os);
651 
652  for (unsigned int x = 0; x < static_cast<unsigned int>(getXNumCells()); x++)
653  {
654  for (unsigned int y = 0; y < static_cast<unsigned int>(getYNumCells()); y++)
655  {
656  for (unsigned int z = 0; z < static_cast<unsigned int>(getZNumCells()); z += 8)
657  {
658  std::bitset<8> bs(0);
659  unsigned int zv = std::min((unsigned int)8, getZNumCells() - z);
660  for (unsigned int zi = 0; zi < zv; zi++)
661  {
662  if (getCell(x, y, z + zi).distance_square_ == 0)
663  {
664  // std::cout << "Marking obs cell " << x << " " << y << " " << z+zi << std::endl;
665  bs[zi] = 1;
666  }
667  }
668  out.write((char*)&bs, sizeof(char));
669  }
670  }
671  }
672  out.flush();
673  return true;
674 }
675 
676 bool PropagationDistanceField::readFromStream(std::istream& is)
677 {
678  if (!is.good())
679  return false;
680 
681  std::string temp;
682 
683  is >> temp;
684  if (temp != "resolution:")
685  return false;
686  is >> resolution_;
687 
688  is >> temp;
689  if (temp != "size_x:")
690  return false;
691  is >> size_x_;
692 
693  is >> temp;
694  if (temp != "size_y:")
695  return false;
696  is >> size_y_;
697 
698  is >> temp;
699  if (temp != "size_z:")
700  return false;
701  is >> size_z_;
702 
703  is >> temp;
704  if (temp != "origin_x:")
705  return false;
706  is >> origin_x_;
707 
708  is >> temp;
709  if (temp != "origin_y:")
710  return false;
711  is >> origin_y_;
712 
713  is >> temp;
714  if (temp != "origin_z:")
715  return false;
716  is >> origin_z_;
717 
718  // previous values for propogation_negative_ and max_distance_ will be used
719 
720  initialize();
721 
722  // this should be newline
723  char nl;
724  is.get(nl);
725 
726  // now we start the compressed portion
727  boost::iostreams::filtering_istream in;
728  in.push(boost::iostreams::zlib_decompressor());
729  in.push(is);
730 
731  // std::cout << "Nums " << getXNumCells() << " " << getYNumCells() << " " << getZNumCells() << std::endl;
732 
733  EigenSTL::vector_Vector3i obs_points;
734  for (unsigned int x = 0; x < static_cast<unsigned int>(getXNumCells()); x++)
735  {
736  for (unsigned int y = 0; y < static_cast<unsigned int>(getYNumCells()); y++)
737  {
738  for (unsigned int z = 0; z < static_cast<unsigned int>(getZNumCells()); z += 8)
739  {
740  char inchar;
741  if (!in.good())
742  {
743  return false;
744  }
745  in.get(inchar);
746  std::bitset<8> inbit((unsigned long long)inchar);
747  unsigned int zv = std::min((unsigned int)8, getZNumCells() - z);
748  for (unsigned int zi = 0; zi < zv; zi++)
749  {
750  if (inbit[zi] == 1)
751  {
752  // std::cout << "Adding obs cell " << x << " " << y << " " << z+zi << std::endl;
753  obs_points.push_back(Eigen::Vector3i(x, y, z + zi));
754  }
755  }
756  }
757  }
758  }
759  addNewObstacleVoxels(obs_points);
760  return true;
761 }
762 } // namespace distance_field
distance_field::PropagationDistanceField::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, bool propagate_negative_distances=false)
Constructor that initializes entire distance field to empty - all cells will be assigned maximum dist...
Definition: propagation_distance_field.cpp:78
initialize
bool initialize(MeshCollisionTraversalNode< BV > &node, BVHModel< BV > &model1, Transform3< typename BV::S > &tf1, BVHModel< BV > &model2, Transform3< typename BV::S > &tf2, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, bool use_refit, bool refit_bottomup)
print
void print(PropagationDistanceField &pdf, int numX, int numY, int numZ)
Definition: test_distance_field.cpp:69
propagation_distance_field.h
distance_field::PropDistanceFieldVoxel::closest_point_
Eigen::Vector3i closest_point_
Closest occupied cell.
Definition: propagation_distance_field.h:101
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
distance_field::PropDistanceFieldVoxel::closest_negative_point_
Eigen::Vector3i closest_negative_point_
Closest unoccupied cell.
Definition: propagation_distance_field.h:102
getDistance
S getDistance(const Vector3< S > &p)
octomath::Vector3
octomap::OcTree
console.h
EigenSTL::vector_Vector3i
std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > vector_Vector3i
Definition: propagation_distance_field.h:80
distance_field::PropDistanceFieldVoxel::update_direction_
int update_direction_
Direction from which this voxel was updated for occupied distance propagation.
Definition: propagation_distance_field.h:103
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
y
double y
distance_field::DIM_Y
@ DIM_Y
Definition: voxel_grid.h:114
d
d
distance_field::DIM_Z
@ DIM_Z
Definition: voxel_grid.h:115
distance_field::DIM_X
@ DIM_X
Definition: voxel_grid.h:113
distance_field
Namespace for holding classes that generate distance fields.
Definition: distance_field.h:64
point
std::chrono::system_clock::time_point point
set
ROSCPP_DECL void set(const std::string &key, bool b)
distance_field::PropDistanceFieldVoxel::distance_square_
int distance_square_
Distance in cells to the closest obstacle, squared.
Definition: propagation_distance_field.h:99
distance_field::PropDistanceFieldVoxel::negative_distance_square_
int negative_distance_square_
Distance in cells to the nearest unoccupied cell, squared.
Definition: propagation_distance_field.h:100
x
double x
distance_field::PropDistanceFieldVoxel::negative_update_direction_
int negative_update_direction_
Direction from which this voxel was updated for negative distance propagation.
Definition: propagation_distance_field.h:104
z
double z
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
distance_field::PropDistanceFieldVoxel
Structure that holds voxel information for the DistanceField. Will be used in VoxelGrid.
Definition: propagation_distance_field.h:75


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Oct 12 2021 02:24:44