| 439 | //} |
| 440 | |
| 441 | void RosBaseDataProvider::publishPerFrameMesh3D( |
| 442 | const SpinOutputPacket& vio_output) const { |
| 443 | const Mesh2D& mesh_2d = vio_output.mesh_2d_; |
| 444 | const Mesh3D& mesh_3d = vio_output.mesh_3d_; |
| 445 | size_t number_mesh_2d_polygons = mesh_2d.getNumberOfPolygons(); |
| 446 | size_t mesh_2d_poly_dim = mesh_2d.getMeshPolygonDimension(); |
| 447 | |
| 448 | static const size_t cam_width = |
| 449 | stereo_calib_.left_camera_info_.image_size_.width; |
| 450 | static const size_t cam_height = |
| 451 | stereo_calib_.left_camera_info_.image_size_.height; |
| 452 | DCHECK_GT(cam_width, 0); |
| 453 | DCHECK_GT(cam_height, 0); |
| 454 | |
| 455 | pcl_msgs::PolygonMesh::Ptr msg(new pcl_msgs::PolygonMesh()); |
| 456 | msg->header.stamp.fromNSec(vio_output.getTimestamp()); |
| 457 | msg->header.frame_id = world_frame_id_; |
| 458 | |
| 459 | // Create point cloud to hold vertices. |
| 460 | pcl::PointCloud<PointNormalUV> cloud; |
| 461 | cloud.points.reserve(number_mesh_2d_polygons * mesh_2d_poly_dim); |
| 462 | msg->polygons.reserve(number_mesh_2d_polygons); |
| 463 | |
| 464 | Mesh2D::Polygon polygon; |
| 465 | for (size_t i = 0; i < number_mesh_2d_polygons; i++) { |
| 466 | CHECK(mesh_2d.getPolygon(i, &polygon)) << "Could not retrieve 2d polygon."; |
| 467 | const LandmarkId& lmk0_id = polygon.at(0).getLmkId(); |
| 468 | const LandmarkId& lmk1_id = polygon.at(1).getLmkId(); |
| 469 | const LandmarkId& lmk2_id = polygon.at(2).getLmkId(); |
| 470 | |
| 471 | // Returns indices of points in the 3D mesh corresponding to the |
| 472 | // vertices |
| 473 | // in the 2D mesh. |
| 474 | int p0_id, p1_id, p2_id; |
| 475 | Mesh3D::VertexType vtx0, vtx1, vtx2; |
| 476 | if (mesh_3d.getVertex(lmk0_id, &vtx0, &p0_id) && |
| 477 | mesh_3d.getVertex(lmk1_id, &vtx1, &p1_id) && |
| 478 | mesh_3d.getVertex(lmk2_id, &vtx2, &p2_id)) { |
| 479 | // Get pixel coordinates of the vertices of the 2D mesh. |
| 480 | const Vertex2D& px0 = polygon.at(0).getVertexPosition(); |
| 481 | const Vertex2D& px1 = polygon.at(1).getVertexPosition(); |
| 482 | const Vertex2D& px2 = polygon.at(2).getVertexPosition(); |
| 483 | |
| 484 | // Get 3D coordinates of the vertices of the 3D mesh. |
| 485 | const Vertex3D& lmk0_pos = vtx0.getVertexPosition(); |
| 486 | const Vertex3D& lmk1_pos = vtx1.getVertexPosition(); |
| 487 | const Vertex3D& lmk2_pos = vtx2.getVertexPosition(); |
| 488 | |
| 489 | // Get normals of the vertices of the 3D mesh. |
| 490 | const Mesh3D::VertexNormal& normal0 = vtx0.getVertexNormal(); |
| 491 | const Mesh3D::VertexNormal& normal1 = vtx1.getVertexNormal(); |
| 492 | const Mesh3D::VertexNormal& normal2 = vtx2.getVertexNormal(); |
| 493 | |
| 494 | // FILL POINTCLOUD |
| 495 | // clang-format off |
| 496 | PointNormalUV pn0, pn1, pn2; |
| 497 | pn0.x = lmk0_pos.x; pn1.x = lmk1_pos.x; pn2.x = lmk2_pos.x; |
| 498 | pn0.y = lmk0_pos.y; pn1.y = lmk1_pos.y; pn2.y = lmk2_pos.y; |