| 506 | } |
| 507 | |
| 508 | void MultiMapManager::insertChunkToMap(const MapChunk& chunk, const int& drone_id) |
| 509 | { |
| 510 | |
| 511 | // // Transform from other drone's local frame to this drone's |
| 512 | // Eigen::Vector4d transform; |
| 513 | // map_->getBaseCoor(drone_id, transform); |
| 514 | |
| 515 | // double yaw = transform[3]; |
| 516 | // Eigen::Matrix3d rot; |
| 517 | // rot << cos(yaw), -sin(yaw), 0, sin(yaw), cos(yaw), 0, 0, 0, 1; |
| 518 | // Eigen::Vector3d trans = transform.head<3>(); |
| 519 | |
| 520 | for (int i = 0; i < chunk.voxel_adrs_.size(); ++i) { |
| 521 | // Insert occ info |
| 522 | |
| 523 | auto& adr = chunk.voxel_adrs_[i]; |
| 524 | |
| 525 | Eigen::Vector3i idx; |
| 526 | adrToIndex(adr, idx); |
| 527 | |
| 528 | Eigen::Vector3d pos; |
| 529 | map_->indexToPos(idx, pos); |
| 530 | |
| 531 | // pos = rot * pos + trans; |
| 532 | if (!map_->isInMap(pos)) |
| 533 | continue; |
| 534 | |
| 535 | map_->posToIndex(pos, idx); |
| 536 | auto adr_tf = map_->toAddress(idx); |
| 537 | |
| 538 | // map_->md_->occupancy_buffer_[adr] = |
| 539 | // chunk.voxel_occ_[i] == 1 ? map_->mp_->clamp_max_log_ : map_->mp_->clamp_min_log_; |
| 540 | map_->md_->occupancy_buffer_[adr_tf] = |
| 541 | chunk.voxel_occ_[i] == 1 ? map_->mp_->clamp_max_log_ : map_->mp_->clamp_min_log_; |
| 542 | |
| 543 | // Update the chunk box |
| 544 | |
| 545 | if (chunk_boxes_[drone_id - 1].valid_) { |
| 546 | for (int k = 0; k < 3; ++k) { |
| 547 | chunk_boxes_[drone_id - 1].min_[k] = min(chunk_boxes_[drone_id - 1].min_[k], pos[k]); |
| 548 | chunk_boxes_[drone_id - 1].max_[k] = max(chunk_boxes_[drone_id - 1].max_[k], pos[k]); |
| 549 | } |
| 550 | } |
| 551 | else { |
| 552 | chunk_boxes_[drone_id - 1].min_ = chunk_boxes_[drone_id - 1].max_ = pos; |
| 553 | chunk_boxes_[drone_id - 1].valid_ = true; |
| 554 | } |
| 555 | |
| 556 | // Update the all box |
| 557 | for (int k = 0; k < 3; ++k) { |
| 558 | map_->md_->all_min_[k] = min(map_->md_->all_min_[k], pos[k]); |
| 559 | map_->md_->all_max_[k] = max(map_->md_->all_max_[k], pos[k]); |
| 560 | } |
| 561 | // Inflate for the occupied |
| 562 | if (chunk.voxel_occ_[i] == 1) { |
| 563 | static const int inf_step = ceil(map_->mp_->obstacles_inflation_ / map_->mp_->resolution_); |
| 564 | for (int inf_x = -inf_step; inf_x <= inf_step; ++inf_x) |
| 565 | for (int inf_y = -inf_step; inf_y <= inf_step; ++inf_y) |
nothing calls this directly
no test coverage detected