From ef5090af2e934e8876e26686f9a8cf1e4f372a79 Mon Sep 17 00:00:00 2001 From: Hermann Date: Wed, 3 Aug 2022 13:00:56 +0200 Subject: [PATCH 1/4] node that saves the meshes send to RVIZ --- panoptic_mapping_utils/CMakeLists.txt | 6 + .../app/mesh_saver_node.cpp | 22 ++++ .../scannet/mesh_saver.h | 35 ++++++ .../launch/scannnet_visualizer.launch | 44 ++++++++ .../src/scannetv2/mesh_saver.cpp | 106 ++++++++++++++++++ 5 files changed, 213 insertions(+) create mode 100644 panoptic_mapping_utils/app/mesh_saver_node.cpp create mode 100644 panoptic_mapping_utils/include/panoptic_mapping_utils/scannet/mesh_saver.h create mode 100644 panoptic_mapping_utils/launch/scannnet_visualizer.launch create mode 100644 panoptic_mapping_utils/src/scannetv2/mesh_saver.cpp diff --git a/panoptic_mapping_utils/CMakeLists.txt b/panoptic_mapping_utils/CMakeLists.txt index ab57c7f3..b0b08df2 100644 --- a/panoptic_mapping_utils/CMakeLists.txt +++ b/panoptic_mapping_utils/CMakeLists.txt @@ -12,6 +12,7 @@ catkin_package() cs_add_library(${PROJECT_NAME} src/evaluation/map_evaluator.cpp + src/scannetv2/mesh_saver.cpp ) ############### @@ -28,6 +29,11 @@ cs_add_executable(multi_map_evaluation ) target_link_libraries(multi_map_evaluation ${PROJECT_NAME}) +cs_add_executable(mesh_saver + app/mesh_saver_node.cpp + ) +target_link_libraries(mesh_saver ${PROJECT_NAME}) + ########## # Export # ########## diff --git a/panoptic_mapping_utils/app/mesh_saver_node.cpp b/panoptic_mapping_utils/app/mesh_saver_node.cpp new file mode 100644 index 00000000..978caece --- /dev/null +++ b/panoptic_mapping_utils/app/mesh_saver_node.cpp @@ -0,0 +1,22 @@ +#include +#include + +#include "panoptic_mapping_utils/scannet/mesh_saver.h" + +int main(int argc, char** argv) { + // Start Ros. + ros::init(argc, argv, "mesh_saver", ros::init_options::NoSigintHandler); + + + // Setup logging. + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + google::ParseCommandLineFlags(&argc, &argv, false); + + // Setup node. + ros::NodeHandle nh_private("~"); + panoptic_mapping::MeshSaver mesh_saver(nh_private); + ros::spin(); + ros::waitForShutdown(); + return 0; +} diff --git a/panoptic_mapping_utils/include/panoptic_mapping_utils/scannet/mesh_saver.h b/panoptic_mapping_utils/include/panoptic_mapping_utils/scannet/mesh_saver.h new file mode 100644 index 00000000..aff30d71 --- /dev/null +++ b/panoptic_mapping_utils/include/panoptic_mapping_utils/scannet/mesh_saver.h @@ -0,0 +1,35 @@ +#ifndef PANOPTIC_MAPPING_UTILS_SCANNET_MESHSAVER_H_ +#define PANOPTIC_MAPPING_UTILS_SCANNET_MESHSAVER_H_ + +#include +#include + +#include + +#include "voxblox/core/block_hash.h" +#include "voxblox/core/common.h" +#include "voxblox/mesh/mesh.h" +#include "voxblox/mesh/mesh_utils.h" +#include "voxblox/io/mesh_ply.h" +#include +#include + +namespace panoptic_mapping { + +class MeshSaver { + public: + MeshSaver(const ros::NodeHandle& nh); + virtual ~MeshSaver() = default; + void gotMeshCallback(const voxblox_msgs::MultiMesh& msg); + + private: + void setupRos(); + + ros::NodeHandle nh_; + + ros::Subscriber mesh_sub_; +}; + +} // namespace panoptic_mapping + +#endif diff --git a/panoptic_mapping_utils/launch/scannnet_visualizer.launch b/panoptic_mapping_utils/launch/scannnet_visualizer.launch new file mode 100644 index 00000000..26472a4d --- /dev/null +++ b/panoptic_mapping_utils/launch/scannnet_visualizer.launch @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/panoptic_mapping_utils/src/scannetv2/mesh_saver.cpp b/panoptic_mapping_utils/src/scannetv2/mesh_saver.cpp new file mode 100644 index 00000000..3cd70bdf --- /dev/null +++ b/panoptic_mapping_utils/src/scannetv2/mesh_saver.cpp @@ -0,0 +1,106 @@ +#include "panoptic_mapping_utils/scannet/mesh_saver.h" + +namespace panoptic_mapping { + +MeshSaver::MeshSaver(const ros::NodeHandle& nh) : nh_(nh) { setupRos(); } +void MeshSaver::setupRos() { + mesh_sub_ = + nh_.subscribe(nh_.param( + "topic", "/panoptic_mapper/visualization/submaps/mesh"), + 10, &MeshSaver::gotMeshCallback, this); +} + +void MeshSaver::gotMeshCallback(const voxblox_msgs::MultiMesh& msg) { + voxblox::Mesh full_mesh; + bool first = true; + + for (const voxblox_msgs::MeshBlock& mesh_block : msg.mesh.mesh_blocks) { + const voxblox::BlockIndex index(mesh_block.index[0], mesh_block.index[1], + mesh_block.index[2]); + + if (mesh_block.x.size() == 0) { + continue; + } + + size_t vertex_index = 0u; + voxblox::Mesh mesh; + mesh.vertices.reserve(mesh_block.x.size()); + mesh.indices.reserve(mesh_block.x.size()); + + // translate vertex data from message to voxblox mesh + for (size_t i = 0; i < mesh_block.x.size(); ++i) { + // Each vertex is given as its distance from the blocks origin in units of + // (2*block_size), see mesh_vis.h for the slightly convoluted + // justification of the 2. + constexpr float point_conv_factor = + 2.0f / std::numeric_limits::max(); + const float mesh_x = + (static_cast(mesh_block.x[i]) * point_conv_factor + + static_cast(index[0])) * + msg.mesh.block_edge_length; + const float mesh_y = + (static_cast(mesh_block.y[i]) * point_conv_factor + + static_cast(index[1])) * + msg.mesh.block_edge_length; + const float mesh_z = + (static_cast(mesh_block.z[i]) * point_conv_factor + + static_cast(index[2])) * + msg.mesh.block_edge_length; + + mesh.indices.push_back(vertex_index++); + mesh.vertices.emplace_back(mesh_x, mesh_y, mesh_z); + } + + // calculate normals + mesh.normals.reserve(mesh.vertices.size()); + for (size_t i = 0; i < mesh.vertices.size(); i += 3) { + const voxblox::Point dir0 = mesh.vertices[i] - mesh.vertices[i + 1]; + const voxblox::Point dir1 = mesh.vertices[i] - mesh.vertices[i + 2]; + const voxblox::Point normal = dir0.cross(dir1).normalized(); + + mesh.normals.push_back(normal); + mesh.normals.push_back(normal); + mesh.normals.push_back(normal); + } + + // add color information + mesh.colors.reserve(mesh.vertices.size()); + const bool has_color = mesh_block.x.size() == mesh_block.r.size(); + for (size_t i = 0; i < mesh_block.x.size(); ++i) { + voxblox::Color color; + if (has_color) { + color.r = mesh_block.r[i]; + color.g = mesh_block.g[i]; + color.b = mesh_block.b[i]; + + } else { + // reconstruct normals coloring + color.r = std::numeric_limits::max() * + (mesh.normals[i].x() * 0.5f + 0.5f); + color.g = std::numeric_limits::max() * + (mesh.normals[i].y() * 0.5f + 0.5f); + color.b = std::numeric_limits::max() * + (mesh.normals[i].z() * 0.5f + 0.5f); + } + color.a = 1.0; + mesh.colors.push_back(color); + } + + // connect mesh + + if (first) { + voxblox::createConnectedMesh(mesh, &full_mesh); + first = false; + } else { + voxblox::Mesh connected_mesh; + voxblox::createConnectedMesh(mesh, &connected_mesh); + full_mesh.concatenateMesh(connected_mesh); + } + } + // save mesh + std::string output_path = + nh_.param("output path", "/tmp/map_mesh.ply"); + voxblox::outputMeshAsPly(output_path, full_mesh); + std::cout << "mesh saved to " << output_path << std::endl; +} +} // namespace panoptic_mapping From ac7df3cb224945bf4a53cd0fa3b5bc5adcc59440 Mon Sep 17 00:00:00 2001 From: Hermann Date: Tue, 8 Nov 2022 06:27:24 +0100 Subject: [PATCH 2/4] move source files --- panoptic_mapping_utils/CMakeLists.txt | 2 +- panoptic_mapping_utils/app/mesh_saver_node.cpp | 2 +- .../include/panoptic_mapping_utils/{scannet => }/mesh_saver.h | 0 panoptic_mapping_utils/src/{scannetv2 => }/mesh_saver.cpp | 2 +- 4 files changed, 3 insertions(+), 3 deletions(-) rename panoptic_mapping_utils/include/panoptic_mapping_utils/{scannet => }/mesh_saver.h (100%) rename panoptic_mapping_utils/src/{scannetv2 => }/mesh_saver.cpp (98%) diff --git a/panoptic_mapping_utils/CMakeLists.txt b/panoptic_mapping_utils/CMakeLists.txt index b0b08df2..4b7728e6 100644 --- a/panoptic_mapping_utils/CMakeLists.txt +++ b/panoptic_mapping_utils/CMakeLists.txt @@ -12,7 +12,7 @@ catkin_package() cs_add_library(${PROJECT_NAME} src/evaluation/map_evaluator.cpp - src/scannetv2/mesh_saver.cpp + src/mesh_saver.cpp ) ############### diff --git a/panoptic_mapping_utils/app/mesh_saver_node.cpp b/panoptic_mapping_utils/app/mesh_saver_node.cpp index 978caece..a7037cb1 100644 --- a/panoptic_mapping_utils/app/mesh_saver_node.cpp +++ b/panoptic_mapping_utils/app/mesh_saver_node.cpp @@ -1,7 +1,7 @@ #include #include -#include "panoptic_mapping_utils/scannet/mesh_saver.h" +#include "panoptic_mapping_utils/mesh_saver.h" int main(int argc, char** argv) { // Start Ros. diff --git a/panoptic_mapping_utils/include/panoptic_mapping_utils/scannet/mesh_saver.h b/panoptic_mapping_utils/include/panoptic_mapping_utils/mesh_saver.h similarity index 100% rename from panoptic_mapping_utils/include/panoptic_mapping_utils/scannet/mesh_saver.h rename to panoptic_mapping_utils/include/panoptic_mapping_utils/mesh_saver.h diff --git a/panoptic_mapping_utils/src/scannetv2/mesh_saver.cpp b/panoptic_mapping_utils/src/mesh_saver.cpp similarity index 98% rename from panoptic_mapping_utils/src/scannetv2/mesh_saver.cpp rename to panoptic_mapping_utils/src/mesh_saver.cpp index 3cd70bdf..2b7a98a3 100644 --- a/panoptic_mapping_utils/src/scannetv2/mesh_saver.cpp +++ b/panoptic_mapping_utils/src/mesh_saver.cpp @@ -1,4 +1,4 @@ -#include "panoptic_mapping_utils/scannet/mesh_saver.h" +#include "panoptic_mapping_utils/mesh_saver.h" namespace panoptic_mapping { From d6b77244261704cff8ed8f67f95b8e6cb6f32f37 Mon Sep 17 00:00:00 2001 From: Hermann Date: Tue, 8 Nov 2022 06:27:43 +0100 Subject: [PATCH 3/4] add mesh saver documentation --- panoptic_mapping_utils/README.md | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/panoptic_mapping_utils/README.md b/panoptic_mapping_utils/README.md index bafc5e19..d3599d93 100644 --- a/panoptic_mapping_utils/README.md +++ b/panoptic_mapping_utils/README.md @@ -1,2 +1,17 @@ # Panoptic Mapping Utils -Utility tools and scripts around **panoptic_mapping**. Playing and creating datasets and similar. \ No newline at end of file +Utility tools and scripts around **panoptic_mapping**. Playing and creating datasets and similar. + +## Mesh Saver + +Run the mesh saver to save the same mesh that is visualised in RVIZ as a `.ply` file: + +``` + + + + +``` From 4fecf646ca7af7fced297bd09ee9e432ac921e95 Mon Sep 17 00:00:00 2001 From: Hermann <5478501+hermannsblum@users.noreply.github.com> Date: Wed, 23 Nov 2022 14:16:31 +0100 Subject: [PATCH 4/4] don't use space in rosparam --- panoptic_mapping_utils/src/mesh_saver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panoptic_mapping_utils/src/mesh_saver.cpp b/panoptic_mapping_utils/src/mesh_saver.cpp index 2b7a98a3..8f766adc 100644 --- a/panoptic_mapping_utils/src/mesh_saver.cpp +++ b/panoptic_mapping_utils/src/mesh_saver.cpp @@ -99,7 +99,7 @@ void MeshSaver::gotMeshCallback(const voxblox_msgs::MultiMesh& msg) { } // save mesh std::string output_path = - nh_.param("output path", "/tmp/map_mesh.ply"); + nh_.param("output_path", "/tmp/map_mesh.ply"); voxblox::outputMeshAsPly(output_path, full_mesh); std::cout << "mesh saved to " << output_path << std::endl; }