| 610 | |
| 611 | |
| 612 | void SceneData::PointDistanceToCamera() |
| 613 | { |
| 614 | point_cloud.data.resize(point_cloud.NumVertices()); |
| 615 | #pragma omp parallel for |
| 616 | for (int i = 0; i < point_cloud.NumVertices(); ++i) |
| 617 | { |
| 618 | float best_dis = 105235545; |
| 619 | |
| 620 | vec3 p = point_cloud.position[i]; |
| 621 | |
| 622 | for (auto& f : frames) |
| 623 | { |
| 624 | auto [ip, z] = f.Project3(p); |
| 625 | if (z <= 0 || !f.inImage(ip)) continue; |
| 626 | float dis = (p - f.pose.translation().cast<float>()).squaredNorm(); |
| 627 | |
| 628 | if (dis < best_dis) |
| 629 | { |
| 630 | best_dis = dis; |
| 631 | } |
| 632 | } |
| 633 | |
| 634 | point_cloud.data[i](0) = sqrt(best_dis); |
| 635 | } |
| 636 | } |
| 637 | void SceneData::SortBlocksByRadius(int block_size) |
| 638 | { |
| 639 | int n_blocks = point_cloud.NumVertices() / block_size; |