Merge pull request #54 from ethz-asl/feature/mesh-saver-2

node that saves the meshes send to RVIZ

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 #
 ##########

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.
+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:
+
+```
+<node pkg="panoptic_mapping_utils"
+      type="mesh_saver"
+      name="mesh_saver"
+      output="screen"
+      required="true">
+  <param name="topic" value="/panoptic_mapper/visualization/submaps/mesh" />
+  <param name="output path" value="/tmp/mesh.ply" />
+</node>
+```

panoptic_mapping_utils/app/mesh_saver_node.cpp

@@ -0,0 +1,22 @@
+#include <glog/logging.h>
+#include <ros/ros.h>
+
+#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;
+}

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 <string>
+#include <vector>
+
+#include <ros/ros.h>
+
+#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 <voxblox_msgs/MultiMesh.h>
+#include <voxblox_ros/mesh_vis.h>
+
+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

panoptic_mapping_utils/launch/scannnet_visualizer.launch

@@ -0,0 +1,44 @@
+<launch>
+<!-- ============ Arguments ============ -->
+  <arg name="scene"/>
+  <arg name="model"/>
+  <arg name="data_path" default="/media/blumh/data/scannet/valscans/$(arg scene)"/>
+  <arg name="inference_path" default="/media/blumh/data/clustering_outputs/scannet_inference/$(arg scene)/$(arg model)"/>
+  <arg name="mapname" default="pred" />
+  <arg name="wait" default="5"/>
+  <arg name="namespace" default="data"/>
+  <arg name="global_frame_name" default="world"/>
+  <arg name="sensor_frame_name" default="depthcam"/>
+
+
+  <!-- Mapper -->
+  <node name="panoptic_mapper" pkg="panoptic_mapping_ros" type="panoptic_mapper_node" output="screen" required="true">
+    <!-- Config -->
+    <rosparam file="$(find panoptic_mapping_ros)/config/mapper/scannetv2_single_tsdf.yaml"/>
+    <param name="save_map_path_when_finished" value=""/>
+
+    <!-- Input -->
+    <remap from="color_image_in" to="$(arg namespace)/color_image"/>
+    <remap from="depth_image_in" to="$(arg namespace)/depth_image"/>
+    <remap from="segmentation_image_in" to="$(arg namespace)/segmentation_image"/>
+  </node>
+
+  <!-- Map loader -->
+  <node name="map_loader" pkg="panoptic_mapping_utils" type="map_loader.py" output="screen">
+     <param name="path" value="$(arg inference_path)/$(arg mapname).panmap" />
+     <param name="srv_name" value="/panoptic_mapper/load_map" />
+     <param name="delay" value="0.1" />
+  </node>
+
+   <node pkg="panoptic_mapping_utils"
+         type="mesh_saver"
+         name="mesh_saver"
+         output="screen"
+         required="true">
+    <param name="topic" value="/panoptic_mapper/visualization/submaps/mesh" />
+  </node>
+
+  <!-- RVIZ Visualization-->
+  <node type="rviz" name="rviz" pkg="rviz" args="-d $(find panoptic_mapping_ros)/config/rviz/devel.rviz" output="screen"/>
+
+</launch>

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<std::string>(
+                        "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<uint16_t>::max();
+      const float mesh_x =
+          (static_cast<float>(mesh_block.x[i]) * point_conv_factor +
+           static_cast<float>(index[0])) *
+          msg.mesh.block_edge_length;
+      const float mesh_y =
+          (static_cast<float>(mesh_block.y[i]) * point_conv_factor +
+           static_cast<float>(index[1])) *
+          msg.mesh.block_edge_length;
+      const float mesh_z =
+          (static_cast<float>(mesh_block.z[i]) * point_conv_factor +
+           static_cast<float>(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<uint8_t>::max() *
+                  (mesh.normals[i].x() * 0.5f + 0.5f);
+        color.g = std::numeric_limits<uint8_t>::max() *
+                  (mesh.normals[i].y() * 0.5f + 0.5f);
+        color.b = std::numeric_limits<uint8_t>::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<std::string>("output_path", "/tmp/map_mesh.ply");
+  voxblox::outputMeshAsPly(output_path, full_mesh);
+  std::cout << "mesh saved to " << output_path << std::endl;
+}
+}  // namespace panoptic_mapping