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