| 356 | } |
| 357 | |
| 358 | void RosBaseDataProvider::publishTimeHorizonPointCloud( |
| 359 | const Timestamp& timestamp, const PointsWithIdMap& points_with_id, |
| 360 | const LmkIdToLmkTypeMap& lmk_id_to_lmk_type_map) const { |
| 361 | PointCloudXYZRGB::Ptr msg(new PointCloudXYZRGB); |
| 362 | msg->header.frame_id = world_frame_id_; |
| 363 | msg->is_dense = true; |
| 364 | msg->height = 1; |
| 365 | msg->width = points_with_id.size(); |
| 366 | msg->points.resize(points_with_id.size()); |
| 367 | |
| 368 | LOG(ERROR) << "Points with id size: " << msg->points.size(); |
| 369 | |
| 370 | bool color_the_cloud = false; |
| 371 | if (lmk_id_to_lmk_type_map.size() != 0) { |
| 372 | color_the_cloud = true; |
| 373 | CHECK_EQ(points_with_id.size(), lmk_id_to_lmk_type_map.size()); |
| 374 | } |
| 375 | |
| 376 | if (points_with_id.size() == 0) { |
| 377 | // No points to visualize. |
| 378 | return; |
| 379 | } |
| 380 | |
| 381 | // Populate cloud structure with 3D points. |
| 382 | size_t i = 0; |
| 383 | for (const std::pair<LandmarkId, gtsam::Point3>& id_point : points_with_id) { |
| 384 | const gtsam::Point3 point_3d = id_point.second; |
| 385 | msg->points[i].x = static_cast<float>(point_3d.x()); |
| 386 | msg->points[i].y = static_cast<float>(point_3d.y()); |
| 387 | msg->points[i].z = static_cast<float>(point_3d.z()); |
| 388 | if (color_the_cloud) { |
| 389 | DCHECK(lmk_id_to_lmk_type_map.find(id_point.first) != |
| 390 | lmk_id_to_lmk_type_map.end()); |
| 391 | switch (lmk_id_to_lmk_type_map.at(id_point.first)) { |
| 392 | case LandmarkType::SMART: { |
| 393 | // point_cloud_color.col(i) = cv::viz::Color::white(); |
| 394 | msg->points[i].r = 0; |
| 395 | msg->points[i].g = 255; |
| 396 | msg->points[i].b = 0; |
| 397 | break; |
| 398 | } |
| 399 | case LandmarkType::PROJECTION: { |
| 400 | // point_cloud_color.col(i) = cv::viz::Color::green(); |
| 401 | msg->points[i].r = 0; |
| 402 | msg->points[i].g = 0; |
| 403 | msg->points[i].b = 255; |
| 404 | break; |
| 405 | } |
| 406 | default: { |
| 407 | // point_cloud_color.col(i) = cv::viz::Color::white(); |
| 408 | msg->points[i].r = 255; |
| 409 | msg->points[i].g = 0; |
| 410 | msg->points[i].b = 0; |
| 411 | break; |
| 412 | } |
| 413 | } |
| 414 | } |
| 415 | i++; |