00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/distance_field/propagation_distance_field.h>
00038 #include <visualization_msgs/Marker.h>
00039 #include <console_bridge/console.h>
00040 #include <boost/iostreams/filtering_stream.hpp>
00041 #include <boost/iostreams/copy.hpp>
00042 #include <boost/iostreams/filter/zlib.hpp>
00043
00044 namespace distance_field
00045 {
00046 PropagationDistanceField::PropagationDistanceField(double size_x, double size_y, double size_z, double resolution,
00047 double origin_x, double origin_y, double origin_z,
00048 double max_distance, bool propagate_negative)
00049 : DistanceField(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z)
00050 , propagate_negative_(propagate_negative)
00051 , max_distance_(max_distance)
00052 {
00053 initialize();
00054 }
00055
00056 PropagationDistanceField::PropagationDistanceField(const octomap::OcTree& octree, const octomap::point3d& bbx_min,
00057 const octomap::point3d& bbx_max, double max_distance,
00058 bool propagate_negative_distances)
00059 : DistanceField(bbx_max.x() - bbx_min.x(), bbx_max.y() - bbx_min.y(), bbx_max.z() - bbx_min.z(),
00060 octree.getResolution(), bbx_min.x(), bbx_min.y(), bbx_min.z())
00061 , propagate_negative_(propagate_negative_distances)
00062 , max_distance_(max_distance)
00063 {
00064 initialize();
00065 addOcTreeToField(&octree);
00066 }
00067
00068 PropagationDistanceField::PropagationDistanceField(std::istream& is, double max_distance,
00069 bool propagate_negative_distances)
00070 : DistanceField(0, 0, 0, 0, 0, 0, 0), propagate_negative_(propagate_negative_distances), max_distance_(max_distance)
00071 {
00072 readFromStream(is);
00073 }
00074
00075 void PropagationDistanceField::initialize()
00076 {
00077 max_distance_sq_ = ceil(max_distance_ / resolution_) * ceil(max_distance_ / resolution_);
00078 voxel_grid_.reset(new VoxelGrid<PropDistanceFieldVoxel>(size_x_, size_y_, size_z_, resolution_, origin_x_, origin_y_,
00079 origin_z_, PropDistanceFieldVoxel(max_distance_sq_, 0)));
00080
00081 initNeighborhoods();
00082
00083 bucket_queue_.resize(max_distance_sq_ + 1);
00084 negative_bucket_queue_.resize(max_distance_sq_ + 1);
00085
00086
00087 sqrt_table_.resize(max_distance_sq_ + 1);
00088 for (int i = 0; i <= max_distance_sq_; ++i)
00089 sqrt_table_[i] = sqrt(double(i)) * resolution_;
00090
00091 reset();
00092 }
00093
00094 int PropagationDistanceField::eucDistSq(Eigen::Vector3i point1, Eigen::Vector3i point2)
00095 {
00096 int dx = point1.x() - point2.x();
00097 int dy = point1.y() - point2.y();
00098 int dz = point1.z() - point2.z();
00099 return dx * dx + dy * dy + dz * dz;
00100 }
00101
00102 void PropagationDistanceField::print(const VoxelSet& set)
00103 {
00104 logDebug("[");
00105 VoxelSet::const_iterator it;
00106 for (it = set.begin(); it != set.end(); ++it)
00107 {
00108 Eigen::Vector3i loc1 = *it;
00109 logDebug("%d, %d, %d ", loc1.x(), loc1.y(), loc1.z());
00110 }
00111 logDebug("] size=%u\n", (unsigned int)set.size());
00112 }
00113
00114 void PropagationDistanceField::print(const EigenSTL::vector_Vector3d& points)
00115 {
00116 logDebug("[");
00117 EigenSTL::vector_Vector3d::const_iterator it;
00118 for (it = points.begin(); it != points.end(); ++it)
00119 {
00120 Eigen::Vector3d loc1 = *it;
00121 logDebug("%g, %g, %g ", loc1.x(), loc1.y(), loc1.z());
00122 }
00123 logDebug("] size=%u\n", (unsigned int)points.size());
00124 }
00125
00126 void PropagationDistanceField::updatePointsInField(const EigenSTL::vector_Vector3d& old_points,
00127 const EigenSTL::vector_Vector3d& new_points)
00128 {
00129 VoxelSet old_point_set;
00130 for (unsigned int i = 0; i < old_points.size(); i++)
00131 {
00132 Eigen::Vector3i voxel_loc;
00133 bool valid = worldToGrid(old_points[i].x(), old_points[i].y(), old_points[i].z(), voxel_loc.x(), voxel_loc.y(),
00134 voxel_loc.z());
00135 if (valid)
00136 {
00137 old_point_set.insert(voxel_loc);
00138 }
00139 }
00140
00141 VoxelSet new_point_set;
00142 for (unsigned int i = 0; i < new_points.size(); i++)
00143 {
00144 Eigen::Vector3i voxel_loc;
00145 bool valid = worldToGrid(new_points[i].x(), new_points[i].y(), new_points[i].z(), voxel_loc.x(), voxel_loc.y(),
00146 voxel_loc.z());
00147 if (valid)
00148 {
00149 new_point_set.insert(voxel_loc);
00150 }
00151 }
00152 compareEigen_Vector3i comp;
00153
00154 std::vector<Eigen::Vector3i> old_not_new;
00155 std::set_difference(old_point_set.begin(), old_point_set.end(), new_point_set.begin(), new_point_set.end(),
00156 std::inserter(old_not_new, old_not_new.end()), comp);
00157
00158 std::vector<Eigen::Vector3i> new_not_old;
00159 std::set_difference(new_point_set.begin(), new_point_set.end(), old_point_set.begin(), old_point_set.end(),
00160 std::inserter(new_not_old, new_not_old.end()), comp);
00161
00162 std::vector<Eigen::Vector3i> new_not_in_current;
00163 for (unsigned int i = 0; i < new_not_old.size(); i++)
00164 {
00165 if (voxel_grid_->getCell(new_not_old[i].x(), new_not_old[i].y(), new_not_old[i].z()).distance_square_ != 0)
00166 {
00167 new_not_in_current.push_back(new_not_old[i]);
00168 }
00169
00170 }
00171
00172 removeObstacleVoxels(old_not_new);
00173 addNewObstacleVoxels(new_not_in_current);
00174
00175
00176
00177
00178
00179
00180
00181
00182 }
00183
00184 void PropagationDistanceField::addPointsToField(const EigenSTL::vector_Vector3d& points)
00185 {
00186 std::vector<Eigen::Vector3i> voxel_points;
00187
00188 for (unsigned int i = 0; i < points.size(); i++)
00189 {
00190
00191 Eigen::Vector3i voxel_loc;
00192 bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(), voxel_loc.x(), voxel_loc.y(), voxel_loc.z());
00193
00194 if (valid)
00195 {
00196 if (voxel_grid_->getCell(voxel_loc.x(), voxel_loc.y(), voxel_loc.z()).distance_square_ > 0)
00197 {
00198 voxel_points.push_back(voxel_loc);
00199 }
00200 }
00201 }
00202 addNewObstacleVoxels(voxel_points);
00203 }
00204
00205 void PropagationDistanceField::removePointsFromField(const EigenSTL::vector_Vector3d& points)
00206 {
00207 std::vector<Eigen::Vector3i> voxel_points;
00208
00209
00210 for (unsigned int i = 0; i < points.size(); i++)
00211 {
00212
00213 Eigen::Vector3i voxel_loc;
00214 bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(), voxel_loc.x(), voxel_loc.y(), voxel_loc.z());
00215
00216 if (valid)
00217 {
00218 voxel_points.push_back(voxel_loc);
00219
00220
00221
00222 }
00223 }
00224
00225 removeObstacleVoxels(voxel_points);
00226 }
00227
00228 void PropagationDistanceField::addNewObstacleVoxels(const std::vector<Eigen::Vector3i>& voxel_points)
00229 {
00230 int initial_update_direction = getDirectionNumber(0, 0, 0);
00231 bucket_queue_[0].reserve(voxel_points.size());
00232 std::vector<Eigen::Vector3i> negative_stack;
00233 if (propagate_negative_)
00234 {
00235 negative_stack.reserve(getXNumCells() * getYNumCells() * getZNumCells());
00236 negative_bucket_queue_[0].reserve(voxel_points.size());
00237 }
00238
00239 for (unsigned int i = 0; i < voxel_points.size(); i++)
00240 {
00241 PropDistanceFieldVoxel& voxel = voxel_grid_->getCell(voxel_points[i].x(), voxel_points[i].y(), voxel_points[i].z());
00242 const Eigen::Vector3i& loc = voxel_points[i];
00243 voxel.distance_square_ = 0;
00244 voxel.closest_point_ = loc;
00245 voxel.update_direction_ = initial_update_direction;
00246 bucket_queue_[0].push_back(loc);
00247 if (propagate_negative_)
00248 {
00249 voxel.negative_distance_square_ = max_distance_sq_;
00250 voxel.closest_negative_point_.x() = PropDistanceFieldVoxel::UNINITIALIZED;
00251 voxel.closest_negative_point_.y() = PropDistanceFieldVoxel::UNINITIALIZED;
00252 voxel.closest_negative_point_.z() = PropDistanceFieldVoxel::UNINITIALIZED;
00253 negative_stack.push_back(loc);
00254 }
00255 }
00256 propagatePositive();
00257
00258 if (propagate_negative_)
00259 {
00260 while (!negative_stack.empty())
00261 {
00262 Eigen::Vector3i loc = negative_stack.back();
00263 negative_stack.pop_back();
00264
00265 for (int neighbor = 0; neighbor < 27; neighbor++)
00266 {
00267 Eigen::Vector3i diff = getLocationDifference(neighbor);
00268 Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
00269
00270 if (isCellValid(nloc.x(), nloc.y(), nloc.z()))
00271 {
00272 PropDistanceFieldVoxel& nvoxel = voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z());
00273 Eigen::Vector3i& close_point = nvoxel.closest_negative_point_;
00274 if (!isCellValid(close_point.x(), close_point.y(), close_point.z()))
00275 {
00276 close_point = nloc;
00277 }
00278 PropDistanceFieldVoxel& closest_point_voxel =
00279 voxel_grid_->getCell(close_point.x(), close_point.y(), close_point.z());
00280
00281
00282 if (closest_point_voxel.negative_distance_square_ != 0)
00283 {
00284
00285
00286
00287
00288 if (nvoxel.negative_distance_square_ != max_distance_sq_)
00289 {
00290 nvoxel.negative_distance_square_ = max_distance_sq_;
00291 nvoxel.closest_negative_point_.x() = PropDistanceFieldVoxel::UNINITIALIZED;
00292 nvoxel.closest_negative_point_.y() = PropDistanceFieldVoxel::UNINITIALIZED;
00293 nvoxel.closest_negative_point_.z() = PropDistanceFieldVoxel::UNINITIALIZED;
00294 negative_stack.push_back(nloc);
00295 }
00296 }
00297 else
00298 {
00299
00300 nvoxel.negative_update_direction_ = initial_update_direction;
00301 negative_bucket_queue_[0].push_back(nloc);
00302 }
00303 }
00304 }
00305 }
00306 propagateNegative();
00307 }
00308 }
00309
00310 void PropagationDistanceField::removeObstacleVoxels(const std::vector<Eigen::Vector3i>& voxel_points)
00311
00312 {
00313 std::vector<Eigen::Vector3i> stack;
00314 std::vector<Eigen::Vector3i> negative_stack;
00315 int initial_update_direction = getDirectionNumber(0, 0, 0);
00316
00317 stack.reserve(getXNumCells() * getYNumCells() * getZNumCells());
00318 bucket_queue_[0].reserve(voxel_points.size());
00319 if (propagate_negative_)
00320 {
00321 negative_stack.reserve(getXNumCells() * getYNumCells() * getZNumCells());
00322 negative_bucket_queue_[0].reserve(voxel_points.size());
00323 }
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333 for (unsigned int i = 0; i < voxel_points.size(); i++)
00334 {
00335 PropDistanceFieldVoxel& voxel = voxel_grid_->getCell(voxel_points[i].x(), voxel_points[i].y(), voxel_points[i].z());
00336 voxel.distance_square_ = max_distance_sq_;
00337 voxel.closest_point_ = voxel_points[i];
00338 voxel.update_direction_ = initial_update_direction;
00339 stack.push_back(voxel_points[i]);
00340 if (propagate_negative_)
00341 {
00342 voxel.negative_distance_square_ = 0.0;
00343 voxel.closest_negative_point_ = voxel_points[i];
00344 voxel.negative_update_direction_ = initial_update_direction;
00345 negative_bucket_queue_[0].push_back(voxel_points[i]);
00346 }
00347 }
00348
00349
00350 while (!stack.empty())
00351 {
00352 Eigen::Vector3i loc = stack.back();
00353 stack.pop_back();
00354
00355 for (int neighbor = 0; neighbor < 27; neighbor++)
00356 {
00357 Eigen::Vector3i diff = getLocationDifference(neighbor);
00358 Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
00359
00360 if (isCellValid(nloc.x(), nloc.y(), nloc.z()))
00361 {
00362 PropDistanceFieldVoxel& nvoxel = voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z());
00363 Eigen::Vector3i& close_point = nvoxel.closest_point_;
00364 if (!isCellValid(close_point.x(), close_point.y(), close_point.z()))
00365 {
00366 close_point = nloc;
00367 }
00368 PropDistanceFieldVoxel& closest_point_voxel =
00369 voxel_grid_->getCell(close_point.x(), close_point.y(), close_point.z());
00370
00371 if (closest_point_voxel.distance_square_ != 0)
00372 {
00373 if (nvoxel.distance_square_ != max_distance_sq_)
00374 {
00375 nvoxel.distance_square_ = max_distance_sq_;
00376 nvoxel.closest_point_ = nloc;
00377 nvoxel.update_direction_ = initial_update_direction;
00378 stack.push_back(nloc);
00379 }
00380 }
00381 else
00382 {
00383 nvoxel.update_direction_ = initial_update_direction;
00384 bucket_queue_[0].push_back(nloc);
00385 }
00386 }
00387 }
00388 }
00389 propagatePositive();
00390
00391 if (propagate_negative_)
00392 {
00393 propagateNegative();
00394 }
00395 }
00396
00397 void PropagationDistanceField::propagatePositive()
00398 {
00399
00400 for (unsigned int i = 0; i < bucket_queue_.size(); ++i)
00401 {
00402 std::vector<Eigen::Vector3i>::iterator list_it = bucket_queue_[i].begin();
00403 std::vector<Eigen::Vector3i>::iterator list_end = bucket_queue_[i].end();
00404 for (; list_it != list_end; ++list_it)
00405 {
00406 const Eigen::Vector3i& loc = *list_it;
00407 PropDistanceFieldVoxel* vptr = &voxel_grid_->getCell(loc.x(), loc.y(), loc.z());
00408
00409
00410 std::vector<Eigen::Vector3i>* neighborhood;
00411 int D = i;
00412 if (D > 1)
00413 D = 1;
00414
00415
00416 if (vptr->update_direction_ < 0 || vptr->update_direction_ > 26)
00417 {
00418 logError("PROGRAMMING ERROR: Invalid update direction detected: %d", vptr->update_direction_);
00419 continue;
00420 }
00421
00422 neighborhood = &neighborhoods_[D][vptr->update_direction_];
00423
00424 for (unsigned int n = 0; n < neighborhood->size(); n++)
00425 {
00426 Eigen::Vector3i diff = (*neighborhood)[n];
00427 Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
00428 if (!isCellValid(nloc.x(), nloc.y(), nloc.z()))
00429 continue;
00430
00431
00432
00433 PropDistanceFieldVoxel* neighbor = &voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z());
00434 int new_distance_sq = eucDistSq(vptr->closest_point_, nloc);
00435 if (new_distance_sq > max_distance_sq_)
00436 continue;
00437
00438 if (new_distance_sq < neighbor->distance_square_)
00439 {
00440
00441 neighbor->distance_square_ = new_distance_sq;
00442 neighbor->closest_point_ = vptr->closest_point_;
00443 neighbor->update_direction_ = getDirectionNumber(diff.x(), diff.y(), diff.z());
00444
00445
00446 bucket_queue_[new_distance_sq].push_back(nloc);
00447 }
00448 }
00449 }
00450 bucket_queue_[i].clear();
00451 }
00452 }
00453
00454 void PropagationDistanceField::propagateNegative()
00455 {
00456
00457 for (unsigned int i = 0; i < negative_bucket_queue_.size(); ++i)
00458 {
00459 std::vector<Eigen::Vector3i>::iterator list_it = negative_bucket_queue_[i].begin();
00460 std::vector<Eigen::Vector3i>::iterator list_end = negative_bucket_queue_[i].end();
00461 for (; list_it != list_end; ++list_it)
00462 {
00463 const Eigen::Vector3i& loc = *list_it;
00464 PropDistanceFieldVoxel* vptr = &voxel_grid_->getCell(loc.x(), loc.y(), loc.z());
00465
00466
00467 std::vector<Eigen::Vector3i>* neighborhood;
00468 int D = i;
00469 if (D > 1)
00470 D = 1;
00471
00472
00473
00474 if (vptr->negative_update_direction_ < 0 || vptr->negative_update_direction_ > 26)
00475 {
00476 logError("PROGRAMMING ERROR: Invalid update direction detected: %d", vptr->update_direction_);
00477 continue;
00478 }
00479
00480 neighborhood = &neighborhoods_[D][vptr->negative_update_direction_];
00481
00482 for (unsigned int n = 0; n < neighborhood->size(); n++)
00483 {
00484 Eigen::Vector3i diff = (*neighborhood)[n];
00485 Eigen::Vector3i nloc(loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z());
00486 if (!isCellValid(nloc.x(), nloc.y(), nloc.z()))
00487 continue;
00488
00489
00490
00491 PropDistanceFieldVoxel* neighbor = &voxel_grid_->getCell(nloc.x(), nloc.y(), nloc.z());
00492 int new_distance_sq = eucDistSq(vptr->closest_negative_point_, nloc);
00493 if (new_distance_sq > max_distance_sq_)
00494 continue;
00495
00496
00497 if (new_distance_sq < neighbor->negative_distance_square_)
00498 {
00499
00500
00501
00502 neighbor->negative_distance_square_ = new_distance_sq;
00503 neighbor->closest_negative_point_ = vptr->closest_negative_point_;
00504 neighbor->negative_update_direction_ = getDirectionNumber(diff.x(), diff.y(), diff.z());
00505
00506
00507 negative_bucket_queue_[new_distance_sq].push_back(nloc);
00508 }
00509 }
00510 }
00511 negative_bucket_queue_[i].clear();
00512 }
00513 }
00514
00515 void PropagationDistanceField::reset()
00516 {
00517 voxel_grid_->reset(PropDistanceFieldVoxel(max_distance_sq_, 0));
00518 for (int x = 0; x < getXNumCells(); x++)
00519 {
00520 for (int y = 0; y < getYNumCells(); y++)
00521 {
00522 for (int z = 0; z < getZNumCells(); z++)
00523 {
00524 PropDistanceFieldVoxel& voxel = voxel_grid_->getCell(x, y, z);
00525 voxel.closest_negative_point_.x() = x;
00526 voxel.closest_negative_point_.y() = y;
00527 voxel.closest_negative_point_.z() = z;
00528 voxel.negative_distance_square_ = 0;
00529 }
00530 }
00531 }
00532
00533 }
00534
00535 void PropagationDistanceField::initNeighborhoods()
00536 {
00537
00538 direction_number_to_direction_.resize(27);
00539 for (int dx = -1; dx <= 1; ++dx)
00540 {
00541 for (int dy = -1; dy <= 1; ++dy)
00542 {
00543 for (int dz = -1; dz <= 1; ++dz)
00544 {
00545 int direction_number = getDirectionNumber(dx, dy, dz);
00546 Eigen::Vector3i n_point(dx, dy, dz);
00547 direction_number_to_direction_[direction_number] = n_point;
00548 }
00549 }
00550 }
00551
00552 neighborhoods_.resize(2);
00553 for (int n = 0; n < 2; n++)
00554 {
00555 neighborhoods_[n].resize(27);
00556
00557 for (int dx = -1; dx <= 1; ++dx)
00558 {
00559 for (int dy = -1; dy <= 1; ++dy)
00560 {
00561 for (int dz = -1; dz <= 1; ++dz)
00562 {
00563 int direction_number = getDirectionNumber(dx, dy, dz);
00564
00565 for (int tdx = -1; tdx <= 1; ++tdx)
00566 {
00567 for (int tdy = -1; tdy <= 1; ++tdy)
00568 {
00569 for (int tdz = -1; tdz <= 1; ++tdz)
00570 {
00571 if (tdx == 0 && tdy == 0 && tdz == 0)
00572 continue;
00573 if (n >= 1)
00574 {
00575 if ((abs(tdx) + abs(tdy) + abs(tdz)) != 1)
00576 continue;
00577 if (dx * tdx < 0 || dy * tdy < 0 || dz * tdz < 0)
00578 continue;
00579 }
00580 Eigen::Vector3i n_point(tdx, tdy, tdz);
00581 neighborhoods_[n][direction_number].push_back(n_point);
00582 }
00583 }
00584 }
00585
00586
00587 }
00588 }
00589 }
00590 }
00591 }
00592
00593 int PropagationDistanceField::getDirectionNumber(int dx, int dy, int dz) const
00594 {
00595 return (dx + 1) * 9 + (dy + 1) * 3 + dz + 1;
00596 }
00597
00598 Eigen::Vector3i PropagationDistanceField::getLocationDifference(int directionNumber) const
00599 {
00600 return direction_number_to_direction_[directionNumber];
00601 }
00602
00603 double PropagationDistanceField::getDistance(double x, double y, double z) const
00604 {
00605 return getDistance((*voxel_grid_.get())(x, y, z));
00606 }
00607
00608 double PropagationDistanceField::getDistance(int x, int y, int z) const
00609 {
00610 return getDistance(voxel_grid_->getCell(x, y, z));
00611 }
00612
00613 bool PropagationDistanceField::isCellValid(int x, int y, int z) const
00614 {
00615 return voxel_grid_->isCellValid(x, y, z);
00616 }
00617
00618 int PropagationDistanceField::getXNumCells() const
00619 {
00620 return voxel_grid_->getNumCells(DIM_X);
00621 }
00622
00623 int PropagationDistanceField::getYNumCells() const
00624 {
00625 return voxel_grid_->getNumCells(DIM_Y);
00626 }
00627
00628 int PropagationDistanceField::getZNumCells() const
00629 {
00630 return voxel_grid_->getNumCells(DIM_Z);
00631 }
00632
00633 bool PropagationDistanceField::gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const
00634 {
00635 voxel_grid_->gridToWorld(x, y, z, world_x, world_y, world_z);
00636 return true;
00637 }
00638
00639 bool PropagationDistanceField::worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const
00640 {
00641 return voxel_grid_->worldToGrid(world_x, world_y, world_z, x, y, z);
00642 }
00643
00644 bool PropagationDistanceField::writeToStream(std::ostream& os) const
00645 {
00646 os << "resolution: " << resolution_ << std::endl;
00647 os << "size_x: " << size_x_ << std::endl;
00648 os << "size_y: " << size_y_ << std::endl;
00649 os << "size_z: " << size_z_ << std::endl;
00650 os << "origin_x: " << origin_x_ << std::endl;
00651 os << "origin_y: " << origin_y_ << std::endl;
00652 os << "origin_z: " << origin_z_ << std::endl;
00653
00654
00655
00656 boost::iostreams::filtering_ostream out;
00657 out.push(boost::iostreams::zlib_compressor());
00658 out.push(os);
00659
00660 for (unsigned int x = 0; x < static_cast<unsigned int>(getXNumCells()); x++)
00661 {
00662 for (unsigned int y = 0; y < static_cast<unsigned int>(getYNumCells()); y++)
00663 {
00664 for (unsigned int z = 0; z < static_cast<unsigned int>(getZNumCells()); z += 8)
00665 {
00666 std::bitset<8> bs(0);
00667 unsigned int zv = std::min((unsigned int)8, getZNumCells() - z);
00668 for (unsigned int zi = 0; zi < zv; zi++)
00669 {
00670 if (getCell(x, y, z + zi).distance_square_ == 0)
00671 {
00672
00673 bs[zi] = 1;
00674 }
00675 }
00676 out.write((char*)&bs, sizeof(char));
00677 }
00678 }
00679 }
00680 out.flush();
00681 return true;
00682 }
00683
00684 bool PropagationDistanceField::readFromStream(std::istream& is)
00685 {
00686 if (!is.good())
00687 return false;
00688
00689 std::string temp;
00690
00691 is >> temp;
00692 if (temp != "resolution:")
00693 return false;
00694 is >> resolution_;
00695
00696 is >> temp;
00697 if (temp != "size_x:")
00698 return false;
00699 is >> size_x_;
00700
00701 is >> temp;
00702 if (temp != "size_y:")
00703 return false;
00704 is >> size_y_;
00705
00706 is >> temp;
00707 if (temp != "size_z:")
00708 return false;
00709 is >> size_z_;
00710
00711 is >> temp;
00712 if (temp != "origin_x:")
00713 return false;
00714 is >> origin_x_;
00715
00716 is >> temp;
00717 if (temp != "origin_y:")
00718 return false;
00719 is >> origin_y_;
00720
00721 is >> temp;
00722 if (temp != "origin_z:")
00723 return false;
00724 is >> origin_z_;
00725
00726
00727
00728 initialize();
00729
00730
00731 char nl;
00732 is.get(nl);
00733
00734
00735 boost::iostreams::filtering_istream in;
00736 in.push(boost::iostreams::zlib_decompressor());
00737 in.push(is);
00738
00739
00740
00741 std::vector<Eigen::Vector3i> obs_points;
00742 for (unsigned int x = 0; x < static_cast<unsigned int>(getXNumCells()); x++)
00743 {
00744 for (unsigned int y = 0; y < static_cast<unsigned int>(getYNumCells()); y++)
00745 {
00746 for (unsigned int z = 0; z < static_cast<unsigned int>(getZNumCells()); z += 8)
00747 {
00748 char inchar;
00749 if (!in.good())
00750 {
00751 return false;
00752 }
00753 in.get(inchar);
00754 std::bitset<8> inbit((unsigned long long)inchar);
00755 unsigned int zv = std::min((unsigned int)8, getZNumCells() - z);
00756 for (unsigned int zi = 0; zi < zv; zi++)
00757 {
00758 if (inbit[zi] == 1)
00759 {
00760
00761 obs_points.push_back(Eigen::Vector3i(x, y, z + zi));
00762 }
00763 }
00764 }
00765 }
00766 }
00767 addNewObstacleVoxels(obs_points);
00768 return true;
00769 }
00770 }