$search
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 // create a sqrt table: 00057 sqrt_table_.resize(max_distance_sq_+1); 00058 for (int i=0; i<=max_distance_sq_; ++i) 00059 sqrt_table_[i] = sqrt(double(i))*resolution; 00060 } 00061 00062 int PropagationDistanceField::eucDistSq(int* point1, int* point2) 00063 { 00064 int dx = point1[DIM_X] - point2[DIM_X]; 00065 int dy = point1[DIM_Y] - point2[DIM_Y]; 00066 int dz = point1[DIM_Z] - point2[DIM_Z]; 00067 return dx*dx + dy*dy + dz*dz; 00068 } 00069 00070 void PropagationDistanceField::addPointsToField(const std::vector<btVector3> points) 00071 { 00072 // initialize the bucket queue 00073 bucket_queue_.resize(max_distance_sq_+1); 00074 00075 bucket_queue_[0].reserve(points.size()); 00076 // first mark all the points as distance=0, and add them to the queue 00077 int x, y, z, nx, ny, nz; 00078 int loc[3]; 00079 int initial_update_direction = getDirectionNumber(0,0,0); 00080 for (unsigned int i=0; i<points.size(); ++i) 00081 { 00082 bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(), x, y, z); 00083 if (!valid) 00084 continue; 00085 PropDistanceFieldVoxel& voxel = getCell(x,y,z); 00086 voxel.distance_square_ = 0; 00087 voxel.closest_point_[DIM_X] = x; 00088 voxel.closest_point_[DIM_Y] = y; 00089 voxel.closest_point_[DIM_Z] = z; 00090 voxel.location_[DIM_X] = x; 00091 voxel.location_[DIM_Y] = y; 00092 voxel.location_[DIM_Z] = z; 00093 voxel.update_direction_ = initial_update_direction; 00094 bucket_queue_[0].push_back(&voxel); 00095 } 00096 00097 // now process the queue: 00098 for (unsigned int i=0; i<bucket_queue_.size(); ++i) 00099 { 00100 std::vector<PropDistanceFieldVoxel*>::iterator list_it = bucket_queue_[i].begin(); 00101 while(list_it!=bucket_queue_[i].end()) 00102 { 00103 PropDistanceFieldVoxel* vptr = *list_it; 00104 00105 x = vptr->location_[DIM_X]; 00106 y = vptr->location_[DIM_Y]; 00107 z = vptr->location_[DIM_Z]; 00108 00109 // select the neighborhood list based on the update direction: 00110 std::vector<std::vector<int> >* neighborhood; 00111 int D = i; 00112 if (D>1) 00113 D=1; 00114 // avoid a possible segfault situation: 00115 if (vptr->update_direction_<0 || vptr->update_direction_>26) 00116 { 00117 // ROS_WARN("Invalid update direction detected: %d", vptr->update_direction_); 00118 ++list_it; 00119 continue; 00120 } 00121 00122 neighborhood = &neighborhoods_[D][vptr->update_direction_]; 00123 00124 for (unsigned int n=0; n<neighborhood->size(); n++) 00125 { 00126 int dx = (*neighborhood)[n][DIM_X]; 00127 int dy = (*neighborhood)[n][DIM_Y]; 00128 int dz = (*neighborhood)[n][DIM_Z]; 00129 nx = x + dx; 00130 ny = y + dy; 00131 nz = z + dz; 00132 if (!isCellValid(nx,ny,nz)) 00133 continue; 00134 00135 // the real update code: 00136 // calculate the neighbor's new distance based on my closest filled voxel: 00137 PropDistanceFieldVoxel* neighbor = &getCell(nx, ny, nz); 00138 loc[DIM_X] = nx; 00139 loc[DIM_Y] = ny; 00140 loc[DIM_Z] = nz; 00141 int new_distance_sq = eucDistSq(vptr->closest_point_, loc); 00142 if (new_distance_sq > max_distance_sq_) 00143 continue; 00144 if (new_distance_sq < neighbor->distance_square_) 00145 { 00146 // update the neighboring voxel 00147 neighbor->distance_square_ = new_distance_sq; 00148 neighbor->closest_point_[DIM_X] = vptr->closest_point_[DIM_X]; 00149 neighbor->closest_point_[DIM_Y] = vptr->closest_point_[DIM_Y]; 00150 neighbor->closest_point_[DIM_Z] = vptr->closest_point_[DIM_Z]; 00151 neighbor->location_[DIM_X] = loc[DIM_X]; 00152 neighbor->location_[DIM_Y] = loc[DIM_Y]; 00153 neighbor->location_[DIM_Z] = loc[DIM_Z]; 00154 neighbor->update_direction_ = getDirectionNumber(dx, dy, dz); 00155 00156 // and put it in the queue: 00157 bucket_queue_[new_distance_sq].push_back(neighbor); 00158 } 00159 } 00160 00161 ++list_it; 00162 } 00163 bucket_queue_[i].clear(); 00164 } 00165 00166 } 00167 00168 void PropagationDistanceField::reset() 00169 { 00170 VoxelGrid<PropDistanceFieldVoxel>::reset(PropDistanceFieldVoxel(max_distance_sq_)); 00171 } 00172 00173 void PropagationDistanceField::initNeighborhoods() 00174 { 00175 // first initialize the direction number mapping: 00176 direction_number_to_direction_.resize(27); 00177 for (int dx=-1; dx<=1; ++dx) 00178 { 00179 for (int dy=-1; dy<=1; ++dy) 00180 { 00181 for (int dz=-1; dz<=1; ++dz) 00182 { 00183 int direction_number = getDirectionNumber(dx, dy, dz); 00184 std::vector<int> n_point(3); 00185 n_point[DIM_X] = dx; 00186 n_point[DIM_Y] = dy; 00187 n_point[DIM_Z] = dz; 00188 direction_number_to_direction_[direction_number] = n_point; 00189 } 00190 } 00191 } 00192 00193 neighborhoods_.resize(2); 00194 for (int n=0; n<2; n++) 00195 { 00196 neighborhoods_[n].resize(27); 00197 // source directions 00198 for (int dx=-1; dx<=1; ++dx) 00199 { 00200 for (int dy=-1; dy<=1; ++dy) 00201 { 00202 for (int dz=-1; dz<=1; ++dz) 00203 { 00204 int direction_number = getDirectionNumber(dx, dy, dz); 00205 // target directions: 00206 for (int tdx=-1; tdx<=1; ++tdx) 00207 { 00208 for (int tdy=-1; tdy<=1; ++tdy) 00209 { 00210 for (int tdz=-1; tdz<=1; ++tdz) 00211 { 00212 if (tdx==0 && tdy==0 && tdz==0) 00213 continue; 00214 if (n>=1) 00215 { 00216 if ((abs(tdx) + abs(tdy) + abs(tdz))!=1) 00217 continue; 00218 if (dx*tdx<0 || dy*tdy<0 || dz*tdz <0) 00219 continue; 00220 } 00221 std::vector<int> n_point(3); 00222 n_point[DIM_X] = tdx; 00223 n_point[DIM_Y] = tdy; 00224 n_point[DIM_Z] = tdz; 00225 neighborhoods_[n][direction_number].push_back(n_point); 00226 } 00227 } 00228 } 00229 //printf("n=%d, dx=%d, dy=%d, dz=%d, neighbors = %d\n", n, dx, dy, dz, neighborhoods_[n][direction_number].size()); 00230 } 00231 } 00232 } 00233 } 00234 00235 } 00236 00237 int PropagationDistanceField::getDirectionNumber(int dx, int dy, int dz) const 00238 { 00239 return (dx+1)*9 + (dy+1)*3 + dz+1; 00240 } 00241 00242 SignedPropagationDistanceField::~SignedPropagationDistanceField() 00243 { 00244 } 00245 00246 SignedPropagationDistanceField::SignedPropagationDistanceField(double size_x, double size_y, double size_z, double resolution, 00247 double origin_x, double origin_y, double origin_z, double max_distance): 00248 DistanceField<SignedPropDistanceFieldVoxel>(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, SignedPropDistanceFieldVoxel(max_distance,0)) 00249 { 00250 max_distance_ = max_distance; 00251 int max_dist_int = ceil(max_distance_/resolution); 00252 max_distance_sq_ = (max_dist_int*max_dist_int); 00253 initNeighborhoods(); 00254 00255 // create a sqrt table: 00256 sqrt_table_.resize(max_distance_sq_+1); 00257 for (int i=0; i<=max_distance_sq_; ++i) 00258 sqrt_table_[i] = sqrt(double(i))*resolution; 00259 } 00260 00261 int SignedPropagationDistanceField::eucDistSq(int* point1, int* point2) 00262 { 00263 int dx = point1[DIM_X] - point2[DIM_X]; 00264 int dy = point1[DIM_Y] - point2[DIM_Y]; 00265 int dz = point1[DIM_Z] - point2[DIM_Z]; 00266 return dx*dx + dy*dy + dz*dz; 00267 } 00268 00269 void SignedPropagationDistanceField::addPointsToField(const std::vector<btVector3> points) 00270 { 00271 // initialize the bucket queue 00272 positive_bucket_queue_.resize(max_distance_sq_+1); 00273 negative_bucket_queue_.resize(max_distance_sq_+1); 00274 00275 positive_bucket_queue_[0].reserve(points.size()); 00276 negative_bucket_queue_[0].reserve(points.size()); 00277 00278 for(int x = 0; x < num_cells_[0]; x++) 00279 { 00280 for(int y = 0; y < num_cells_[1]; y++) 00281 { 00282 for(int z = 0; z < num_cells_[2]; z++) 00283 { 00284 SignedPropDistanceFieldVoxel& voxel = getCell(x,y,z); 00285 voxel.closest_negative_point_[DIM_X] = x; 00286 voxel.closest_negative_point_[DIM_Y] = y; 00287 voxel.closest_negative_point_[DIM_Z] = z; 00288 voxel.negative_distance_square_ = 0; 00289 } 00290 } 00291 } 00292 00293 // first mark all the points as distance=0, and add them to the queue 00294 int x, y, z, nx, ny, nz; 00295 int loc[3]; 00296 int initial_update_direction = getDirectionNumber(0,0,0); 00297 for (unsigned int i=0; i<points.size(); ++i) 00298 { 00299 bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(), x, y, z); 00300 if (!valid) 00301 continue; 00302 SignedPropDistanceFieldVoxel& voxel = getCell(x,y,z); 00303 voxel.positive_distance_square_ = 0; 00304 voxel.negative_distance_square_ = max_distance_sq_; 00305 voxel.closest_positive_point_[DIM_X] = x; 00306 voxel.closest_positive_point_[DIM_Y] = y; 00307 voxel.closest_positive_point_[DIM_Z] = z; 00308 voxel.closest_negative_point_[DIM_X] = SignedPropDistanceFieldVoxel::UNINITIALIZED; 00309 voxel.closest_negative_point_[DIM_Y] = SignedPropDistanceFieldVoxel::UNINITIALIZED; 00310 voxel.closest_negative_point_[DIM_Z] = SignedPropDistanceFieldVoxel::UNINITIALIZED; 00311 voxel.location_[DIM_X] = x; 00312 voxel.location_[DIM_Y] = y; 00313 voxel.location_[DIM_Z] = z; 00314 voxel.update_direction_ = initial_update_direction; 00315 positive_bucket_queue_[0].push_back(&voxel); 00316 } 00317 00318 // now process the queue: 00319 for (unsigned int i=0; i<positive_bucket_queue_.size(); ++i) 00320 { 00321 std::vector<SignedPropDistanceFieldVoxel*>::iterator list_it = positive_bucket_queue_[i].begin(); 00322 while(list_it!=positive_bucket_queue_[i].end()) 00323 { 00324 SignedPropDistanceFieldVoxel* vptr = *list_it; 00325 00326 x = vptr->location_[DIM_X]; 00327 y = vptr->location_[DIM_Y]; 00328 z = vptr->location_[DIM_Z]; 00329 00330 // select the neighborhood list based on the update direction: 00331 std::vector<std::vector<int> >* neighborhood; 00332 int D = i; 00333 if (D>1) 00334 D=1; 00335 // avoid a possible segfault situation: 00336 if (vptr->update_direction_<0 || vptr->update_direction_>26) 00337 { 00338 // ROS_WARN("Invalid update direction detected: %d", vptr->update_direction_); 00339 ++list_it; 00340 continue; 00341 } 00342 00343 neighborhood = &neighborhoods_[D][vptr->update_direction_]; 00344 00345 for (unsigned int n=0; n<neighborhood->size(); n++) 00346 { 00347 int dx = (*neighborhood)[n][DIM_X]; 00348 int dy = (*neighborhood)[n][DIM_Y]; 00349 int dz = (*neighborhood)[n][DIM_Z]; 00350 nx = x + dx; 00351 ny = y + dy; 00352 nz = z + dz; 00353 if (!isCellValid(nx,ny,nz)) 00354 continue; 00355 00356 // the real update code: 00357 // calculate the neighbor's new distance based on my closest filled voxel: 00358 SignedPropDistanceFieldVoxel* neighbor = &getCell(nx, ny, nz); 00359 loc[DIM_X] = nx; 00360 loc[DIM_Y] = ny; 00361 loc[DIM_Z] = nz; 00362 int new_distance_sq = eucDistSq(vptr->closest_positive_point_, loc); 00363 if (new_distance_sq > max_distance_sq_) 00364 continue; 00365 if (new_distance_sq < neighbor->positive_distance_square_) 00366 { 00367 // update the neighboring voxel 00368 neighbor->positive_distance_square_ = new_distance_sq; 00369 neighbor->closest_positive_point_[DIM_X] = vptr->closest_positive_point_[DIM_X]; 00370 neighbor->closest_positive_point_[DIM_Y] = vptr->closest_positive_point_[DIM_Y]; 00371 neighbor->closest_positive_point_[DIM_Z] = vptr->closest_positive_point_[DIM_Z]; 00372 neighbor->location_[DIM_X] = loc[DIM_X]; 00373 neighbor->location_[DIM_Y] = loc[DIM_Y]; 00374 neighbor->location_[DIM_Z] = loc[DIM_Z]; 00375 neighbor->update_direction_ = getDirectionNumber(dx, dy, dz); 00376 00377 // and put it in the queue: 00378 positive_bucket_queue_[new_distance_sq].push_back(neighbor); 00379 } 00380 } 00381 00382 ++list_it; 00383 } 00384 positive_bucket_queue_[i].clear(); 00385 } 00386 00387 00388 for(unsigned int i = 0; i < points.size(); i++) 00389 { 00390 bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(), x, y, z); 00391 if(!valid) 00392 continue; 00393 00394 for(int dx = -1; dx <= 1; dx ++) 00395 { 00396 for(int dy = -1; dy<= 1; dy ++) 00397 { 00398 for(int dz = -1; dz <= 1; dz++) 00399 { 00400 nx = x + dx; 00401 ny = y + dy; 00402 nz = z + dz; 00403 00404 if(!isCellValid(nx, ny, nz)) 00405 continue; 00406 00407 SignedPropDistanceFieldVoxel* neighbor = &getCell(nx, ny, nz); 00408 00409 if(neighbor->closest_negative_point_[DIM_X] != SignedPropDistanceFieldVoxel::UNINITIALIZED) 00410 { 00411 neighbor->update_direction_ = initial_update_direction; 00412 negative_bucket_queue_[0].push_back(neighbor); 00413 } 00414 } 00415 } 00416 } 00417 00418 } 00419 00420 for (unsigned int i=0; i<negative_bucket_queue_.size(); ++i) 00421 { 00422 std::vector<SignedPropDistanceFieldVoxel*>::iterator list_it = negative_bucket_queue_[i].begin(); 00423 while(list_it!=negative_bucket_queue_[i].end()) 00424 { 00425 SignedPropDistanceFieldVoxel* vptr = *list_it; 00426 00427 x = vptr->location_[DIM_X]; 00428 y = vptr->location_[DIM_Y]; 00429 z = vptr->location_[DIM_Z]; 00430 00431 // select the neighborhood list based on the update direction: 00432 std::vector<std::vector<int> >* neighborhood; 00433 int D = i; 00434 if (D>1) 00435 D=1; 00436 // avoid a possible segfault situation: 00437 if (vptr->update_direction_<0 || vptr->update_direction_>26) 00438 { 00439 // ROS_WARN("Invalid update direction detected: %d", vptr->update_direction_); 00440 ++list_it; 00441 continue; 00442 } 00443 00444 neighborhood = &neighborhoods_[D][vptr->update_direction_]; 00445 00446 for (unsigned int n=0; n<neighborhood->size(); n++) 00447 { 00448 int dx = (*neighborhood)[n][DIM_X]; 00449 int dy = (*neighborhood)[n][DIM_Y]; 00450 int dz = (*neighborhood)[n][DIM_Z]; 00451 nx = x + dx; 00452 ny = y + dy; 00453 nz = z + dz; 00454 if (!isCellValid(nx,ny,nz)) 00455 continue; 00456 00457 // the real update code: 00458 // calculate the neighbor's new distance based on my closest filled voxel: 00459 SignedPropDistanceFieldVoxel* neighbor = &getCell(nx, ny, nz); 00460 loc[DIM_X] = nx; 00461 loc[DIM_Y] = ny; 00462 loc[DIM_Z] = nz; 00463 int new_distance_sq = eucDistSq(vptr->closest_negative_point_, loc); 00464 if (new_distance_sq > max_distance_sq_) 00465 continue; 00466 if (new_distance_sq < neighbor->negative_distance_square_) 00467 { 00468 // update the neighboring voxel 00469 neighbor->negative_distance_square_ = new_distance_sq; 00470 neighbor->closest_negative_point_[DIM_X] = vptr->closest_negative_point_[DIM_X]; 00471 neighbor->closest_negative_point_[DIM_Y] = vptr->closest_negative_point_[DIM_Y]; 00472 neighbor->closest_negative_point_[DIM_Z] = vptr->closest_negative_point_[DIM_Z]; 00473 neighbor->location_[DIM_X] = loc[DIM_X]; 00474 neighbor->location_[DIM_Y] = loc[DIM_Y]; 00475 neighbor->location_[DIM_Z] = loc[DIM_Z]; 00476 neighbor->update_direction_ = getDirectionNumber(dx, dy, dz); 00477 00478 // and put it in the queue: 00479 negative_bucket_queue_[new_distance_sq].push_back(neighbor); 00480 } 00481 } 00482 00483 ++list_it; 00484 } 00485 negative_bucket_queue_[i].clear(); 00486 } 00487 00488 } 00489 00490 void SignedPropagationDistanceField::reset() 00491 { 00492 VoxelGrid<SignedPropDistanceFieldVoxel>::reset(SignedPropDistanceFieldVoxel(max_distance_sq_, 0)); 00493 } 00494 00495 void SignedPropagationDistanceField::initNeighborhoods() 00496 { 00497 // first initialize the direction number mapping: 00498 direction_number_to_direction_.resize(27); 00499 for (int dx=-1; dx<=1; ++dx) 00500 { 00501 for (int dy=-1; dy<=1; ++dy) 00502 { 00503 for (int dz=-1; dz<=1; ++dz) 00504 { 00505 int direction_number = getDirectionNumber(dx, dy, dz); 00506 std::vector<int> n_point(3); 00507 n_point[DIM_X] = dx; 00508 n_point[DIM_Y] = dy; 00509 n_point[DIM_Z] = dz; 00510 direction_number_to_direction_[direction_number] = n_point; 00511 } 00512 } 00513 } 00514 00515 neighborhoods_.resize(2); 00516 for (int n=0; n<2; n++) 00517 { 00518 neighborhoods_[n].resize(27); 00519 // source directions 00520 for (int dx=-1; dx<=1; ++dx) 00521 { 00522 for (int dy=-1; dy<=1; ++dy) 00523 { 00524 for (int dz=-1; dz<=1; ++dz) 00525 { 00526 int direction_number = getDirectionNumber(dx, dy, dz); 00527 // target directions: 00528 for (int tdx=-1; tdx<=1; ++tdx) 00529 { 00530 for (int tdy=-1; tdy<=1; ++tdy) 00531 { 00532 for (int tdz=-1; tdz<=1; ++tdz) 00533 { 00534 if (tdx==0 && tdy==0 && tdz==0) 00535 continue; 00536 if (n>=1) 00537 { 00538 if ((abs(tdx) + abs(tdy) + abs(tdz))!=1) 00539 continue; 00540 if (dx*tdx<0 || dy*tdy<0 || dz*tdz <0) 00541 continue; 00542 } 00543 std::vector<int> n_point(3); 00544 n_point[DIM_X] = tdx; 00545 n_point[DIM_Y] = tdy; 00546 n_point[DIM_Z] = tdz; 00547 neighborhoods_[n][direction_number].push_back(n_point); 00548 } 00549 } 00550 } 00551 //printf("n=%d, dx=%d, dy=%d, dz=%d, neighbors = %d\n", n, dx, dy, dz, neighborhoods_[n][direction_number].size()); 00552 } 00553 } 00554 } 00555 } 00556 00557 00558 00559 } 00560 00561 int SignedPropagationDistanceField::getDirectionNumber(int dx, int dy, int dz) const 00562 { 00563 return (dx+1)*9 + (dy+1)*3 + dz+1; 00564 } 00565 00566 00567 }