| 434 | } |
| 435 | |
| 436 | void SDFMap::inputGlobalMap(const pcl::PointCloud<pcl::PointXYZ>& points, const int& point_num) |
| 437 | { |
| 438 | if (point_num == 0) |
| 439 | return; |
| 440 | Eigen::Vector3d pt_w; |
| 441 | Eigen::Vector3i idx; |
| 442 | int vox_adr; |
| 443 | for (size_t i = 0; i < point_num; i++) { |
| 444 | auto& pt = points.points[i]; |
| 445 | pt_w << pt.x, pt.y, pt.z; |
| 446 | |
| 447 | posToIndex(pt_w, idx); |
| 448 | vox_adr = toAddress(idx); |
| 449 | |
| 450 | md_->global_occ_[vox_adr] = 1; |
| 451 | } |
| 452 | } |
| 453 | |
| 454 | void SDFMap::clearAndInflateLocalMap() |
| 455 | { |