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 
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   // create a sqrt table:
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     //logInform("Adding obstacle voxel %d %d %d", (*it).x(), (*it).y(), (*it).z());
00186   }
00187 
00188   removeObstacleVoxels(old_not_new);
00189   addNewObstacleVoxels(new_not_in_current);
00190 
00191   // logDebug( "new=" );
00192   // print(points_added);
00193   // logDebug( "removed=" );
00194   // print(points_removed);
00195   // logDebug( "obstacle_voxel_locations_=" );
00196   // print(object_voxel_locations_);
00197   // logDebug("");
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     // Convert to voxel coordinates
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   //VoxelSet voxel_locs;
00225 
00226   for( unsigned int i=0; i<points.size(); i++)
00227   {
00228     // Convert to voxel coordinates
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       //if(voxel_grid_->getCell(voxel_loc.x(),voxel_loc.y(),voxel_loc.z()).distance_square_ == 0) {
00237       //  voxel_locs.insert(voxel_loc);
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           //our closest non-obstacle cell has become an obstacle
00294           if( closest_point_voxel.negative_distance_square_ != 0 )
00295           {
00296             // find all neigbors inside pre-existing obstacles whose
00297             // closest_negative_point_ is now an obstacle.  These must all be
00298             // set to max_distance_sq_ so they will be re-propogated with a new
00299             // closest_negative_point_ that is outside the obstacle.
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             //this cell still has a valid non-obstacle cell, so we need to propogate from it
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 //const VoxelSet& locations )
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   // First reset the obstacle voxels,
00337   // VoxelSet::const_iterator it = locations.begin();
00338   // for( it=locations.begin(); it!=locations.end(); ++it)
00339   // {
00340   //   Eigen::Vector3i loc = *it;
00341   //   bool valid = isCellValid( loc.x(), loc.y(), loc.z());
00342   //   if (!valid)
00343   //     continue;
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; // not needed?
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   // Reset all neighbors who's closest point is now gone.
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         {       // closest point no longer exists
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;  // not needed?
00386             stack.push_back(nloc);
00387           }
00388         }
00389         else
00390         {       // add to queue so we can propagate the values
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   // now process the queue:
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       // select the neighborhood list based on the update direction:
00418       std::vector<Eigen::Vector3i >* neighborhood;
00419       int D = i;
00420       if (D>1)
00421         D=1;
00422 
00423       // This will never happen.  update_direction_ is always set before voxel is added to bucket queue.
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         // the real update code:
00440         // calculate the neighbor's new distance based on my closest filled voxel:
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           // update the neighboring voxel
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           // and put it in the queue:
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   // now process the queue:
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       // select the neighborhood list based on the update direction:
00477       std::vector<Eigen::Vector3i >* neighborhood;
00478       int D = i;
00479       if (D>1)
00480         D=1;
00481 
00482       // This will never happen.  negative_update_direction_ is always set before voxel is added to negative_bucket_queue_.
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         // the real update code:
00499         // calculate the neighbor's new distance based on my closest filled voxel:
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         //std::cout << "Looking at " << nloc.x() << " " << nloc.y() << " " << nloc.z() << " " << new_distance_sq << " " << neighbor->negative_distance_square_ << std::endl;
00505         if (new_distance_sq < neighbor->negative_distance_square_)
00506         {
00507           //std::cout << "Updating " << nloc.x() << " " << nloc.y() << " " << nloc.z() << " " << new_distance_sq << std::endl;
00508           // update the neighboring voxel
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           // and put it in the queue:
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   //object_voxel_locations_.clear();
00540 }
00541 
00542 void PropagationDistanceField::initNeighborhoods()
00543 {
00544   // first initialize the direction number mapping:
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     // source directions
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           // target directions:
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           //printf("n=%d, dx=%d, dy=%d, dz=%d, neighbors = %d\n", n, dx, dy, dz, neighborhoods_[n][direction_number].size());
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   //now the binary stuff
00660 
00661   //first writing to zlib compressed buffer
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             //std::cout << "Marking obs cell " << x << " " << y << " " << z+zi << std::endl;
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   //previous values for propogation_negative_ and max_distance_ will be used
00720 
00721   initialize();
00722 
00723   //this should be newline
00724   char nl;
00725   is.get(nl);
00726 
00727   //now we start the compressed portion
00728   boost::iostreams::filtering_istream in;
00729   in.push(boost::iostreams::zlib_decompressor());
00730   in.push(is);
00731 
00732   //std::cout << "Nums " << getXNumCells() << " " << getYNumCells() << " " << getZNumCells() << std::endl;
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             //std::cout << "Adding obs cell " << x << " " << y << " " << z+zi << std::endl;
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 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:53