propagation_distance_field.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Mrinal Kalakrishnan, Ken Anderson */
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   // create a sqrt table:
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     // logInform("Adding obstacle voxel %d %d %d", (*it).x(), (*it).y(), (*it).z());
00170   }
00171 
00172   removeObstacleVoxels(old_not_new);
00173   addNewObstacleVoxels(new_not_in_current);
00174 
00175   // logDebug( "new=" );
00176   // print(points_added);
00177   // logDebug( "removed=" );
00178   // print(points_removed);
00179   // logDebug( "obstacle_voxel_locations_=" );
00180   // print(object_voxel_locations_);
00181   // logDebug("");
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     // Convert to voxel coordinates
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   // VoxelSet voxel_locs;
00209 
00210   for (unsigned int i = 0; i < points.size(); i++)
00211   {
00212     // Convert to voxel coordinates
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       // if(voxel_grid_->getCell(voxel_loc.x(),voxel_loc.y(),voxel_loc.z()).distance_square_ == 0) {
00220       //  voxel_locs.insert(voxel_loc);
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           // our closest non-obstacle cell has become an obstacle
00282           if (closest_point_voxel.negative_distance_square_ != 0)
00283           {
00284             // find all neigbors inside pre-existing obstacles whose
00285             // closest_negative_point_ is now an obstacle.  These must all be
00286             // set to max_distance_sq_ so they will be re-propogated with a new
00287             // closest_negative_point_ that is outside the obstacle.
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             // this cell still has a valid non-obstacle cell, so we need to propogate from it
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 // const VoxelSet& locations )
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   // First reset the obstacle voxels,
00326   // VoxelSet::const_iterator it = locations.begin();
00327   // for( it=locations.begin(); it!=locations.end(); ++it)
00328   // {
00329   //   Eigen::Vector3i loc = *it;
00330   //   bool valid = isCellValid( loc.x(), loc.y(), loc.z());
00331   //   if (!valid)
00332   //     continue;
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;  // not needed?
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   // Reset all neighbors who's closest point is now gone.
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         {  // closest point no longer exists
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;  // not needed?
00378             stack.push_back(nloc);
00379           }
00380         }
00381         else
00382         {  // add to queue so we can propagate the values
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   // now process the queue:
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       // select the neighborhood list based on the update direction:
00410       std::vector<Eigen::Vector3i>* neighborhood;
00411       int D = i;
00412       if (D > 1)
00413         D = 1;
00414 
00415       // This will never happen.  update_direction_ is always set before voxel is added to bucket queue.
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         // the real update code:
00432         // calculate the neighbor's new distance based on my closest filled voxel:
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           // update the neighboring voxel
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           // and put it in the queue:
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   // now process the queue:
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       // select the neighborhood list based on the update direction:
00467       std::vector<Eigen::Vector3i>* neighborhood;
00468       int D = i;
00469       if (D > 1)
00470         D = 1;
00471 
00472       // This will never happen.  negative_update_direction_ is always set before voxel is added to
00473       // negative_bucket_queue_.
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         // the real update code:
00490         // calculate the neighbor's new distance based on my closest filled voxel:
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         // std::cout << "Looking at " << nloc.x() << " " << nloc.y() << " " << nloc.z() << " " << new_distance_sq << " "
00496         // << neighbor->negative_distance_square_ << std::endl;
00497         if (new_distance_sq < neighbor->negative_distance_square_)
00498         {
00499           // std::cout << "Updating " << nloc.x() << " " << nloc.y() << " " << nloc.z() << " " << new_distance_sq <<
00500           // std::endl;
00501           // update the neighboring voxel
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           // and put it in the queue:
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   // object_voxel_locations_.clear();
00533 }
00534 
00535 void PropagationDistanceField::initNeighborhoods()
00536 {
00537   // first initialize the direction number mapping:
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     // source directions
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           // target directions:
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           // printf("n=%d, dx=%d, dy=%d, dz=%d, neighbors = %d\n", n, dx, dy, dz,
00586           // neighborhoods_[n][direction_number].size());
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   // now the binary stuff
00654 
00655   // first writing to zlib compressed buffer
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             // std::cout << "Marking obs cell " << x << " " << y << " " << z+zi << std::endl;
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   // previous values for propogation_negative_ and max_distance_ will be used
00727 
00728   initialize();
00729 
00730   // this should be newline
00731   char nl;
00732   is.get(nl);
00733 
00734   // now we start the compressed portion
00735   boost::iostreams::filtering_istream in;
00736   in.push(boost::iostreams::zlib_decompressor());
00737   in.push(is);
00738 
00739   // std::cout << "Nums " << getXNumCells() << " " << getYNumCells() << " " << getZNumCells() << std::endl;
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             // std::cout << "Adding obs cell " << x << " " << y << " " << z+zi << std::endl;
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 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49