Skip to content

Commit 61b578a

Browse files
SchmlukSchmluk
authored
Merge pull request #54 from ethz-asl/feature/mesh-saver-2
node that saves the meshes send to RVIZ
2 parents 4b62754 + 4fecf64 commit 61b578a

File tree

6 files changed

+229
-1
lines changed

6 files changed

+229
-1
lines changed

‎panoptic_mapping_utils/CMakeLists.txt

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@ catkin_package()
1212

1313
cs_add_library(${PROJECT_NAME}
1414
src/evaluation/map_evaluator.cpp
15+
src/mesh_saver.cpp
1516
)
1617

1718
###############
@@ -28,6 +29,11 @@ cs_add_executable(multi_map_evaluation
2829
)
2930
target_link_libraries(multi_map_evaluation ${PROJECT_NAME})
3031

32+
cs_add_executable(mesh_saver
33+
app/mesh_saver_node.cpp
34+
)
35+
target_link_libraries(mesh_saver ${PROJECT_NAME})
36+
3137
##########
3238
# Export #
3339
##########

‎panoptic_mapping_utils/README.md

Lines changed: 16 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,2 +1,17 @@
11
# Panoptic Mapping Utils
2-
Utility tools and scripts around **panoptic_mapping**. Playing and creating datasets and similar.
2+
Utility tools and scripts around **panoptic_mapping**. Playing and creating datasets and similar.
3+
4+
## Mesh Saver
5+
6+
Run the mesh saver to save the same mesh that is visualised in RVIZ as a `.ply` file:
7+
8+
```
9+
<node pkg="panoptic_mapping_utils"
10+
type="mesh_saver"
11+
name="mesh_saver"
12+
output="screen"
13+
required="true">
14+
<param name="topic" value="/panoptic_mapper/visualization/submaps/mesh" />
15+
<param name="output path" value="/tmp/mesh.ply" />
16+
</node>
17+
```

‎panoptic_mapping_utils/app/mesh_saver_node.cpp

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
#include <glog/logging.h>
2+
#include <ros/ros.h>
3+
4+
#include "panoptic_mapping_utils/mesh_saver.h"
5+
6+
int main(int argc, char** argv) {
7+
// Start Ros.
8+
ros::init(argc, argv, "mesh_saver", ros::init_options::NoSigintHandler);
9+
10+
11+
// Setup logging.
12+
google::InitGoogleLogging(argv[0]);
13+
google::InstallFailureSignalHandler();
14+
google::ParseCommandLineFlags(&argc, &argv, false);
15+
16+
// Setup node.
17+
ros::NodeHandle nh_private("~");
18+
panoptic_mapping::MeshSaver mesh_saver(nh_private);
19+
ros::spin();
20+
ros::waitForShutdown();
21+
return 0;
22+
}

‎panoptic_mapping_utils/include/panoptic_mapping_utils/mesh_saver.h

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
#ifndef PANOPTIC_MAPPING_UTILS_SCANNET_MESHSAVER_H_
2+
#define PANOPTIC_MAPPING_UTILS_SCANNET_MESHSAVER_H_
3+
4+
#include <string>
5+
#include <vector>
6+
7+
#include <ros/ros.h>
8+
9+
#include "voxblox/core/block_hash.h"
10+
#include "voxblox/core/common.h"
11+
#include "voxblox/mesh/mesh.h"
12+
#include "voxblox/mesh/mesh_utils.h"
13+
#include "voxblox/io/mesh_ply.h"
14+
#include <voxblox_msgs/MultiMesh.h>
15+
#include <voxblox_ros/mesh_vis.h>
16+
17+
namespace panoptic_mapping {
18+
19+
class MeshSaver {
20+
public:
21+
MeshSaver(const ros::NodeHandle& nh);
22+
virtual ~MeshSaver() = default;
23+
void gotMeshCallback(const voxblox_msgs::MultiMesh& msg);
24+
25+
private:
26+
void setupRos();
27+
28+
ros::NodeHandle nh_;
29+
30+
ros::Subscriber mesh_sub_;
31+
};
32+
33+
} // namespace panoptic_mapping
34+
35+
#endif

‎panoptic_mapping_utils/launch/scannnet_visualizer.launch

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
<launch>
2+
<!-- ============ Arguments ============ -->
3+
<arg name="scene"/>
4+
<arg name="model"/>
5+
<arg name="data_path" default="/media/blumh/data/scannet/valscans/$(arg scene)"/>
6+
<arg name="inference_path" default="/media/blumh/data/clustering_outputs/scannet_inference/$(arg scene)/$(arg model)"/>
7+
<arg name="mapname" default="pred" />
8+
<arg name="wait" default="5"/>
9+
<arg name="namespace" default="data"/>
10+
<arg name="global_frame_name" default="world"/>
11+
<arg name="sensor_frame_name" default="depthcam"/>
12+
13+
14+
<!-- Mapper -->
15+
<node name="panoptic_mapper" pkg="panoptic_mapping_ros" type="panoptic_mapper_node" output="screen" required="true">
16+
<!-- Config -->
17+
<rosparam file="$(find panoptic_mapping_ros)/config/mapper/scannetv2_single_tsdf.yaml"/>
18+
<param name="save_map_path_when_finished" value=""/>
19+
20+
<!-- Input -->
21+
<remap from="color_image_in" to="$(arg namespace)/color_image"/>
22+
<remap from="depth_image_in" to="$(arg namespace)/depth_image"/>
23+
<remap from="segmentation_image_in" to="$(arg namespace)/segmentation_image"/>
24+
</node>
25+
26+
<!-- Map loader -->
27+
<node name="map_loader" pkg="panoptic_mapping_utils" type="map_loader.py" output="screen">
28+
<param name="path" value="$(arg inference_path)/$(arg mapname).panmap" />
29+
<param name="srv_name" value="/panoptic_mapper/load_map" />
30+
<param name="delay" value="0.1" />
31+
</node>
32+
33+
<node pkg="panoptic_mapping_utils"
34+
type="mesh_saver"
35+
name="mesh_saver"
36+
output="screen"
37+
required="true">
38+
<param name="topic" value="/panoptic_mapper/visualization/submaps/mesh" />
39+
</node>
40+
41+
<!-- RVIZ Visualization-->
42+
<node type="rviz" name="rviz" pkg="rviz" args="-d $(find panoptic_mapping_ros)/config/rviz/devel.rviz" output="screen"/>
43+
44+
</launch>

‎panoptic_mapping_utils/src/mesh_saver.cpp

Lines changed: 106 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,106 @@
1+
#include "panoptic_mapping_utils/mesh_saver.h"
2+
3+
namespace panoptic_mapping {
4+
5+
MeshSaver::MeshSaver(const ros::NodeHandle& nh) : nh_(nh) { setupRos(); }
6+
void MeshSaver::setupRos() {
7+
mesh_sub_ =
8+
nh_.subscribe(nh_.param<std::string>(
9+
"topic", "/panoptic_mapper/visualization/submaps/mesh"),
10+
10, &MeshSaver::gotMeshCallback, this);
11+
}
12+
13+
void MeshSaver::gotMeshCallback(const voxblox_msgs::MultiMesh& msg) {
14+
voxblox::Mesh full_mesh;
15+
bool first = true;
16+
17+
for (const voxblox_msgs::MeshBlock& mesh_block : msg.mesh.mesh_blocks) {
18+
const voxblox::BlockIndex index(mesh_block.index[0], mesh_block.index[1],
19+
mesh_block.index[2]);
20+
21+
if (mesh_block.x.size() == 0) {
22+
continue;
23+
}
24+
25+
size_t vertex_index = 0u;
26+
voxblox::Mesh mesh;
27+
mesh.vertices.reserve(mesh_block.x.size());
28+
mesh.indices.reserve(mesh_block.x.size());
29+
30+
// translate vertex data from message to voxblox mesh
31+
for (size_t i = 0; i < mesh_block.x.size(); ++i) {
32+
// Each vertex is given as its distance from the blocks origin in units of
33+
// (2*block_size), see mesh_vis.h for the slightly convoluted
34+
// justification of the 2.
35+
constexpr float point_conv_factor =
36+
2.0f / std::numeric_limits<uint16_t>::max();
37+
const float mesh_x =
38+
(static_cast<float>(mesh_block.x[i]) * point_conv_factor +
39+
static_cast<float>(index[0])) *
40+
msg.mesh.block_edge_length;
41+
const float mesh_y =
42+
(static_cast<float>(mesh_block.y[i]) * point_conv_factor +
43+
static_cast<float>(index[1])) *
44+
msg.mesh.block_edge_length;
45+
const float mesh_z =
46+
(static_cast<float>(mesh_block.z[i]) * point_conv_factor +
47+
static_cast<float>(index[2])) *
48+
msg.mesh.block_edge_length;
49+
50+
mesh.indices.push_back(vertex_index++);
51+
mesh.vertices.emplace_back(mesh_x, mesh_y, mesh_z);
52+
}
53+
54+
// calculate normals
55+
mesh.normals.reserve(mesh.vertices.size());
56+
for (size_t i = 0; i < mesh.vertices.size(); i += 3) {
57+
const voxblox::Point dir0 = mesh.vertices[i] - mesh.vertices[i + 1];
58+
const voxblox::Point dir1 = mesh.vertices[i] - mesh.vertices[i + 2];
59+
const voxblox::Point normal = dir0.cross(dir1).normalized();
60+
61+
mesh.normals.push_back(normal);
62+
mesh.normals.push_back(normal);
63+
mesh.normals.push_back(normal);
64+
}
65+
66+
// add color information
67+
mesh.colors.reserve(mesh.vertices.size());
68+
const bool has_color = mesh_block.x.size() == mesh_block.r.size();
69+
for (size_t i = 0; i < mesh_block.x.size(); ++i) {
70+
voxblox::Color color;
71+
if (has_color) {
72+
color.r = mesh_block.r[i];
73+
color.g = mesh_block.g[i];
74+
color.b = mesh_block.b[i];
75+
76+
} else {
77+
// reconstruct normals coloring
78+
color.r = std::numeric_limits<uint8_t>::max() *
79+
(mesh.normals[i].x() * 0.5f + 0.5f);
80+
color.g = std::numeric_limits<uint8_t>::max() *
81+
(mesh.normals[i].y() * 0.5f + 0.5f);
82+
color.b = std::numeric_limits<uint8_t>::max() *
83+
(mesh.normals[i].z() * 0.5f + 0.5f);
84+
}
85+
color.a = 1.0;
86+
mesh.colors.push_back(color);
87+
}
88+
89+
// connect mesh
90+
91+
if (first) {
92+
voxblox::createConnectedMesh(mesh, &full_mesh);
93+
first = false;
94+
} else {
95+
voxblox::Mesh connected_mesh;
96+
voxblox::createConnectedMesh(mesh, &connected_mesh);
97+
full_mesh.concatenateMesh(connected_mesh);
98+
}
99+
}
100+
// save mesh
101+
std::string output_path =
102+
nh_.param<std::string>("output_path", "/tmp/map_mesh.ply");
103+
voxblox::outputMeshAsPly(output_path, full_mesh);
104+
std::cout << "mesh saved to " << output_path << std::endl;
105+
}
106+
} // namespace panoptic_mapping

0 commit comments

Comments
 (0)