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 
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 
77 {
81 
83 
84  bucket_queue_.resize(max_distance_sq_ + 1);
86 
87  // create a sqrt table:
88  sqrt_table_.resize(max_distance_sq_ + 1);
89  for (int i = 0; i <= max_distance_sq_; ++i)
90  sqrt_table_[i] = sqrt(double(i)) * resolution_;
91 
92  reset();
93 }
94 
95 int PropagationDistanceField::eucDistSq(Eigen::Vector3i point1, Eigen::Vector3i point2)
96 {
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;
101 }
102 
104 {
105  ROS_DEBUG_NAMED("distance_field", "[");
106  VoxelSet::const_iterator it;
107  for (it = set.begin(); it != set.end(); ++it)
108  {
109  Eigen::Vector3i loc1 = *it;
110  ROS_DEBUG_NAMED("distance_field", "%d, %d, %d ", loc1.x(), loc1.y(), loc1.z());
111  }
112  ROS_DEBUG_NAMED("distance_field", "] size=%u\n", (unsigned int)set.size());
113 }
114 
116 {
117  ROS_DEBUG_NAMED("distance_field", "[");
118  EigenSTL::vector_Vector3d::const_iterator it;
119  for (it = points.begin(); it != points.end(); ++it)
120  {
121  Eigen::Vector3d loc1 = *it;
122  ROS_DEBUG_NAMED("distance_field", "%g, %g, %g ", loc1.x(), loc1.y(), loc1.z());
123  }
124  ROS_DEBUG_NAMED("distance_field", "] size=%u\n", (unsigned int)points.size());
125 }
126 
128  const EigenSTL::vector_Vector3d& new_points)
129 {
130  VoxelSet old_point_set;
131  for (unsigned int i = 0; i < old_points.size(); i++)
132  {
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(),
135  voxel_loc.z());
136  if (valid)
137  {
138  old_point_set.insert(voxel_loc);
139  }
140  }
141 
142  VoxelSet new_point_set;
143  for (unsigned int i = 0; i < new_points.size(); i++)
144  {
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(),
147  voxel_loc.z());
148  if (valid)
149  {
150  new_point_set.insert(voxel_loc);
151  }
152  }
154 
155  EigenSTL::vector_Vector3i old_not_new;
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);
158 
159  EigenSTL::vector_Vector3i new_not_old;
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);
162 
163  EigenSTL::vector_Vector3i new_not_in_current;
164  for (unsigned int i = 0; i < new_not_old.size(); i++)
165  {
166  if (voxel_grid_->getCell(new_not_old[i].x(), new_not_old[i].y(), new_not_old[i].z()).distance_square_ != 0)
167  {
168  new_not_in_current.push_back(new_not_old[i]);
169  }
170  // ROS_INFO_NAMED("distance_field", "Adding obstacle voxel %d %d %d", (*it).x(), (*it).y(), (*it).z());
171  }
172 
173  removeObstacleVoxels(old_not_new);
174  addNewObstacleVoxels(new_not_in_current);
175 
176  // ROS_DEBUG_NAMED("distance_field", "new=" );
177  // print(points_added);
178  // ROS_DEBUG_NAMED("distance_field", "removed=" );
179  // print(points_removed);
180  // ROS_DEBUG_NAMED("distance_field", "obstacle_voxel_locations_=" );
181  // print(object_voxel_locations_);
182  // ROS_DEBUG_NAMED("distance_field", "");
183 }
184 
186 {
187  EigenSTL::vector_Vector3i voxel_points;
188 
189  for (unsigned int i = 0; i < points.size(); i++)
190  {
191  // Convert to voxel coordinates
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());
194 
195  if (valid)
196  {
197  if (voxel_grid_->getCell(voxel_loc.x(), voxel_loc.y(), voxel_loc.z()).distance_square_ > 0)
198  {
199  voxel_points.push_back(voxel_loc);
200  }
201  }
202  }
203  addNewObstacleVoxels(voxel_points);
204 }
205 
207 {
208  EigenSTL::vector_Vector3i voxel_points;
209  // VoxelSet voxel_locs;
210 
211  for (unsigned int i = 0; i < points.size(); i++)
212  {
213  // Convert to voxel coordinates
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());
216 
217  if (valid)
218  {
219  voxel_points.push_back(voxel_loc);
220  // if(voxel_grid_->getCell(voxel_loc.x(),voxel_loc.y(),voxel_loc.z()).distance_square_ == 0) {
221  // voxel_locs.insert(voxel_loc);
222  //}
223  }
224  }
225 
226  removeObstacleVoxels(voxel_points);
227 }
228 
230 {
231  int initial_update_direction = getDirectionNumber(0, 0, 0);
232  bucket_queue_[0].reserve(voxel_points.size());
233  EigenSTL::vector_Vector3i negative_stack;
235  {
236  negative_stack.reserve(getXNumCells() * getYNumCells() * getZNumCells());
237  negative_bucket_queue_[0].reserve(voxel_points.size());
238  }
239 
240  for (unsigned int i = 0; i < voxel_points.size(); i++)
241  {
242  PropDistanceFieldVoxel& voxel = voxel_grid_->getCell(voxel_points[i].x(), voxel_points[i].y(), voxel_points[i].z());
243  const Eigen::Vector3i& loc = voxel_points[i];
244  voxel.distance_square_ = 0;
245  voxel.closest_point_ = loc;
246  voxel.update_direction_ = initial_update_direction;
247  bucket_queue_[0].push_back(loc);
249  {
254  negative_stack.push_back(loc);
255  }
256  }
258 
260  {
261  while (!negative_stack.empty())
262  {
263  Eigen::Vector3i loc = negative_stack.back();
264  negative_stack.pop_back();
265 
266  for (int neighbor = 0; neighbor < 27; neighbor++)
267  {
268  Eigen::Vector3i diff = getLocationDifference(neighbor);
269  Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
270 
271  if (isCellValid(nloc.x(), nloc.y(), nloc.z()))
272  {
273  PropDistanceFieldVoxel& nvoxel = voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z());
274  Eigen::Vector3i& close_point = nvoxel.closest_negative_point_;
275  if (!isCellValid(close_point.x(), close_point.y(), close_point.z()))
276  {
277  close_point = nloc;
278  }
279  PropDistanceFieldVoxel& closest_point_voxel =
280  voxel_grid_->getCell(close_point.x(), close_point.y(), close_point.z());
281 
282  // our closest non-obstacle cell has become an obstacle
283  if (closest_point_voxel.negative_distance_square_ != 0)
284  {
285  // find all neigbors inside pre-existing obstacles whose
286  // closest_negative_point_ is now an obstacle. These must all be
287  // set to max_distance_sq_ so they will be re-propogated with a new
288  // closest_negative_point_ that is outside the obstacle.
290  {
295  negative_stack.push_back(nloc);
296  }
297  }
298  else
299  {
300  // this cell still has a valid non-obstacle cell, so we need to propogate from it
301  nvoxel.negative_update_direction_ = initial_update_direction;
302  negative_bucket_queue_[0].push_back(nloc);
303  }
304  }
305  }
306  }
308  }
309 }
310 
312 // const VoxelSet& locations )
313 {
315  EigenSTL::vector_Vector3i negative_stack;
316  int initial_update_direction = getDirectionNumber(0, 0, 0);
317 
318  stack.reserve(getXNumCells() * getYNumCells() * getZNumCells());
319  bucket_queue_[0].reserve(voxel_points.size());
321  {
322  negative_stack.reserve(getXNumCells() * getYNumCells() * getZNumCells());
323  negative_bucket_queue_[0].reserve(voxel_points.size());
324  }
325 
326  // First reset the obstacle voxels,
327  // VoxelSet::const_iterator it = locations.begin();
328  // for( it=locations.begin(); it!=locations.end(); ++it)
329  // {
330  // Eigen::Vector3i loc = *it;
331  // bool valid = isCellValid( loc.x(), loc.y(), loc.z());
332  // if (!valid)
333  // continue;
334  for (unsigned int i = 0; i < voxel_points.size(); i++)
335  {
336  PropDistanceFieldVoxel& voxel = voxel_grid_->getCell(voxel_points[i].x(), voxel_points[i].y(), voxel_points[i].z());
338  voxel.closest_point_ = voxel_points[i];
339  voxel.update_direction_ = initial_update_direction; // not needed?
340  stack.push_back(voxel_points[i]);
342  {
343  voxel.negative_distance_square_ = 0.0;
344  voxel.closest_negative_point_ = voxel_points[i];
345  voxel.negative_update_direction_ = initial_update_direction;
346  negative_bucket_queue_[0].push_back(voxel_points[i]);
347  }
348  }
349 
350  // Reset all neighbors who's closest point is now gone.
351  while (!stack.empty())
352  {
353  Eigen::Vector3i loc = stack.back();
354  stack.pop_back();
355 
356  for (int neighbor = 0; neighbor < 27; neighbor++)
357  {
358  Eigen::Vector3i diff = getLocationDifference(neighbor);
359  Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
360 
361  if (isCellValid(nloc.x(), nloc.y(), nloc.z()))
362  {
363  PropDistanceFieldVoxel& nvoxel = voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z());
364  Eigen::Vector3i& close_point = nvoxel.closest_point_;
365  if (!isCellValid(close_point.x(), close_point.y(), close_point.z()))
366  {
367  close_point = nloc;
368  }
369  PropDistanceFieldVoxel& closest_point_voxel =
370  voxel_grid_->getCell(close_point.x(), close_point.y(), close_point.z());
371 
372  if (closest_point_voxel.distance_square_ != 0)
373  { // closest point no longer exists
374  if (nvoxel.distance_square_ != max_distance_sq_)
375  {
377  nvoxel.closest_point_ = nloc;
378  nvoxel.update_direction_ = initial_update_direction; // not needed?
379  stack.push_back(nloc);
380  }
381  }
382  else
383  { // add to queue so we can propagate the values
384  nvoxel.update_direction_ = initial_update_direction;
385  bucket_queue_[0].push_back(nloc);
386  }
387  }
388  }
389  }
391 
393  {
395  }
396 }
397 
399 {
400  // now process the queue:
401  for (unsigned int i = 0; i < bucket_queue_.size(); ++i)
402  {
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)
406  {
407  const Eigen::Vector3i& loc = *list_it;
408  PropDistanceFieldVoxel* vptr = &voxel_grid_->getCell(loc.x(), loc.y(), loc.z());
409 
410  // select the neighborhood list based on the update direction:
411  EigenSTL::vector_Vector3i* neighborhood;
412  int D = i;
413  if (D > 1)
414  D = 1;
415 
416  // This will never happen. update_direction_ is always set before voxel is added to bucket queue.
417  if (vptr->update_direction_ < 0 || vptr->update_direction_ > 26)
418  {
419  ROS_ERROR_NAMED("distance_field", "PROGRAMMING ERROR: Invalid update direction detected: %d",
420  vptr->update_direction_);
421  continue;
422  }
423 
424  neighborhood = &neighborhoods_[D][vptr->update_direction_];
425 
426  for (unsigned int n = 0; n < neighborhood->size(); n++)
427  {
428  Eigen::Vector3i diff = (*neighborhood)[n];
429  Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
430  if (!isCellValid(nloc.x(), nloc.y(), nloc.z()))
431  continue;
432 
433  // the real update code:
434  // calculate the neighbor's new distance based on my closest filled voxel:
435  PropDistanceFieldVoxel* neighbor = &voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z());
436  int new_distance_sq = eucDistSq(vptr->closest_point_, nloc);
437  if (new_distance_sq > max_distance_sq_)
438  continue;
439 
440  if (new_distance_sq < neighbor->distance_square_)
441  {
442  // update the neighboring voxel
443  neighbor->distance_square_ = new_distance_sq;
444  neighbor->closest_point_ = vptr->closest_point_;
445  neighbor->update_direction_ = getDirectionNumber(diff.x(), diff.y(), diff.z());
446 
447  // and put it in the queue:
448  bucket_queue_[new_distance_sq].push_back(nloc);
449  }
450  }
451  }
452  bucket_queue_[i].clear();
453  }
454 }
455 
457 {
458  // now process the queue:
459  for (unsigned int i = 0; i < negative_bucket_queue_.size(); ++i)
460  {
461  EigenSTL::vector_Vector3i::iterator list_it = negative_bucket_queue_[i].begin();
462  EigenSTL::vector_Vector3i::iterator list_end = negative_bucket_queue_[i].end();
463  for (; list_it != list_end; ++list_it)
464  {
465  const Eigen::Vector3i& loc = *list_it;
466  PropDistanceFieldVoxel* vptr = &voxel_grid_->getCell(loc.x(), loc.y(), loc.z());
467 
468  // select the neighborhood list based on the update direction:
469  EigenSTL::vector_Vector3i* neighborhood;
470  int D = i;
471  if (D > 1)
472  D = 1;
473 
474  // This will never happen. negative_update_direction_ is always set before voxel is added to
475  // negative_bucket_queue_.
476  if (vptr->negative_update_direction_ < 0 || vptr->negative_update_direction_ > 26)
477  {
478  ROS_ERROR_NAMED("distance_field", "PROGRAMMING ERROR: Invalid update direction detected: %d",
479  vptr->update_direction_);
480  continue;
481  }
482 
483  neighborhood = &neighborhoods_[D][vptr->negative_update_direction_];
484 
485  for (unsigned int n = 0; n < neighborhood->size(); n++)
486  {
487  Eigen::Vector3i diff = (*neighborhood)[n];
488  Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
489  if (!isCellValid(nloc.x(), nloc.y(), nloc.z()))
490  continue;
491 
492  // the real update code:
493  // calculate the neighbor's new distance based on my closest filled voxel:
494  PropDistanceFieldVoxel* neighbor = &voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z());
495  int new_distance_sq = eucDistSq(vptr->closest_negative_point_, nloc);
496  if (new_distance_sq > max_distance_sq_)
497  continue;
498  // std::cout << "Looking at " << nloc.x() << " " << nloc.y() << " " << nloc.z() << " " << new_distance_sq << " "
499  // << neighbor->negative_distance_square_ << std::endl;
500  if (new_distance_sq < neighbor->negative_distance_square_)
501  {
502  // std::cout << "Updating " << nloc.x() << " " << nloc.y() << " " << nloc.z() << " " << new_distance_sq <<
503  // std::endl;
504  // update the neighboring voxel
505  neighbor->negative_distance_square_ = new_distance_sq;
507  neighbor->negative_update_direction_ = getDirectionNumber(diff.x(), diff.y(), diff.z());
508 
509  // and put it in the queue:
510  negative_bucket_queue_[new_distance_sq].push_back(nloc);
511  }
512  }
513  }
514  negative_bucket_queue_[i].clear();
515  }
516 }
517 
519 {
521  for (int x = 0; x < getXNumCells(); x++)
522  {
523  for (int y = 0; y < getYNumCells(); y++)
524  {
525  for (int z = 0; z < getZNumCells(); z++)
526  {
527  PropDistanceFieldVoxel& voxel = voxel_grid_->getCell(x, y, z);
528  voxel.closest_negative_point_.x() = x;
529  voxel.closest_negative_point_.y() = y;
530  voxel.closest_negative_point_.z() = z;
531  voxel.negative_distance_square_ = 0;
532  }
533  }
534  }
535  // object_voxel_locations_.clear();
536 }
537 
539 {
540  // first initialize the direction number mapping:
542  for (int dx = -1; dx <= 1; ++dx)
543  {
544  for (int dy = -1; dy <= 1; ++dy)
545  {
546  for (int dz = -1; dz <= 1; ++dz)
547  {
548  int direction_number = getDirectionNumber(dx, dy, dz);
549  Eigen::Vector3i n_point(dx, dy, dz);
550  direction_number_to_direction_[direction_number] = n_point;
551  }
552  }
553  }
554 
555  neighborhoods_.resize(2);
556  for (int n = 0; n < 2; n++)
557  {
558  neighborhoods_[n].resize(27);
559  // source directions
560  for (int dx = -1; dx <= 1; ++dx)
561  {
562  for (int dy = -1; dy <= 1; ++dy)
563  {
564  for (int dz = -1; dz <= 1; ++dz)
565  {
566  int direction_number = getDirectionNumber(dx, dy, dz);
567  // target directions:
568  for (int tdx = -1; tdx <= 1; ++tdx)
569  {
570  for (int tdy = -1; tdy <= 1; ++tdy)
571  {
572  for (int tdz = -1; tdz <= 1; ++tdz)
573  {
574  if (tdx == 0 && tdy == 0 && tdz == 0)
575  continue;
576  if (n >= 1)
577  {
578  if ((abs(tdx) + abs(tdy) + abs(tdz)) != 1)
579  continue;
580  if (dx * tdx < 0 || dy * tdy < 0 || dz * tdz < 0)
581  continue;
582  }
583  Eigen::Vector3i n_point(tdx, tdy, tdz);
584  neighborhoods_[n][direction_number].push_back(n_point);
585  }
586  }
587  }
588  // printf("n=%d, dx=%d, dy=%d, dz=%d, neighbors = %d\n", n, dx, dy, dz,
589  // neighborhoods_[n][direction_number].size());
590  }
591  }
592  }
593  }
594 }
595 
596 int PropagationDistanceField::getDirectionNumber(int dx, int dy, int dz) const
597 {
598  return (dx + 1) * 9 + (dy + 1) * 3 + dz + 1;
599 }
600 
601 Eigen::Vector3i PropagationDistanceField::getLocationDifference(int directionNumber) const
602 {
603  return direction_number_to_direction_[directionNumber];
604 }
605 
606 double PropagationDistanceField::getDistance(double x, double y, double z) const
607 {
608  return getDistance((*voxel_grid_.get())(x, y, z));
609 }
610 
611 double PropagationDistanceField::getDistance(int x, int y, int z) const
612 {
613  return getDistance(voxel_grid_->getCell(x, y, z));
614 }
615 
616 bool PropagationDistanceField::isCellValid(int x, int y, int z) const
617 {
618  return voxel_grid_->isCellValid(x, y, z);
619 }
620 
622 {
623  return voxel_grid_->getNumCells(DIM_X);
624 }
625 
627 {
628  return voxel_grid_->getNumCells(DIM_Y);
629 }
630 
632 {
633  return voxel_grid_->getNumCells(DIM_Z);
634 }
635 
636 bool PropagationDistanceField::gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const
637 {
638  voxel_grid_->gridToWorld(x, y, z, world_x, world_y, world_z);
639  return true;
640 }
641 
642 bool PropagationDistanceField::worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const
643 {
644  return voxel_grid_->worldToGrid(world_x, world_y, world_z, x, y, z);
645 }
646 
647 bool PropagationDistanceField::writeToStream(std::ostream& os) const
648 {
649  os << "resolution: " << resolution_ << std::endl;
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;
656  // now the binary stuff
657 
658  // first writing to zlib compressed buffer
659  boost::iostreams::filtering_ostream out;
660  out.push(boost::iostreams::zlib_compressor());
661  out.push(os);
662 
663  for (unsigned int x = 0; x < static_cast<unsigned int>(getXNumCells()); x++)
664  {
665  for (unsigned int y = 0; y < static_cast<unsigned int>(getYNumCells()); y++)
666  {
667  for (unsigned int z = 0; z < static_cast<unsigned int>(getZNumCells()); z += 8)
668  {
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++)
672  {
673  if (getCell(x, y, z + zi).distance_square_ == 0)
674  {
675  // std::cout << "Marking obs cell " << x << " " << y << " " << z+zi << std::endl;
676  bs[zi] = 1;
677  }
678  }
679  out.write((char*)&bs, sizeof(char));
680  }
681  }
682  }
683  out.flush();
684  return true;
685 }
686 
688 {
689  if (!is.good())
690  return false;
691 
692  std::string temp;
693 
694  is >> temp;
695  if (temp != "resolution:")
696  return false;
697  is >> resolution_;
698 
699  is >> temp;
700  if (temp != "size_x:")
701  return false;
702  is >> size_x_;
703 
704  is >> temp;
705  if (temp != "size_y:")
706  return false;
707  is >> size_y_;
708 
709  is >> temp;
710  if (temp != "size_z:")
711  return false;
712  is >> size_z_;
713 
714  is >> temp;
715  if (temp != "origin_x:")
716  return false;
717  is >> origin_x_;
718 
719  is >> temp;
720  if (temp != "origin_y:")
721  return false;
722  is >> origin_y_;
723 
724  is >> temp;
725  if (temp != "origin_z:")
726  return false;
727  is >> origin_z_;
728 
729  // previous values for propogation_negative_ and max_distance_ will be used
730 
731  initialize();
732 
733  // this should be newline
734  char nl;
735  is.get(nl);
736 
737  // now we start the compressed portion
738  boost::iostreams::filtering_istream in;
739  in.push(boost::iostreams::zlib_decompressor());
740  in.push(is);
741 
742  // std::cout << "Nums " << getXNumCells() << " " << getYNumCells() << " " << getZNumCells() << std::endl;
743 
744  EigenSTL::vector_Vector3i obs_points;
745  for (unsigned int x = 0; x < static_cast<unsigned int>(getXNumCells()); x++)
746  {
747  for (unsigned int y = 0; y < static_cast<unsigned int>(getYNumCells()); y++)
748  {
749  for (unsigned int z = 0; z < static_cast<unsigned int>(getZNumCells()); z += 8)
750  {
751  char inchar;
752  if (!in.good())
753  {
754  return false;
755  }
756  in.get(inchar);
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++)
760  {
761  if (inbit[zi] == 1)
762  {
763  // std::cout << "Adding obs cell " << x << " " << y << " " << z+zi << std::endl;
764  obs_points.push_back(Eigen::Vector3i(x, y, z + zi));
765  }
766  }
767  }
768  }
769  }
770  addNewObstacleVoxels(obs_points);
771  return true;
772 }
773 }
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...
Definition: voxel_grid.h:62
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 y
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 z
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)
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.
double x
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...


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:33