| 389 | } |
| 390 | |
| 391 | Eigen::Vector3d SDFMap::closetPointInMap( |
| 392 | const Eigen::Vector3d& pt, const Eigen::Vector3d& camera_pt) { |
| 393 | Eigen::Vector3d diff = pt - camera_pt; |
| 394 | Eigen::Vector3d max_tc = mp_->map_max_boundary_ - camera_pt; |
| 395 | Eigen::Vector3d min_tc = mp_->map_min_boundary_ - camera_pt; |
| 396 | double min_t = 1000000; |
| 397 | for (int i = 0; i < 3; ++i) { |
| 398 | if (fabs(diff[i]) > 0) { |
| 399 | double t1 = max_tc[i] / diff[i]; |
| 400 | if (t1 > 0 && t1 < min_t) min_t = t1; |
| 401 | double t2 = min_tc[i] / diff[i]; |
| 402 | if (t2 > 0 && t2 < min_t) min_t = t2; |
| 403 | } |
| 404 | } |
| 405 | return camera_pt + (min_t - 1e-3) * diff; |
| 406 | } |
| 407 | |
| 408 | void SDFMap::clearAndInflateLocalMap() { |
| 409 | // /*clear outside local*/ |