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 the 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 
00037 #include <distance_field/propagation_distance_field.h>
00038 #include <visualization_msgs/Marker.h>
00039 
00040 namespace distance_field
00041 {
00042 
00043 PropagationDistanceField::~PropagationDistanceField()
00044 {
00045 }
00046 
00047 PropagationDistanceField::PropagationDistanceField(double size_x, double size_y, double size_z, double resolution,
00048     double origin_x, double origin_y, double origin_z, double max_distance):
00049       DistanceField<PropDistanceFieldVoxel>(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, PropDistanceFieldVoxel(max_distance))
00050 {
00051   max_distance_ = max_distance;
00052   int max_dist_int = ceil(max_distance_/resolution);
00053   max_distance_sq_ = (max_dist_int*max_dist_int);
00054   initNeighborhoods();
00055 
00056   bucket_queue_.resize(max_distance_sq_+1);
00057 
00058   // create a sqrt table:
00059   sqrt_table_.resize(max_distance_sq_+1);
00060   for (int i=0; i<=max_distance_sq_; ++i)
00061     sqrt_table_[i] = sqrt(double(i))*resolution;
00062 }
00063 
00064 int PropagationDistanceField::eucDistSq(int3 point1, int3 point2)
00065 {
00066   int dx = point1.x() - point2.x();
00067   int dy = point1.y() - point2.y();
00068   int dz = point1.z() - point2.z();
00069   return dx*dx + dy*dy + dz*dz;
00070 }
00071 
00072 void PropagationDistanceField::updatePointsInField(const std::vector<tf::Vector3>& points, bool iterative)
00073 {
00074   if( iterative )
00075   {
00076     VoxelSet points_added;
00077     VoxelSet points_removed(object_voxel_locations_);
00078 
00079     // Compare and figure out what points are new,
00080     // and what points are to be deleted
00081     for( unsigned int i=0; i<points.size(); i++)
00082     {
00083       // Convert to voxel coordinates
00084       int3 voxel_loc;
00085       bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(),
00086                                 voxel_loc.x(), voxel_loc.y(), voxel_loc.z() );
00087       if( valid )
00088       {
00089         if( iterative )
00090         {
00091           bool already_obstacle_voxel = ( object_voxel_locations_.find(voxel_loc) != object_voxel_locations_.end() );
00092           if( !already_obstacle_voxel )
00093           {
00094             // Not already in set of existing obstacles, so add to voxel list
00095             object_voxel_locations_.insert(voxel_loc);
00096 
00097             // Add point to the set or expansion
00098             points_added.insert(voxel_loc);
00099           }
00100           else
00101           {
00102             // Already an existing obstacle, so take off removal list
00103             points_removed.erase(voxel_loc);
00104           }
00105         }
00106         else
00107         {
00108           object_voxel_locations_.insert(voxel_loc);
00109           points_added.insert(voxel_loc);
00110         }
00111       }
00112     }
00113 
00114    removeObstacleVoxels( points_removed );
00115    addNewObstacleVoxels( points_added );
00116   }
00117 
00118   else  // !iterative
00119   {
00120     VoxelSet points_added;
00121     reset();
00122 
00123     for( unsigned int i=0; i<points.size(); i++)
00124     {
00125       // Convert to voxel coordinates
00126       int3 voxel_loc;
00127       bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(),
00128                                 voxel_loc.x(), voxel_loc.y(), voxel_loc.z() );
00129       if( valid )
00130       {
00131         object_voxel_locations_.insert(voxel_loc);
00132         points_added.insert(voxel_loc);
00133       }
00134     }
00135     addNewObstacleVoxels( points_added );
00136   }
00137 }
00138 
00139 void PropagationDistanceField::addPointsToField(const std::vector<tf::Vector3>& points)
00140 {
00141   VoxelSet voxel_locs;
00142 
00143   for( unsigned int i=0; i<points.size(); i++)
00144   {
00145     // Convert to voxel coordinates
00146     int3 voxel_loc;
00147     bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(),
00148                               voxel_loc.x(), voxel_loc.y(), voxel_loc.z() );
00149 
00150     if( valid )
00151     {
00152       bool already_obstacle_voxel = ( object_voxel_locations_.find(voxel_loc) != object_voxel_locations_.end() );
00153       if( !already_obstacle_voxel )
00154       {
00155         // Not already in set of existing obstacles, so add to voxel list
00156         object_voxel_locations_.insert(voxel_loc);
00157 
00158         // Add point to the queue for expansion
00159         voxel_locs.insert(voxel_loc);
00160       }
00161     }
00162   }
00163 
00164   addNewObstacleVoxels( voxel_locs );
00165 }
00166 
00167 void PropagationDistanceField::addNewObstacleVoxels(const VoxelSet& locations)
00168 {
00169   int x, y, z;
00170   int initial_update_direction = getDirectionNumber(0,0,0);
00171   bucket_queue_[0].reserve(locations.size());
00172 
00173   VoxelSet::const_iterator it = locations.begin();
00174   for( it=locations.begin(); it!=locations.end(); ++it)
00175   {
00176     int3 loc = *it;
00177     x = loc.x();
00178     y = loc.y();
00179     z = loc.z();
00180     bool valid = isCellValid( x, y, z);
00181     if (!valid)
00182       continue;
00183     PropDistanceFieldVoxel& voxel = getCell(x,y,z);
00184     voxel.distance_square_ = 0;
00185     voxel.closest_point_ = loc;
00186     voxel.location_ = loc;
00187     voxel.update_direction_ = initial_update_direction;
00188     bucket_queue_[0].push_back(&voxel);
00189   }
00190 
00191   propogate();
00192 }
00193 
00194 void PropagationDistanceField::removeObstacleVoxels(const VoxelSet& locations )
00195 {
00196   std::vector<int3> stack;
00197   int initial_update_direction = getDirectionNumber(0,0,0);
00198 
00199   stack.reserve( num_cells_[DIM_X] * num_cells_[DIM_Y] * num_cells_[DIM_Z] );
00200   bucket_queue_[0].reserve(locations.size());
00201 
00202   // First reset the obstacle voxels,
00203   VoxelSet::const_iterator it = locations.begin();
00204   for( it=locations.begin(); it!=locations.end(); ++it)
00205   {
00206     int3 loc = *it;
00207     bool valid = isCellValid( loc.x(), loc.y(), loc.z());
00208     if (!valid)
00209       continue;
00210     PropDistanceFieldVoxel& voxel = getCell(loc.x(), loc.y(), loc.z());
00211     voxel.distance_square_ = max_distance_sq_;
00212     voxel.closest_point_ = loc;
00213     voxel.location_ = loc;
00214     voxel.update_direction_ = initial_update_direction;
00215     stack.push_back(loc);
00216   }
00217 
00218   // Reset all neighbors who's closest point is now gone.
00219   while(stack.size() > 0)
00220   {
00221     int3 loc = stack.back();
00222     stack.pop_back();
00223 
00224     for( int neighbor=0; neighbor<27; neighbor++ )
00225     {
00226       int3 diff = getLocationDifference(neighbor);
00227       int3 nloc( loc.x() + diff.x(), loc.y() + diff.y(), loc.z() + diff.z() );
00228 
00229       if( isCellValid(nloc.x(), nloc.y(), nloc.z()) )
00230       {
00231         PropDistanceFieldVoxel& nvoxel = getCell(nloc.x(), nloc.y(), nloc.z());
00232         int3& close_point = nvoxel.closest_point_;
00233         PropDistanceFieldVoxel& closest_point_voxel = getCell( close_point.x(), close_point.y(), close_point.z() );
00234 
00235         if( closest_point_voxel.distance_square_ != 0 )
00236         {       // closest point no longer exists
00237           if( nvoxel.distance_square_!=max_distance_sq_)
00238           {
00239             nvoxel.distance_square_ = max_distance_sq_;
00240             nvoxel.closest_point_ = nloc;
00241             nvoxel.location_ = nloc;
00242             nvoxel.update_direction_ = initial_update_direction;
00243             stack.push_back(nloc);
00244           }
00245         }
00246         else
00247         {       // add to queue so we can propogate the values
00248           bucket_queue_[0].push_back(&nvoxel);
00249         }
00250       }
00251     }
00252   }
00253 
00254   propogate();
00255 }
00256 
00257 void PropagationDistanceField::propogate()
00258 {
00259   int x, y, z, nx, ny, nz;
00260   int3 loc;
00261 
00262   // now process the queue:
00263   for (unsigned int i=0; i<bucket_queue_.size(); ++i)
00264   {
00265     std::vector<PropDistanceFieldVoxel*>::iterator list_it = bucket_queue_[i].begin();
00266     while(list_it!=bucket_queue_[i].end())
00267     {
00268       PropDistanceFieldVoxel* vptr = *list_it;
00269 
00270       x = vptr->location_.x();
00271       y = vptr->location_.y();
00272       z = vptr->location_.z();
00273 
00274       // select the neighborhood list based on the update direction:
00275       std::vector<int3 >* neighborhood;
00276       int D = i;
00277       if (D>1)
00278         D=1;
00279       // avoid a possible segfault situation:
00280       if (vptr->update_direction_<0 || vptr->update_direction_>26)
00281       {
00282    //     ROS_WARN("Invalid update direction detected: %d", vptr->update_direction_);
00283         ++list_it;
00284         continue;
00285       }
00286 
00287       neighborhood = &neighborhoods_[D][vptr->update_direction_];
00288 
00289       for (unsigned int n=0; n<neighborhood->size(); n++)
00290       {
00291         int dx = (*neighborhood)[n].x();
00292         int dy = (*neighborhood)[n].y();
00293         int dz = (*neighborhood)[n].z();
00294         nx = x + dx;
00295         ny = y + dy;
00296         nz = z + dz;
00297         if (!isCellValid(nx,ny,nz))
00298           continue;
00299 
00300         // the real update code:
00301         // calculate the neighbor's new distance based on my closest filled voxel:
00302         PropDistanceFieldVoxel* neighbor = &getCell(nx, ny, nz);
00303         loc.x() = nx;
00304         loc.y() = ny;
00305         loc.z() = nz;
00306         int new_distance_sq = eucDistSq(vptr->closest_point_, loc);
00307         if (new_distance_sq > max_distance_sq_)
00308           continue;
00309         if (new_distance_sq < neighbor->distance_square_)
00310         {
00311           // update the neighboring voxel
00312           neighbor->distance_square_ = new_distance_sq;
00313           neighbor->closest_point_ = vptr->closest_point_;
00314           neighbor->location_ = loc;
00315           neighbor->update_direction_ = getDirectionNumber(dx, dy, dz);
00316 
00317           // and put it in the queue:
00318           bucket_queue_[new_distance_sq].push_back(neighbor);
00319         }
00320       }
00321 
00322       ++list_it;
00323     }
00324     bucket_queue_[i].clear();
00325   }
00326 
00327 }
00328 
00329 void PropagationDistanceField::getOccupiedVoxelMarkers(const std::string & frame_id, 
00330                                                        const ros::Time stamp,
00331                                                        const tf::Transform& cur,
00332                                                        visualization_msgs::Marker& inf_marker)
00333 {
00334   inf_marker.points.clear();
00335   inf_marker.header.frame_id = frame_id;
00336   inf_marker.header.stamp = stamp;
00337   inf_marker.ns = "distance_field_occupied_cells";
00338   inf_marker.id = 1;
00339   inf_marker.type = visualization_msgs::Marker::CUBE_LIST;
00340   inf_marker.action = 0;
00341   inf_marker.scale.x = resolution_[DIM_X];
00342   inf_marker.scale.y = resolution_[DIM_Y];
00343   inf_marker.scale.z = resolution_[DIM_Z];
00344   inf_marker.color.r = 0.0;
00345   inf_marker.color.g = 0.0;
00346   inf_marker.color.b = 1.0;
00347   inf_marker.color.a = 0.1;
00348   //inf_marker.lifetime = ros::Duration(30.0);
00349 
00350   inf_marker.points.reserve(100000);
00351 
00352   VoxelSet::iterator iter;
00353   for(iter = object_voxel_locations_.begin(); iter != object_voxel_locations_.end(); iter++)
00354   {
00355     int last = inf_marker.points.size();
00356     inf_marker.points.resize(last + 1);
00357     double nx, ny, nz;
00358     int x = (*iter).x();
00359     int y = (*iter).y();
00360     int z = (*iter).z();
00361     this->gridToWorld(x,y,z,nx, ny, nz);
00362     tf::Vector3 vec(nx,ny,nz);
00363     vec = cur*vec;
00364     inf_marker.points[last].x = vec.x();
00365     inf_marker.points[last].y = vec.y();
00366     inf_marker.points[last].z = vec.z();    
00367   }
00368 }
00369 
00370 
00371 void PropagationDistanceField::reset()
00372 {
00373   VoxelGrid<PropDistanceFieldVoxel>::reset(PropDistanceFieldVoxel(max_distance_sq_));
00374 }
00375 
00376 void PropagationDistanceField::initNeighborhoods()
00377 {
00378   // first initialize the direction number mapping:
00379   direction_number_to_direction_.resize(27);
00380   for (int dx=-1; dx<=1; ++dx)
00381   {
00382     for (int dy=-1; dy<=1; ++dy)
00383     {
00384       for (int dz=-1; dz<=1; ++dz)
00385       {
00386         int direction_number = getDirectionNumber(dx, dy, dz);
00387         int3 n_point( dx, dy, dz);
00388         direction_number_to_direction_[direction_number] = n_point;
00389       }
00390     }
00391   }
00392 
00393   neighborhoods_.resize(2);
00394   for (int n=0; n<2; n++)
00395   {
00396     neighborhoods_[n].resize(27);
00397     // source directions
00398     for (int dx=-1; dx<=1; ++dx)
00399     {
00400       for (int dy=-1; dy<=1; ++dy)
00401       {
00402         for (int dz=-1; dz<=1; ++dz)
00403         {
00404           int direction_number = getDirectionNumber(dx, dy, dz);
00405           // target directions:
00406           for (int tdx=-1; tdx<=1; ++tdx)
00407           {
00408             for (int tdy=-1; tdy<=1; ++tdy)
00409             {
00410               for (int tdz=-1; tdz<=1; ++tdz)
00411               {
00412                 if (tdx==0 && tdy==0 && tdz==0)
00413                   continue;
00414                 if (n>=1)
00415                 {
00416                   if ((abs(tdx) + abs(tdy) + abs(tdz))!=1)
00417                     continue;
00418                   if (dx*tdx<0 || dy*tdy<0 || dz*tdz <0)
00419                     continue;
00420                 }
00421                 int3 n_point(tdx,tdy,tdz);
00422                 neighborhoods_[n][direction_number].push_back(n_point);
00423               }
00424             }
00425           }
00426           //printf("n=%d, dx=%d, dy=%d, dz=%d, neighbors = %d\n", n, dx, dy, dz, neighborhoods_[n][direction_number].size());
00427         }
00428       }
00429     }
00430   }
00431 
00432 }
00433 
00434 int PropagationDistanceField::getDirectionNumber(int dx, int dy, int dz) const
00435 {
00436   return (dx+1)*9 + (dy+1)*3 + dz+1;
00437 }
00438 
00439 int3 PropagationDistanceField::getLocationDifference(int directionNumber) const
00440 {
00441   return direction_number_to_direction_[ directionNumber ];
00442 }
00443 
00444 SignedPropagationDistanceField::~SignedPropagationDistanceField()
00445 {
00446 }
00447 
00448 SignedPropagationDistanceField::SignedPropagationDistanceField(double size_x, double size_y, double size_z, double resolution,
00449     double origin_x, double origin_y, double origin_z, double max_distance):
00450       DistanceField<SignedPropDistanceFieldVoxel>(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, SignedPropDistanceFieldVoxel(max_distance,0))
00451 {
00452   max_distance_ = max_distance;
00453   int max_dist_int = ceil(max_distance_/resolution);
00454   max_distance_sq_ = (max_dist_int*max_dist_int);
00455   initNeighborhoods();
00456 
00457   // create a sqrt table:
00458   sqrt_table_.resize(max_distance_sq_+1);
00459   for (int i=0; i<=max_distance_sq_; ++i)
00460     sqrt_table_[i] = sqrt(double(i))*resolution;
00461 }
00462 
00463 int SignedPropagationDistanceField::eucDistSq(int3 point1, int3 point2)
00464 {
00465   int dx = point1.x() - point2.x();
00466   int dy = point1.y() - point2.y();
00467   int dz = point1.z() - point2.z();
00468   return dx*dx + dy*dy + dz*dz;
00469 }
00470 
00471 void SignedPropagationDistanceField::addPointsToField(const std::vector<tf::Vector3>& points)
00472 {
00473   // initialize the bucket queue
00474   positive_bucket_queue_.resize(max_distance_sq_+1);
00475   negative_bucket_queue_.resize(max_distance_sq_+1);
00476 
00477   positive_bucket_queue_[0].reserve(points.size());
00478   negative_bucket_queue_[0].reserve(points.size());
00479 
00480   for(int x = 0; x < num_cells_[0]; x++)
00481   {
00482     for(int y = 0; y < num_cells_[1]; y++)
00483     {
00484       for(int z = 0; z < num_cells_[2]; z++)
00485       {
00486         SignedPropDistanceFieldVoxel& voxel = getCell(x,y,z);
00487         voxel.closest_negative_point_.x() = x;
00488         voxel.closest_negative_point_.y() = y;
00489         voxel.closest_negative_point_.z() = z;
00490         voxel.negative_distance_square_ = 0;
00491       }
00492     }
00493   }
00494 
00495   // first mark all the points as distance=0, and add them to the queue
00496   int x, y, z, nx, ny, nz;
00497   int3 loc;
00498   int initial_update_direction = getDirectionNumber(0,0,0);
00499   for (unsigned int i=0; i<points.size(); ++i)
00500   {
00501     bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(), x, y, z);
00502     if (!valid)
00503       continue;
00504     SignedPropDistanceFieldVoxel& voxel = getCell(x,y,z);
00505     voxel.positive_distance_square_ = 0;
00506     voxel.negative_distance_square_ = max_distance_sq_;
00507     voxel.closest_positive_point_.x() = x;
00508     voxel.closest_positive_point_.y() = y;
00509     voxel.closest_positive_point_.z() = z;
00510     voxel.closest_negative_point_.x() = SignedPropDistanceFieldVoxel::UNINITIALIZED;
00511     voxel.closest_negative_point_.y() = SignedPropDistanceFieldVoxel::UNINITIALIZED;
00512     voxel.closest_negative_point_.z() = SignedPropDistanceFieldVoxel::UNINITIALIZED;
00513     voxel.location_.x() = x;
00514     voxel.location_.y() = y;
00515     voxel.location_.z() = z;
00516     voxel.update_direction_ = initial_update_direction;
00517     positive_bucket_queue_[0].push_back(&voxel);
00518   }
00519 
00520   // now process the queue:
00521   for (unsigned int i=0; i<positive_bucket_queue_.size(); ++i)
00522   {
00523     std::vector<SignedPropDistanceFieldVoxel*>::iterator list_it = positive_bucket_queue_[i].begin();
00524     while(list_it!=positive_bucket_queue_[i].end())
00525     {
00526       SignedPropDistanceFieldVoxel* vptr = *list_it;
00527 
00528       x = vptr->location_.x();
00529       y = vptr->location_.y();
00530       z = vptr->location_.z();
00531 
00532       // select the neighborhood list based on the update direction:
00533       std::vector<int3 >* neighborhood;
00534       int D = i;
00535       if (D>1)
00536         D=1;
00537       // avoid a possible segfault situation:
00538       if (vptr->update_direction_<0 || vptr->update_direction_>26)
00539       {
00540    //     ROS_WARN("Invalid update direction detected: %d", vptr->update_direction_);
00541         ++list_it;
00542         continue;
00543       }
00544 
00545       neighborhood = &neighborhoods_[D][vptr->update_direction_];
00546 
00547       for (unsigned int n=0; n<neighborhood->size(); n++)
00548       {
00549         int dx = (*neighborhood)[n].x();
00550         int dy = (*neighborhood)[n].y();
00551         int dz = (*neighborhood)[n].z();
00552         nx = x + dx;
00553         ny = y + dy;
00554         nz = z + dz;
00555         if (!isCellValid(nx,ny,nz))
00556           continue;
00557 
00558         // the real update code:
00559         // calculate the neighbor's new distance based on my closest filled voxel:
00560         SignedPropDistanceFieldVoxel* neighbor = &getCell(nx, ny, nz);
00561         loc.x() = nx;
00562         loc.y() = ny;
00563         loc.z() = nz;
00564         int new_distance_sq = eucDistSq(vptr->closest_positive_point_, loc);
00565         if (new_distance_sq > max_distance_sq_)
00566           continue;
00567         if (new_distance_sq < neighbor->positive_distance_square_)
00568         {
00569           // update the neighboring voxel
00570           neighbor->positive_distance_square_ = new_distance_sq;
00571           neighbor->closest_positive_point_ = vptr->closest_positive_point_;
00572           neighbor->location_ = loc;
00573           neighbor->update_direction_ = getDirectionNumber(dx, dy, dz);
00574 
00575           // and put it in the queue:
00576           positive_bucket_queue_[new_distance_sq].push_back(neighbor);
00577         }
00578       }
00579 
00580       ++list_it;
00581     }
00582     positive_bucket_queue_[i].clear();
00583   }
00584 
00585 
00586   for(unsigned int i = 0; i < points.size(); i++)
00587     {
00588       bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(), x, y, z);
00589       if(!valid)
00590         continue;
00591 
00592       for(int dx = -1; dx <= 1; dx ++)
00593       {
00594         for(int dy = -1; dy<= 1; dy ++)
00595         {
00596           for(int dz = -1; dz <= 1; dz++)
00597           {
00598             nx = x + dx;
00599             ny = y + dy;
00600             nz = z + dz;
00601 
00602             if(!isCellValid(nx, ny, nz))
00603               continue;
00604 
00605             SignedPropDistanceFieldVoxel* neighbor = &getCell(nx, ny, nz);
00606 
00607             if(neighbor->closest_negative_point_.x() != SignedPropDistanceFieldVoxel::UNINITIALIZED)
00608             {
00609               neighbor->update_direction_ = initial_update_direction;
00610               negative_bucket_queue_[0].push_back(neighbor);
00611             }
00612           }
00613         }
00614       }
00615 
00616     }
00617 
00618   for (unsigned int i=0; i<negative_bucket_queue_.size(); ++i)
00619   {
00620     std::vector<SignedPropDistanceFieldVoxel*>::iterator list_it = negative_bucket_queue_[i].begin();
00621     while(list_it!=negative_bucket_queue_[i].end())
00622     {
00623       SignedPropDistanceFieldVoxel* vptr = *list_it;
00624 
00625       x = vptr->location_.x();
00626       y = vptr->location_.y();
00627       z = vptr->location_.z();
00628 
00629       // select the neighborhood list based on the update direction:
00630       std::vector<int3 >* neighborhood;
00631       int D = i;
00632       if (D>1)
00633         D=1;
00634       // avoid a possible segfault situation:
00635       if (vptr->update_direction_<0 || vptr->update_direction_>26)
00636       {
00637    //     ROS_WARN("Invalid update direction detected: %d", vptr->update_direction_);
00638         ++list_it;
00639         continue;
00640       }
00641 
00642       neighborhood = &neighborhoods_[D][vptr->update_direction_];
00643 
00644       for (unsigned int n=0; n<neighborhood->size(); n++)
00645       {
00646         int dx = (*neighborhood)[n].x();
00647         int dy = (*neighborhood)[n].y();
00648         int dz = (*neighborhood)[n].z();
00649         nx = x + dx;
00650         ny = y + dy;
00651         nz = z + dz;
00652         if (!isCellValid(nx,ny,nz))
00653           continue;
00654 
00655         // the real update code:
00656         // calculate the neighbor's new distance based on my closest filled voxel:
00657         SignedPropDistanceFieldVoxel* neighbor = &getCell(nx, ny, nz);
00658         loc.x() = nx;
00659         loc.y() = ny;
00660         loc.z() = nz;
00661         int new_distance_sq = eucDistSq(vptr->closest_negative_point_, loc);
00662         if (new_distance_sq > max_distance_sq_)
00663           continue;
00664         if (new_distance_sq < neighbor->negative_distance_square_)
00665         {
00666           // update the neighboring voxel
00667           neighbor->negative_distance_square_ = new_distance_sq;
00668           neighbor->closest_negative_point_ = vptr->closest_negative_point_;
00669           neighbor->location_ = loc;
00670           neighbor->update_direction_ = getDirectionNumber(dx, dy, dz);
00671 
00672           // and put it in the queue:
00673           negative_bucket_queue_[new_distance_sq].push_back(neighbor);
00674         }
00675       }
00676 
00677       ++list_it;
00678     }
00679     negative_bucket_queue_[i].clear();
00680   }
00681 
00682 }
00683 
00684 void SignedPropagationDistanceField::reset()
00685 {
00686   VoxelGrid<SignedPropDistanceFieldVoxel>::reset(SignedPropDistanceFieldVoxel(max_distance_sq_, 0));
00687 }
00688 
00689 void SignedPropagationDistanceField::initNeighborhoods()
00690 {
00691   // first initialize the direction number mapping:
00692   direction_number_to_direction_.resize(27);
00693   for (int dx=-1; dx<=1; ++dx)
00694   {
00695     for (int dy=-1; dy<=1; ++dy)
00696     {
00697       for (int dz=-1; dz<=1; ++dz)
00698       {
00699         int direction_number = getDirectionNumber(dx, dy, dz);
00700         int3 n_point( dx, dy, dz);
00701         direction_number_to_direction_[direction_number] = n_point;
00702       }
00703     }
00704   }
00705 
00706   neighborhoods_.resize(2);
00707   for (int n=0; n<2; n++)
00708   {
00709     neighborhoods_[n].resize(27);
00710     // source directions
00711     for (int dx=-1; dx<=1; ++dx)
00712     {
00713       for (int dy=-1; dy<=1; ++dy)
00714       {
00715         for (int dz=-1; dz<=1; ++dz)
00716         {
00717           int direction_number = getDirectionNumber(dx, dy, dz);
00718           // target directions:
00719           for (int tdx=-1; tdx<=1; ++tdx)
00720           {
00721             for (int tdy=-1; tdy<=1; ++tdy)
00722             {
00723               for (int tdz=-1; tdz<=1; ++tdz)
00724               {
00725                 if (tdx==0 && tdy==0 && tdz==0)
00726                   continue;
00727                 if (n>=1)
00728                 {
00729                   if ((abs(tdx) + abs(tdy) + abs(tdz))!=1)
00730                     continue;
00731                   if (dx*tdx<0 || dy*tdy<0 || dz*tdz <0)
00732                     continue;
00733                 }
00734                 int3 n_point(tdx,tdy,tdz);
00735                 neighborhoods_[n][direction_number].push_back(n_point);
00736               }
00737             }
00738           }
00739           //printf("n=%d, dx=%d, dy=%d, dz=%d, neighbors = %d\n", n, dx, dy, dz, neighborhoods_[n][direction_number].size());
00740         }
00741       }
00742     }
00743   }
00744 
00745 
00746 
00747 }
00748 
00749 int SignedPropagationDistanceField::getDirectionNumber(int dx, int dy, int dz) const
00750 {
00751   return (dx+1)*9 + (dy+1)*3 + dz+1;
00752 }
00753 
00754 
00755 }


distance_field
Author(s): Mrinal Kalakrishnan / mail@mrinal.net
autogenerated on Thu Dec 12 2013 11:08:36