| 605 | } |
| 606 | |
| 607 | std::vector<std::vector<size_t>> AssignRaysToBlocks( |
| 608 | const std::vector<K::Segment_3>& rays, |
| 609 | const std::vector<std::vector<size_t>>& block_point_indices, |
| 610 | const BlockGrid& grid) { |
| 611 | // For each ray, compute the range of blocks its AABB overlaps with, |
| 612 | // then do an exact segment-box intersection test only on those blocks. |
| 613 | // This is O(rays * local_blocks) instead of O(rays * total_blocks). |
| 614 | const Eigen::Vector3i grid_dims(grid.nx, grid.ny, grid.nz); |
| 615 | const double inv_block_size = 1.0 / grid.block_size; |
| 616 | |
| 617 | std::vector<std::vector<size_t>> block_ray_indices(grid.NumBlocks()); |
| 618 | const size_t log_interval = std::max(rays.size() / 10, size_t{1}); |
| 619 | for (size_t ray_idx = 0; ray_idx < rays.size(); ++ray_idx) { |
| 620 | if (ray_idx % log_interval == 0) { |
| 621 | LOG(INFO) << colmap::StringPrintf( |
| 622 | "Assigning rays to blocks [%d/%d]", ray_idx, rays.size()); |
| 623 | } |
| 624 | |
| 625 | // Compute the ray's AABB in grid coordinates. |
| 626 | const auto& seg = rays[ray_idx]; |
| 627 | const Eigen::Vector3d p0(seg.source().x() - grid.min.x(), |
| 628 | seg.source().y() - grid.min.y(), |
| 629 | seg.source().z() - grid.min.z()); |
| 630 | const Eigen::Vector3d p1(seg.target().x() - grid.min.x(), |
| 631 | seg.target().y() - grid.min.y(), |
| 632 | seg.target().z() - grid.min.z()); |
| 633 | const Eigen::Vector3d ray_min = p0.cwiseMin(p1); |
| 634 | const Eigen::Vector3d ray_max = p0.cwiseMax(p1); |
| 635 | |
| 636 | // Determine the block range that the ray's AABB overlaps (with overlap). |
| 637 | const Eigen::Vector3i b_min = |
| 638 | ((ray_min.array() - grid.overlap) * inv_block_size) |
| 639 | .floor() |
| 640 | .cast<int>() |
| 641 | .max(0); |
| 642 | const Eigen::Vector3i b_max = |
| 643 | ((ray_max.array() + grid.overlap) * inv_block_size) |
| 644 | .floor() |
| 645 | .cast<int>() |
| 646 | .min(grid_dims.array() - 1); |
| 647 | |
| 648 | for (int bx = b_min.x(); bx <= b_max.x(); ++bx) { |
| 649 | for (int by = b_min.y(); by <= b_max.y(); ++by) { |
| 650 | for (int bz = b_min.z(); bz <= b_max.z(); ++bz) { |
| 651 | const int idx = grid.BlockIndex(bx, by, bz); |
| 652 | if (block_point_indices[idx].empty()) { |
| 653 | continue; |
| 654 | } |
| 655 | if (CGAL::do_intersect(seg, grid.BlockBbox(bx, by, bz))) { |
| 656 | block_ray_indices[idx].push_back(ray_idx); |
| 657 | } |
| 658 | } |
| 659 | } |
| 660 | } |
| 661 | } |
| 662 | return block_ray_indices; |
| 663 | } |
| 664 |
no test coverage detected