| 296 | } |
| 297 | |
| 298 | void SDFMap::inputPointCloud(const pcl::PointCloud<pcl::PointXYZ>& points, const int& point_num, |
| 299 | const Eigen::Vector3d& camera_pos) { |
| 300 | if (point_num == 0) return; |
| 301 | md_->raycast_num_ += 1; |
| 302 | |
| 303 | Eigen::Vector3d update_min = camera_pos; |
| 304 | Eigen::Vector3d update_max = camera_pos; |
| 305 | if (md_->reset_updated_box_) { |
| 306 | md_->update_min_ = camera_pos; |
| 307 | md_->update_max_ = camera_pos; |
| 308 | md_->reset_updated_box_ = false; |
| 309 | } |
| 310 | |
| 311 | Eigen::Vector3d pt_w, tmp; |
| 312 | Eigen::Vector3i idx; |
| 313 | int vox_adr; |
| 314 | double length; |
| 315 | for (int i = 0; i < point_num; ++i) { |
| 316 | auto& pt = points.points[i]; |
| 317 | pt_w << pt.x, pt.y, pt.z; |
| 318 | int tmp_flag; |
| 319 | // Set flag for projected point |
| 320 | if (!isInMap(pt_w)) { |
| 321 | // Find closest point in map and set free |
| 322 | pt_w = closetPointInMap(pt_w, camera_pos); |
| 323 | length = (pt_w - camera_pos).norm(); |
| 324 | if (length > mp_->max_ray_length_) |
| 325 | pt_w = (pt_w - camera_pos) / length * mp_->max_ray_length_ + camera_pos; |
| 326 | if (pt_w[2] < 0.2) continue; |
| 327 | tmp_flag = 0; |
| 328 | } else { |
| 329 | length = (pt_w - camera_pos).norm(); |
| 330 | if (length > mp_->max_ray_length_) { |
| 331 | pt_w = (pt_w - camera_pos) / length * mp_->max_ray_length_ + camera_pos; |
| 332 | if (pt_w[2] < 0.2) continue; |
| 333 | tmp_flag = 0; |
| 334 | } else |
| 335 | tmp_flag = 1; |
| 336 | } |
| 337 | posToIndex(pt_w, idx); |
| 338 | vox_adr = toAddress(idx); |
| 339 | setCacheOccupancy(vox_adr, tmp_flag); |
| 340 | |
| 341 | for (int k = 0; k < 3; ++k) { |
| 342 | update_min[k] = min(update_min[k], pt_w[k]); |
| 343 | update_max[k] = max(update_max[k], pt_w[k]); |
| 344 | } |
| 345 | // Raycasting between camera center and point |
| 346 | if (md_->flag_rayend_[vox_adr] == md_->raycast_num_) |
| 347 | continue; |
| 348 | else |
| 349 | md_->flag_rayend_[vox_adr] = md_->raycast_num_; |
| 350 | |
| 351 | caster_->input(pt_w, camera_pos); |
| 352 | caster_->nextId(idx); |
| 353 | while (caster_->nextId(idx)) setCacheOccupancy(toAddress(idx), 0); |
| 354 | } |
| 355 |
no test coverage detected