| 406 | } |
| 407 | |
| 408 | void SDFMap::clearAndInflateLocalMap() { |
| 409 | // /*clear outside local*/ |
| 410 | // const int vec_margin = 5; |
| 411 | |
| 412 | // Eigen::Vector3i min_cut = md_->local_bound_min_ - |
| 413 | // Eigen::Vector3i(mp_->local_map_margin_, mp_->local_map_margin_, |
| 414 | // mp_->local_map_margin_); |
| 415 | // Eigen::Vector3i max_cut = md_->local_bound_max_ + |
| 416 | // Eigen::Vector3i(mp_->local_map_margin_, mp_->local_map_margin_, |
| 417 | // mp_->local_map_margin_); |
| 418 | // boundIndex(min_cut); |
| 419 | // boundIndex(max_cut); |
| 420 | |
| 421 | // Eigen::Vector3i min_cut_m = min_cut - Eigen::Vector3i(vec_margin, vec_margin, |
| 422 | // vec_margin); Eigen::Vector3i max_cut_m = max_cut + Eigen::Vector3i(vec_margin, |
| 423 | // vec_margin, vec_margin); boundIndex(min_cut_m); boundIndex(max_cut_m); |
| 424 | |
| 425 | // // clear data outside the local range |
| 426 | |
| 427 | // for (int x = min_cut_m(0); x <= max_cut_m(0); ++x) |
| 428 | // for (int y = min_cut_m(1); y <= max_cut_m(1); ++y) { |
| 429 | |
| 430 | // for (int z = min_cut_m(2); z < min_cut(2); ++z) { |
| 431 | // int idx = toAddress(x, y, z); |
| 432 | // md_->occupancy_buffer_[idx] = mp_->clamp_min_log_ - mp_->unknown_flag_; |
| 433 | // md_->distance_buffer_all_[idx] = 10000; |
| 434 | // } |
| 435 | |
| 436 | // for (int z = max_cut(2) + 1; z <= max_cut_m(2); ++z) { |
| 437 | // int idx = toAddress(x, y, z); |
| 438 | // md_->occupancy_buffer_[idx] = mp_->clamp_min_log_ - mp_->unknown_flag_; |
| 439 | // md_->distance_buffer_all_[idx] = 10000; |
| 440 | // } |
| 441 | // } |
| 442 | |
| 443 | // for (int z = min_cut_m(2); z <= max_cut_m(2); ++z) |
| 444 | // for (int x = min_cut_m(0); x <= max_cut_m(0); ++x) { |
| 445 | |
| 446 | // for (int y = min_cut_m(1); y < min_cut(1); ++y) { |
| 447 | // int idx = toAddress(x, y, z); |
| 448 | // md_->occupancy_buffer_[idx] = mp_->clamp_min_log_ - mp_->unknown_flag_; |
| 449 | // md_->distance_buffer_all_[idx] = 10000; |
| 450 | // } |
| 451 | |
| 452 | // for (int y = max_cut(1) + 1; y <= max_cut_m(1); ++y) { |
| 453 | // int idx = toAddress(x, y, z); |
| 454 | // md_->occupancy_buffer_[idx] = mp_->clamp_min_log_ - mp_->unknown_flag_; |
| 455 | // md_->distance_buffer_all_[idx] = 10000; |
| 456 | // } |
| 457 | // } |
| 458 | |
| 459 | // for (int y = min_cut_m(1); y <= max_cut_m(1); ++y) |
| 460 | // for (int z = min_cut_m(2); z <= max_cut_m(2); ++z) { |
| 461 | |
| 462 | // for (int x = min_cut_m(0); x < min_cut(0); ++x) { |
| 463 | // int idx = toAddress(x, y, z); |
| 464 | // md_->occupancy_buffer_[idx] = mp_->clamp_min_log_ - mp_->unknown_flag_; |
| 465 | // md_->distance_buffer_all_[idx] = 10000; |
no test coverage detected