diff --git a/panoptic_mapping_utils/CMakeLists.txt b/panoptic_mapping_utils/CMakeLists.txt
index ab57c7f3..4b7728e6 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/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/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:
+
+```
+
+
+
+
+```
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..a7037cb1
--- /dev/null
+++ b/panoptic_mapping_utils/app/mesh_saver_node.cpp
@@ -0,0 +1,22 @@
+#include
+#include
+
+#include "panoptic_mapping_utils/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/mesh_saver.h b/panoptic_mapping_utils/include/panoptic_mapping_utils/mesh_saver.h
new file mode 100644
index 00000000..aff30d71
--- /dev/null
+++ b/panoptic_mapping_utils/include/panoptic_mapping_utils/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/mesh_saver.cpp b/panoptic_mapping_utils/src/mesh_saver.cpp
new file mode 100644
index 00000000..8f766adc
--- /dev/null
+++ b/panoptic_mapping_utils/src/mesh_saver.cpp
@@ -0,0 +1,106 @@
+#include "panoptic_mapping_utils/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