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 
96 {
97  ROS_DEBUG_NAMED("distance_field", "[");
98  VoxelSet::const_iterator it;
99  for (it = set.begin(); it != set.end(); ++it)
100  {
101  Eigen::Vector3i loc1 = *it;
102  ROS_DEBUG_NAMED("distance_field", "%d, %d, %d ", loc1.x(), loc1.y(), loc1.z());
103  }
104  ROS_DEBUG_NAMED("distance_field", "] size=%u\n", (unsigned int)set.size());
105 }
106 
108 {
109  ROS_DEBUG_NAMED("distance_field", "[");
110  EigenSTL::vector_Vector3d::const_iterator it;
111  for (it = points.begin(); it != points.end(); ++it)
112  {
113  Eigen::Vector3d loc1 = *it;
114  ROS_DEBUG_NAMED("distance_field", "%g, %g, %g ", loc1.x(), loc1.y(), loc1.z());
115  }
116  ROS_DEBUG_NAMED("distance_field", "] size=%u\n", (unsigned int)points.size());
117 }
118 
120  const EigenSTL::vector_Vector3d& new_points)
121 {
122  VoxelSet old_point_set;
123  for (unsigned int i = 0; i < old_points.size(); i++)
124  {
125  Eigen::Vector3i voxel_loc;
126  bool valid = worldToGrid(old_points[i].x(), old_points[i].y(), old_points[i].z(), voxel_loc.x(), voxel_loc.y(),
127  voxel_loc.z());
128  if (valid)
129  {
130  old_point_set.insert(voxel_loc);
131  }
132  }
133 
134  VoxelSet new_point_set;
135  for (unsigned int i = 0; i < new_points.size(); i++)
136  {
137  Eigen::Vector3i voxel_loc;
138  bool valid = worldToGrid(new_points[i].x(), new_points[i].y(), new_points[i].z(), voxel_loc.x(), voxel_loc.y(),
139  voxel_loc.z());
140  if (valid)
141  {
142  new_point_set.insert(voxel_loc);
143  }
144  }
146 
147  EigenSTL::vector_Vector3i old_not_new;
148  std::set_difference(old_point_set.begin(), old_point_set.end(), new_point_set.begin(), new_point_set.end(),
149  std::inserter(old_not_new, old_not_new.end()), comp);
150 
151  EigenSTL::vector_Vector3i new_not_old;
152  std::set_difference(new_point_set.begin(), new_point_set.end(), old_point_set.begin(), old_point_set.end(),
153  std::inserter(new_not_old, new_not_old.end()), comp);
154 
155  EigenSTL::vector_Vector3i new_not_in_current;
156  for (unsigned int i = 0; i < new_not_old.size(); i++)
157  {
158  if (voxel_grid_->getCell(new_not_old[i].x(), new_not_old[i].y(), new_not_old[i].z()).distance_square_ != 0)
159  {
160  new_not_in_current.push_back(new_not_old[i]);
161  }
162  // ROS_INFO_NAMED("distance_field", "Adding obstacle voxel %d %d %d", (*it).x(), (*it).y(), (*it).z());
163  }
164 
165  removeObstacleVoxels(old_not_new);
166  addNewObstacleVoxels(new_not_in_current);
167 
168  // ROS_DEBUG_NAMED("distance_field", "new=" );
169  // print(points_added);
170  // ROS_DEBUG_NAMED("distance_field", "removed=" );
171  // print(points_removed);
172  // ROS_DEBUG_NAMED("distance_field", "obstacle_voxel_locations_=" );
173  // print(object_voxel_locations_);
174  // ROS_DEBUG_NAMED("distance_field", "");
175 }
176 
178 {
179  EigenSTL::vector_Vector3i voxel_points;
180 
181  for (unsigned int i = 0; i < points.size(); i++)
182  {
183  // Convert to voxel coordinates
184  Eigen::Vector3i voxel_loc;
185  bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(), voxel_loc.x(), voxel_loc.y(), voxel_loc.z());
186 
187  if (valid)
188  {
189  if (voxel_grid_->getCell(voxel_loc.x(), voxel_loc.y(), voxel_loc.z()).distance_square_ > 0)
190  {
191  voxel_points.push_back(voxel_loc);
192  }
193  }
194  }
195  addNewObstacleVoxels(voxel_points);
196 }
197 
199 {
200  EigenSTL::vector_Vector3i voxel_points;
201  // VoxelSet voxel_locs;
202 
203  for (unsigned int i = 0; i < points.size(); i++)
204  {
205  // Convert to voxel coordinates
206  Eigen::Vector3i voxel_loc;
207  bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(), voxel_loc.x(), voxel_loc.y(), voxel_loc.z());
208 
209  if (valid)
210  {
211  voxel_points.push_back(voxel_loc);
212  // if(voxel_grid_->getCell(voxel_loc.x(),voxel_loc.y(),voxel_loc.z()).distance_square_ == 0) {
213  // voxel_locs.insert(voxel_loc);
214  //}
215  }
216  }
217 
218  removeObstacleVoxels(voxel_points);
219 }
220 
222 {
223  int initial_update_direction = getDirectionNumber(0, 0, 0);
224  bucket_queue_[0].reserve(voxel_points.size());
225  EigenSTL::vector_Vector3i negative_stack;
227  {
228  negative_stack.reserve(getXNumCells() * getYNumCells() * getZNumCells());
229  negative_bucket_queue_[0].reserve(voxel_points.size());
230  }
231 
232  for (unsigned int i = 0; i < voxel_points.size(); i++)
233  {
234  PropDistanceFieldVoxel& voxel = voxel_grid_->getCell(voxel_points[i].x(), voxel_points[i].y(), voxel_points[i].z());
235  const Eigen::Vector3i& loc = voxel_points[i];
236  voxel.distance_square_ = 0;
237  voxel.closest_point_ = loc;
238  voxel.update_direction_ = initial_update_direction;
239  bucket_queue_[0].push_back(loc);
241  {
246  negative_stack.push_back(loc);
247  }
248  }
250 
252  {
253  while (!negative_stack.empty())
254  {
255  Eigen::Vector3i loc = negative_stack.back();
256  negative_stack.pop_back();
257 
258  for (int neighbor = 0; neighbor < 27; neighbor++)
259  {
260  Eigen::Vector3i diff = getLocationDifference(neighbor);
261  Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
262 
263  if (isCellValid(nloc.x(), nloc.y(), nloc.z()))
264  {
265  PropDistanceFieldVoxel& nvoxel = voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z());
266  Eigen::Vector3i& close_point = nvoxel.closest_negative_point_;
267  if (!isCellValid(close_point.x(), close_point.y(), close_point.z()))
268  {
269  close_point = nloc;
270  }
271  PropDistanceFieldVoxel& closest_point_voxel =
272  voxel_grid_->getCell(close_point.x(), close_point.y(), close_point.z());
273 
274  // our closest non-obstacle cell has become an obstacle
275  if (closest_point_voxel.negative_distance_square_ != 0)
276  {
277  // find all neigbors inside pre-existing obstacles whose
278  // closest_negative_point_ is now an obstacle. These must all be
279  // set to max_distance_sq_ so they will be re-propogated with a new
280  // closest_negative_point_ that is outside the obstacle.
282  {
287  negative_stack.push_back(nloc);
288  }
289  }
290  else
291  {
292  // this cell still has a valid non-obstacle cell, so we need to propogate from it
293  nvoxel.negative_update_direction_ = initial_update_direction;
294  negative_bucket_queue_[0].push_back(nloc);
295  }
296  }
297  }
298  }
300  }
301 }
302 
304 // const VoxelSet& locations )
305 {
307  EigenSTL::vector_Vector3i negative_stack;
308  int initial_update_direction = getDirectionNumber(0, 0, 0);
309 
310  stack.reserve(getXNumCells() * getYNumCells() * getZNumCells());
311  bucket_queue_[0].reserve(voxel_points.size());
313  {
314  negative_stack.reserve(getXNumCells() * getYNumCells() * getZNumCells());
315  negative_bucket_queue_[0].reserve(voxel_points.size());
316  }
317 
318  // First reset the obstacle voxels,
319  // VoxelSet::const_iterator it = locations.begin();
320  // for( it=locations.begin(); it!=locations.end(); ++it)
321  // {
322  // Eigen::Vector3i loc = *it;
323  // bool valid = isCellValid( loc.x(), loc.y(), loc.z());
324  // if (!valid)
325  // continue;
326  for (unsigned int i = 0; i < voxel_points.size(); i++)
327  {
328  PropDistanceFieldVoxel& voxel = voxel_grid_->getCell(voxel_points[i].x(), voxel_points[i].y(), voxel_points[i].z());
330  voxel.closest_point_ = voxel_points[i];
331  voxel.update_direction_ = initial_update_direction; // not needed?
332  stack.push_back(voxel_points[i]);
334  {
335  voxel.negative_distance_square_ = 0.0;
336  voxel.closest_negative_point_ = voxel_points[i];
337  voxel.negative_update_direction_ = initial_update_direction;
338  negative_bucket_queue_[0].push_back(voxel_points[i]);
339  }
340  }
341 
342  // Reset all neighbors who's closest point is now gone.
343  while (!stack.empty())
344  {
345  Eigen::Vector3i loc = stack.back();
346  stack.pop_back();
347 
348  for (int neighbor = 0; neighbor < 27; neighbor++)
349  {
350  Eigen::Vector3i diff = getLocationDifference(neighbor);
351  Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
352 
353  if (isCellValid(nloc.x(), nloc.y(), nloc.z()))
354  {
355  PropDistanceFieldVoxel& nvoxel = voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z());
356  Eigen::Vector3i& close_point = nvoxel.closest_point_;
357  if (!isCellValid(close_point.x(), close_point.y(), close_point.z()))
358  {
359  close_point = nloc;
360  }
361  PropDistanceFieldVoxel& closest_point_voxel =
362  voxel_grid_->getCell(close_point.x(), close_point.y(), close_point.z());
363 
364  if (closest_point_voxel.distance_square_ != 0)
365  { // closest point no longer exists
366  if (nvoxel.distance_square_ != max_distance_sq_)
367  {
369  nvoxel.closest_point_ = nloc;
370  nvoxel.update_direction_ = initial_update_direction; // not needed?
371  stack.push_back(nloc);
372  }
373  }
374  else
375  { // add to queue so we can propagate the values
376  nvoxel.update_direction_ = initial_update_direction;
377  bucket_queue_[0].push_back(nloc);
378  }
379  }
380  }
381  }
383 
385  {
387  }
388 }
389 
391 {
392  // now process the queue:
393  for (unsigned int i = 0; i < bucket_queue_.size(); ++i)
394  {
395  EigenSTL::vector_Vector3i::iterator list_it = bucket_queue_[i].begin();
396  EigenSTL::vector_Vector3i::iterator list_end = bucket_queue_[i].end();
397  for (; list_it != list_end; ++list_it)
398  {
399  const Eigen::Vector3i& loc = *list_it;
400  PropDistanceFieldVoxel* vptr = &voxel_grid_->getCell(loc.x(), loc.y(), loc.z());
401 
402  // select the neighborhood list based on the update direction:
403  EigenSTL::vector_Vector3i* neighborhood;
404  int d = i;
405  if (d > 1)
406  d = 1;
407 
408  // This will never happen. update_direction_ is always set before voxel is added to bucket queue.
409  if (vptr->update_direction_ < 0 || vptr->update_direction_ > 26)
410  {
411  ROS_ERROR_NAMED("distance_field", "PROGRAMMING ERROR: Invalid update direction detected: %d",
412  vptr->update_direction_);
413  continue;
414  }
415 
416  neighborhood = &neighborhoods_[d][vptr->update_direction_];
417 
418  for (unsigned int n = 0; n < neighborhood->size(); n++)
419  {
420  Eigen::Vector3i diff = (*neighborhood)[n];
421  Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
422  if (!isCellValid(nloc.x(), nloc.y(), nloc.z()))
423  continue;
424 
425  // the real update code:
426  // calculate the neighbor's new distance based on my closest filled voxel:
427  PropDistanceFieldVoxel* neighbor = &voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z());
428  int new_distance_sq = (vptr->closest_point_ - nloc).squaredNorm();
429  if (new_distance_sq > max_distance_sq_)
430  continue;
431 
432  if (new_distance_sq < neighbor->distance_square_)
433  {
434  // update the neighboring voxel
435  neighbor->distance_square_ = new_distance_sq;
436  neighbor->closest_point_ = vptr->closest_point_;
437  neighbor->update_direction_ = getDirectionNumber(diff.x(), diff.y(), diff.z());
438 
439  // and put it in the queue:
440  bucket_queue_[new_distance_sq].push_back(nloc);
441  }
442  }
443  }
444  bucket_queue_[i].clear();
445  }
446 }
447 
449 {
450  // now process the queue:
451  for (unsigned int i = 0; i < negative_bucket_queue_.size(); ++i)
452  {
453  EigenSTL::vector_Vector3i::iterator list_it = negative_bucket_queue_[i].begin();
454  EigenSTL::vector_Vector3i::iterator list_end = negative_bucket_queue_[i].end();
455  for (; list_it != list_end; ++list_it)
456  {
457  const Eigen::Vector3i& loc = *list_it;
458  PropDistanceFieldVoxel* vptr = &voxel_grid_->getCell(loc.x(), loc.y(), loc.z());
459 
460  // select the neighborhood list based on the update direction:
461  EigenSTL::vector_Vector3i* neighborhood;
462  int d = i;
463  if (d > 1)
464  d = 1;
465 
466  // This will never happen. negative_update_direction_ is always set before voxel is added to
467  // negative_bucket_queue_.
468  if (vptr->negative_update_direction_ < 0 || vptr->negative_update_direction_ > 26)
469  {
470  ROS_ERROR_NAMED("distance_field", "PROGRAMMING ERROR: Invalid update direction detected: %d",
471  vptr->update_direction_);
472  continue;
473  }
474 
475  neighborhood = &neighborhoods_[d][vptr->negative_update_direction_];
476 
477  for (unsigned int n = 0; n < neighborhood->size(); n++)
478  {
479  Eigen::Vector3i diff = (*neighborhood)[n];
480  Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
481  if (!isCellValid(nloc.x(), nloc.y(), nloc.z()))
482  continue;
483 
484  // the real update code:
485  // calculate the neighbor's new distance based on my closest filled voxel:
486  PropDistanceFieldVoxel* neighbor = &voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z());
487  int new_distance_sq = (vptr->closest_negative_point_ - nloc).squaredNorm();
488  if (new_distance_sq > max_distance_sq_)
489  continue;
490  // std::cout << "Looking at " << nloc.x() << " " << nloc.y() << " " << nloc.z() << " " << new_distance_sq << " "
491  // << neighbor->negative_distance_square_ << std::endl;
492  if (new_distance_sq < neighbor->negative_distance_square_)
493  {
494  // std::cout << "Updating " << nloc.x() << " " << nloc.y() << " " << nloc.z() << " " << new_distance_sq <<
495  // std::endl;
496  // update the neighboring voxel
497  neighbor->negative_distance_square_ = new_distance_sq;
499  neighbor->negative_update_direction_ = getDirectionNumber(diff.x(), diff.y(), diff.z());
500 
501  // and put it in the queue:
502  negative_bucket_queue_[new_distance_sq].push_back(nloc);
503  }
504  }
505  }
506  negative_bucket_queue_[i].clear();
507  }
508 }
509 
511 {
513  for (int x = 0; x < getXNumCells(); x++)
514  {
515  for (int y = 0; y < getYNumCells(); y++)
516  {
517  for (int z = 0; z < getZNumCells(); z++)
518  {
519  PropDistanceFieldVoxel& voxel = voxel_grid_->getCell(x, y, z);
520  voxel.closest_negative_point_.x() = x;
521  voxel.closest_negative_point_.y() = y;
522  voxel.closest_negative_point_.z() = z;
523  voxel.negative_distance_square_ = 0;
524  }
525  }
526  }
527  // object_voxel_locations_.clear();
528 }
529 
531 {
532  // first initialize the direction number mapping:
534  for (int dx = -1; dx <= 1; ++dx)
535  {
536  for (int dy = -1; dy <= 1; ++dy)
537  {
538  for (int dz = -1; dz <= 1; ++dz)
539  {
540  int direction_number = getDirectionNumber(dx, dy, dz);
541  Eigen::Vector3i n_point(dx, dy, dz);
542  direction_number_to_direction_[direction_number] = n_point;
543  }
544  }
545  }
546 
547  neighborhoods_.resize(2);
548  for (int n = 0; n < 2; n++)
549  {
550  neighborhoods_[n].resize(27);
551  // source directions
552  for (int dx = -1; dx <= 1; ++dx)
553  {
554  for (int dy = -1; dy <= 1; ++dy)
555  {
556  for (int dz = -1; dz <= 1; ++dz)
557  {
558  int direction_number = getDirectionNumber(dx, dy, dz);
559  // target directions:
560  for (int tdx = -1; tdx <= 1; ++tdx)
561  {
562  for (int tdy = -1; tdy <= 1; ++tdy)
563  {
564  for (int tdz = -1; tdz <= 1; ++tdz)
565  {
566  if (tdx == 0 && tdy == 0 && tdz == 0)
567  continue;
568  if (n >= 1)
569  {
570  if ((abs(tdx) + abs(tdy) + abs(tdz)) != 1)
571  continue;
572  if (dx * tdx < 0 || dy * tdy < 0 || dz * tdz < 0)
573  continue;
574  }
575  Eigen::Vector3i n_point(tdx, tdy, tdz);
576  neighborhoods_[n][direction_number].push_back(n_point);
577  }
578  }
579  }
580  // printf("n=%d, dx=%d, dy=%d, dz=%d, neighbors = %d\n", n, dx, dy, dz,
581  // neighborhoods_[n][direction_number].size());
582  }
583  }
584  }
585  }
586 }
587 
588 int PropagationDistanceField::getDirectionNumber(int dx, int dy, int dz) const
589 {
590  return (dx + 1) * 9 + (dy + 1) * 3 + dz + 1;
591 }
592 
593 Eigen::Vector3i PropagationDistanceField::getLocationDifference(int directionNumber) const
594 {
595  return direction_number_to_direction_[directionNumber];
596 }
597 
598 double PropagationDistanceField::getDistance(double x, double y, double z) const
599 {
600  return getDistance((*voxel_grid_.get())(x, y, z));
601 }
602 
603 double PropagationDistanceField::getDistance(int x, int y, int z) const
604 {
605  return getDistance(voxel_grid_->getCell(x, y, z));
606 }
607 
608 bool PropagationDistanceField::isCellValid(int x, int y, int z) const
609 {
610  return voxel_grid_->isCellValid(x, y, z);
611 }
612 
614 {
615  return voxel_grid_->getNumCells(DIM_X);
616 }
617 
619 {
620  return voxel_grid_->getNumCells(DIM_Y);
621 }
622 
624 {
625  return voxel_grid_->getNumCells(DIM_Z);
626 }
627 
628 bool PropagationDistanceField::gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const
629 {
630  voxel_grid_->gridToWorld(x, y, z, world_x, world_y, world_z);
631  return true;
632 }
633 
634 bool PropagationDistanceField::worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const
635 {
636  return voxel_grid_->worldToGrid(world_x, world_y, world_z, x, y, z);
637 }
638 
639 bool PropagationDistanceField::writeToStream(std::ostream& os) const
640 {
641  os << "resolution: " << resolution_ << std::endl;
642  os << "size_x: " << size_x_ << std::endl;
643  os << "size_y: " << size_y_ << std::endl;
644  os << "size_z: " << size_z_ << std::endl;
645  os << "origin_x: " << origin_x_ << std::endl;
646  os << "origin_y: " << origin_y_ << std::endl;
647  os << "origin_z: " << origin_z_ << std::endl;
648  // now the binary stuff
649 
650  // first writing to zlib compressed buffer
651  boost::iostreams::filtering_ostream out;
652  out.push(boost::iostreams::zlib_compressor());
653  out.push(os);
654 
655  for (unsigned int x = 0; x < static_cast<unsigned int>(getXNumCells()); x++)
656  {
657  for (unsigned int y = 0; y < static_cast<unsigned int>(getYNumCells()); y++)
658  {
659  for (unsigned int z = 0; z < static_cast<unsigned int>(getZNumCells()); z += 8)
660  {
661  std::bitset<8> bs(0);
662  unsigned int zv = std::min((unsigned int)8, getZNumCells() - z);
663  for (unsigned int zi = 0; zi < zv; zi++)
664  {
665  if (getCell(x, y, z + zi).distance_square_ == 0)
666  {
667  // std::cout << "Marking obs cell " << x << " " << y << " " << z+zi << std::endl;
668  bs[zi] = 1;
669  }
670  }
671  out.write((char*)&bs, sizeof(char));
672  }
673  }
674  }
675  out.flush();
676  return true;
677 }
678 
680 {
681  if (!is.good())
682  return false;
683 
684  std::string temp;
685 
686  is >> temp;
687  if (temp != "resolution:")
688  return false;
689  is >> resolution_;
690 
691  is >> temp;
692  if (temp != "size_x:")
693  return false;
694  is >> size_x_;
695 
696  is >> temp;
697  if (temp != "size_y:")
698  return false;
699  is >> size_y_;
700 
701  is >> temp;
702  if (temp != "size_z:")
703  return false;
704  is >> size_z_;
705 
706  is >> temp;
707  if (temp != "origin_x:")
708  return false;
709  is >> origin_x_;
710 
711  is >> temp;
712  if (temp != "origin_y:")
713  return false;
714  is >> origin_y_;
715 
716  is >> temp;
717  if (temp != "origin_z:")
718  return false;
719  is >> origin_z_;
720 
721  // previous values for propogation_negative_ and max_distance_ will be used
722 
723  initialize();
724 
725  // this should be newline
726  char nl;
727  is.get(nl);
728 
729  // now we start the compressed portion
730  boost::iostreams::filtering_istream in;
731  in.push(boost::iostreams::zlib_decompressor());
732  in.push(is);
733 
734  // std::cout << "Nums " << getXNumCells() << " " << getYNumCells() << " " << getZNumCells() << std::endl;
735 
736  EigenSTL::vector_Vector3i obs_points;
737  for (unsigned int x = 0; x < static_cast<unsigned int>(getXNumCells()); x++)
738  {
739  for (unsigned int y = 0; y < static_cast<unsigned int>(getYNumCells()); y++)
740  {
741  for (unsigned int z = 0; z < static_cast<unsigned int>(getZNumCells()); z += 8)
742  {
743  char inchar;
744  if (!in.good())
745  {
746  return false;
747  }
748  in.get(inchar);
749  std::bitset<8> inbit((unsigned long long)inchar);
750  unsigned int zv = std::min((unsigned int)8, getZNumCells() - z);
751  for (unsigned int zi = 0; zi < zv; zi++)
752  {
753  if (inbit[zi] == 1)
754  {
755  // std::cout << "Adding obs cell " << x << " " << y << " " << z+zi << std::endl;
756  obs_points.push_back(Eigen::Vector3i(x, y, z + zi));
757  }
758  }
759  }
760  }
761  }
762  addNewObstacleVoxels(obs_points);
763  return true;
764 }
765 } // namespace distance_field
d
bool propagate_negative_
Whether or not to propagate negative distances.
Eigen::Vector3i getLocationDifference(int directionNumber) const
Helper function that gets change values given single number representing update direction.
int getZNumCells() const override
Gets the number of cells along the Z axis.
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:90
bool writeToStream(std::ostream &stream) const override
Writes the contents of the distance field to the supplied stream.
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
const PropDistanceFieldVoxel & getCell(int x, int y, int z) const
Gets full cell data given an index.
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.
Structure that holds voxel information for the DistanceField. Will be used in VoxelGrid.
double size_x_
X size of the distance field.
bool gridToWorld(int x, int y, int z, double &world_x, double &world_y, double &world_z) const override
Converts from an set of integer indices to a world location given the origin and resolution parameter...
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
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...
void addPointsToField(const EigenSTL::vector_Vector3d &points) override
Add a set of obstacle points to the distance field, updating distance values accordingly. The distance field may already contain obstacle cells.
Eigen::Vector3i closest_point_
Closest occupied cell.
Eigen::Vector3i closest_negative_point_
Closest unoccupied cell.
bool isCellValid(int x, int y, int z) const override
Determines whether or not the cell associated with the supplied indices is valid for this distance fi...
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.
int getXNumCells() const override
Gets the number of cells along the X axis.
double y
double origin_y_
Y origin of the distance field.
bool readFromStream(std::istream &stream) override
Reads, parameterizes, and populates the distance field based on the supplied stream.
#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_.
double size_z_
Z size of the distance field.
void reset() override
Resets the entire distance field to max_distance for positive values and zero for negative values...
VoxelGrid< PropDistanceFieldVoxel >::Ptr voxel_grid_
Actual container for distance data.
bool worldToGrid(double world_x, double world_y, double world_z, int &x, int &y, int &z) const override
Converts from a world location to a set of integer indices. Should return false if the world location...
double z
double origin_z_
Z origin of the distance field.
std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > vector_Vector3i
int getYNumCells() const override
Gets the number of cells along the Y axis.
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_.
double getResolution() const
Gets the resolution of the distance field in meters.
Namespace for holding classes that generate distance fields.
double origin_x_
X origin of the distance field.
void updatePointsInField(const EigenSTL::vector_Vector3d &old_points, const EigenSTL::vector_Vector3d &new_points) override
This function will remove any obstacle points that are in the old point set but not the new point set...
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.
double getDistance(double x, double y, double z) const override
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.
void initialize()
Initializes the field, resetting the voxel grid and building a sqrt lookup table for efficiency based...
void removePointsFromField(const EigenSTL::vector_Vector3d &points) override
Remove a set of obstacle points from the distance field, updating distance values accordingly...
int negative_distance_square_
Distance in cells to the nearest unoccupied cell, squared.
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_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...
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
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.
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 Thu Jan 14 2021 03:57:44