| 414 | } |
| 415 | |
| 416 | Eigen::Vector3d SDFMap::closetPointInMap( |
| 417 | const Eigen::Vector3d& pt, const Eigen::Vector3d& camera_pt) |
| 418 | { |
| 419 | Eigen::Vector3d diff = pt - camera_pt; |
| 420 | Eigen::Vector3d max_tc = mp_->map_max_boundary_ - camera_pt; |
| 421 | Eigen::Vector3d min_tc = mp_->map_min_boundary_ - camera_pt; |
| 422 | double min_t = 1000000; |
| 423 | for (int i = 0; i < 3; ++i) { |
| 424 | if (fabs(diff[i]) > 0) { |
| 425 | double t1 = max_tc[i] / diff[i]; |
| 426 | if (t1 > 0 && t1 < min_t) |
| 427 | min_t = t1; |
| 428 | double t2 = min_tc[i] / diff[i]; |
| 429 | if (t2 > 0 && t2 < min_t) |
| 430 | min_t = t2; |
| 431 | } |
| 432 | } |
| 433 | return camera_pt + (min_t - 1e-3) * diff; |
| 434 | } |
| 435 | |
| 436 | void SDFMap::inputGlobalMap(const pcl::PointCloud<pcl::PointXYZ>& points, const int& point_num) |
| 437 | { |
nothing calls this directly
no outgoing calls
no test coverage detected