From cd1276a99ccffcc491d0b2e50296ec04347ba5f2 Mon Sep 17 00:00:00 2001
From: Wolfgang Hess <whess@google.com>
Date: Wed, 24 May 2017 15:27:52 +0200
Subject: [PATCH] Add an assets pipeline demo for 3D. (#352)

---
 .../assets_writer_backpack_2d.lua             | 14 +++
 .../assets_writer_backpack_3d.lua             | 99 +++++++++++++++++++
 .../launch/assets_writer_backpack_3d.launch   | 27 +++++
 3 files changed, 140 insertions(+)
 create mode 100644 cartographer_ros/configuration_files/assets_writer_backpack_3d.lua
 create mode 100644 cartographer_ros/launch/assets_writer_backpack_3d.launch

diff --git a/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua b/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua
index 0b853e4..95786c1 100644
--- a/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua
+++ b/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua
@@ -1,3 +1,17 @@
+-- Copyright 2016 The Cartographer Authors
+--
+-- Licensed under the Apache License, Version 2.0 (the "License");
+-- you may not use this file except in compliance with the License.
+-- You may obtain a copy of the License at
+--
+--      http://www.apache.org/licenses/LICENSE-2.0
+--
+-- Unless required by applicable law or agreed to in writing, software
+-- distributed under the License is distributed on an "AS IS" BASIS,
+-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+-- See the License for the specific language governing permissions and
+-- limitations under the License.
+
 -- WARNING: we create a lot of X-Rays of a potentially large space in this
 -- pipeline. For example, running over the
 -- cartographer_paper_deutsches_museum.bag requires ~25GiB of memory. You can
diff --git a/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua b/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua
new file mode 100644
index 0000000..b831b3c
--- /dev/null
+++ b/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua
@@ -0,0 +1,99 @@
+-- Copyright 2016 The Cartographer Authors
+--
+-- Licensed under the Apache License, Version 2.0 (the "License");
+-- you may not use this file except in compliance with the License.
+-- You may obtain a copy of the License at
+--
+--      http://www.apache.org/licenses/LICENSE-2.0
+--
+-- Unless required by applicable law or agreed to in writing, software
+-- distributed under the License is distributed on an "AS IS" BASIS,
+-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+-- See the License for the specific language governing permissions and
+-- limitations under the License.
+
+VOXEL_SIZE = 5e-2
+
+XY_TRANSFORM =  {
+  translation = { 0., 0., 0. },
+  rotation = { 0., -math.pi / 2., 0., },
+}
+
+XZ_TRANSFORM =  {
+  translation = { 0., 0., 0. },
+  rotation = { 0. , 0., -math.pi / 2, },
+}
+
+YZ_TRANSFORM =  {
+  translation = { 0., 0., 0. },
+  rotation = { 0. , 0., math.pi, },
+}
+
+options = {
+  tracking_frame = "base_link",
+  pipeline = {
+    {
+      action = "min_max_range_filter",
+      min_range = 1.,
+      max_range = 60.,
+    },
+    {
+      action = "dump_num_points",
+    },
+
+    -- Gray X-Rays. These only use geometry to color pixels.
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_yz_all",
+      transform = YZ_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xy_all",
+      transform = XY_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xz_all",
+      transform = XZ_TRANSFORM,
+    },
+
+    -- Now we recolor our points by frame and write another batch of X-Rays. It
+    -- is visible in them what was seen by the horizontal and the vertical
+    -- laser.
+    {
+      action = "color_points",
+      frame_id = "horizontal_vlp16_link",
+      color = { 255., 0., 0. },
+    },
+    {
+      action = "color_points",
+      frame_id = "vertical_vlp16_link",
+      color = { 0., 255., 0. },
+    },
+
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_yz_all_color",
+      transform = YZ_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xy_all_color",
+      transform = XY_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xz_all_color",
+      transform = XZ_TRANSFORM,
+    },
+  }
+}
+
+return options
diff --git a/cartographer_ros/launch/assets_writer_backpack_3d.launch b/cartographer_ros/launch/assets_writer_backpack_3d.launch
new file mode 100644
index 0000000..0472219
--- /dev/null
+++ b/cartographer_ros/launch/assets_writer_backpack_3d.launch
@@ -0,0 +1,27 @@
+<!--
+  Copyright 2016 The Cartographer Authors
+
+  Licensed under the Apache License, Version 2.0 (the "License");
+  you may not use this file except in compliance with the License.
+  You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+  Unless required by applicable law or agreed to in writing, software
+  distributed under the License is distributed on an "AS IS" BASIS,
+  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  See the License for the specific language governing permissions and
+  limitations under the License.
+-->
+
+<launch>
+  <node name="cartographer_assets_writer" pkg="cartographer_ros" required="true"
+      type="cartographer_assets_writer" args="
+          -configuration_directory $(find cartographer_ros)/configuration_files
+          -configuration_basename assets_writer_backpack_3d.lua
+          -urdf_filename $(find cartographer_ros)/urdf/backpack_3d.urdf
+          -bag_filename $(arg bag_filenames)
+          -trajectory_filename $(arg trajectory_filename)"
+      output="screen">
+  </node>
+</launch>