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  std::vector<Eigen::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  std::vector<Eigen::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  std::vector<Eigen::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  std::vector<Eigen::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  std::vector<Eigen::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 
229 void PropagationDistanceField::addNewObstacleVoxels(const std::vector<Eigen::Vector3i>& voxel_points)
230 {
231  int initial_update_direction = getDirectionNumber(0, 0, 0);
232  bucket_queue_[0].reserve(voxel_points.size());
233  std::vector<Eigen::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 
311 void PropagationDistanceField::removeObstacleVoxels(const std::vector<Eigen::Vector3i>& voxel_points)
312 // const VoxelSet& locations )
313 {
314  std::vector<Eigen::Vector3i> stack;
315  std::vector<Eigen::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  std::vector<Eigen::Vector3i>::iterator list_it = bucket_queue_[i].begin();
404  std::vector<Eigen::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  std::vector<Eigen::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  std::vector<Eigen::Vector3i>::iterator list_it = negative_bucket_queue_[i].begin();
462  std::vector<Eigen::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  std::vector<Eigen::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  std::vector<Eigen::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::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...
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,...)
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 z
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.
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)
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.
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.
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.
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 Wed Jul 18 2018 02:48:31