| 308 | } |
| 309 | |
| 310 | void SDFMap::inputPointCloud(const pcl::PointCloud<pcl::PointXYZ>& points, const int& point_num, |
| 311 | const Eigen::Vector3d& camera_pos) |
| 312 | { |
| 313 | if (point_num == 0) |
| 314 | return; |
| 315 | md_->raycast_num_ += 1; |
| 316 | |
| 317 | Eigen::Vector3d update_min = camera_pos; |
| 318 | Eigen::Vector3d update_max = camera_pos; |
| 319 | if (md_->reset_updated_box_) { |
| 320 | md_->update_min_ = camera_pos; |
| 321 | md_->update_max_ = camera_pos; |
| 322 | md_->reset_updated_box_ = false; |
| 323 | } |
| 324 | |
| 325 | Eigen::Vector3d pt_w, tmp; |
| 326 | Eigen::Vector3i idx; |
| 327 | int vox_adr; |
| 328 | double length; |
| 329 | for (int i = 0; i < point_num; ++i) { |
| 330 | auto& pt = points.points[i]; |
| 331 | pt_w << pt.x, pt.y, pt.z; |
| 332 | int tmp_flag; |
| 333 | // Set flag for projected point |
| 334 | if (!isInMap(pt_w)) { |
| 335 | // Find closest point in map and set free |
| 336 | pt_w = closetPointInMap(pt_w, camera_pos); |
| 337 | length = (pt_w - camera_pos).norm(); |
| 338 | if (length > mp_->max_ray_length_) |
| 339 | pt_w = (pt_w - camera_pos) / length * mp_->max_ray_length_ + camera_pos; |
| 340 | if (pt_w[2] < 0.2) |
| 341 | continue; |
| 342 | tmp_flag = 0; |
| 343 | } |
| 344 | else { |
| 345 | length = (pt_w - camera_pos).norm(); |
| 346 | if (length > mp_->max_ray_length_) { |
| 347 | pt_w = (pt_w - camera_pos) / length * mp_->max_ray_length_ + camera_pos; |
| 348 | if (pt_w[2] < 0.2) |
| 349 | continue; |
| 350 | tmp_flag = 0; |
| 351 | } |
| 352 | else |
| 353 | tmp_flag = 1; |
| 354 | } |
| 355 | posToIndex(pt_w, idx); |
| 356 | vox_adr = toAddress(idx); |
| 357 | if (isInBox(idx)) { |
| 358 | if (md_->global_occ_[vox_adr] == 1) { |
| 359 | md_->exp_occ_[vox_adr] = 1; |
| 360 | } |
| 361 | } |
| 362 | setCacheOccupancy(vox_adr, tmp_flag); |
| 363 | |
| 364 | for (int k = 0; k < 3; ++k) { |
| 365 | update_min[k] = min(update_min[k], pt_w[k]); |
| 366 | update_max[k] = max(update_max[k], pt_w[k]); |
| 367 | } |
no test coverage detected