From 5046bc02cc2511cc7ebb8c6ba8d575eccf28bcad Mon Sep 17 00:00:00 2001
From: Holger Rapp <HolgerRapp@gmx.net>
Date: Thu, 17 Nov 2016 02:17:52 -0800
Subject: [PATCH] Adapt to new Cartographer API. (#169)

And write by-floor X-Rays in the asset writer.
---
 .../cartographer_ros/assets_writer.cc         |  6 +--
 .../cartographer_ros/assets_writer_main.cc    | 22 +++++------
 .../configuration_files/assets_writer.lua     | 38 +++++++++++++++++--
 3 files changed, 48 insertions(+), 18 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc
index d3ec13c..f9c9756 100644
--- a/cartographer_ros/cartographer_ros/assets_writer.cc
+++ b/cartographer_ros/cartographer_ros/assets_writer.cc
@@ -51,15 +51,15 @@ void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
   carto::io::XRayPointsProcessor xy_xray_points_processor(
       voxel_size, carto::transform::Rigid3f::Rotation(
                       Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())),
-      stem + "_xray_xy.png", &null_points_processor);
+      {}, stem + "_xray_xy.png", &null_points_processor);
   carto::io::XRayPointsProcessor yz_xray_points_processor(
       voxel_size, carto::transform::Rigid3f::Rotation(
                       Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())),
-      stem + "_xray_yz.png", &xy_xray_points_processor);
+      {}, stem + "_xray_yz.png", &xy_xray_points_processor);
   carto::io::XRayPointsProcessor xz_xray_points_processor(
       voxel_size, carto::transform::Rigid3f::Rotation(
                       Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
-      stem + "_xray_xz.png", &yz_xray_points_processor);
+      {}, stem + "_xray_xz.png", &yz_xray_points_processor);
   carto::io::PlyWritingPointsProcessor ply_writing_points_processor(
       stem + ".ply", &xz_xray_points_processor);
 
diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc
index 8edc3cc..360e0fd 100644
--- a/cartographer_ros/cartographer_ros/assets_writer_main.cc
+++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc
@@ -56,15 +56,6 @@ namespace {
 
 namespace carto = ::cartographer;
 
-std::unique_ptr<carto::transform::TransformInterpolationBuffer> ReadTrajectory(
-    const std::string& trajectory_filename) {
-  std::ifstream stream(trajectory_filename.c_str());
-  carto::mapping::proto::Trajectory trajectory_proto;
-  CHECK(trajectory_proto.ParseFromIstream(&stream));
-  return carto::transform::TransformInterpolationBuffer::FromTrajectory(
-      trajectory_proto);
-}
-
 void ReadStaticTransformsFromUrdf(const string& urdf_filename,
                                   ::tf2_ros::Buffer* const buffer) {
   urdf::Model model;
@@ -99,9 +90,16 @@ void Run(const string& trajectory_filename, const string& bag_filename,
   ::tf2_ros::Buffer buffer;
   ReadStaticTransformsFromUrdf(urdf_filename, &buffer);
 
-  auto transform_interpolation_buffer = ReadTrajectory(trajectory_filename);
+  std::ifstream stream(trajectory_filename.c_str());
+  carto::mapping::proto::Trajectory trajectory_proto;
+  CHECK(trajectory_proto.ParseFromIstream(&stream));
 
-  auto* builder = carto::io::PointsProcessorPipelineBuilder::instance();
+  auto transform_interpolation_buffer =
+      carto::transform::TransformInterpolationBuffer::FromTrajectory(
+          trajectory_proto);
+
+  carto::io::PointsProcessorPipelineBuilder builder;
+  carto::io::RegisterBuiltInPointsProcessors(trajectory_proto, &builder);
 
   auto file_resolver =
       carto::common::make_unique<carto::common::ConfigurationFileResolver>(
@@ -114,7 +112,7 @@ void Run(const string& trajectory_filename, const string& bag_filename,
       lua_parameter_dictionary.GetString("tracking_frame");
 
   std::vector<std::unique_ptr<carto::io::PointsProcessor>> pipeline =
-      builder->CreatePipeline(
+      builder.CreatePipeline(
           lua_parameter_dictionary.GetDictionary("pipeline").get());
 
   do {
diff --git a/cartographer_ros/configuration_files/assets_writer.lua b/cartographer_ros/configuration_files/assets_writer.lua
index 72197f1..c992f5f 100644
--- a/cartographer_ros/configuration_files/assets_writer.lua
+++ b/cartographer_ros/configuration_files/assets_writer.lua
@@ -1,4 +1,6 @@
 VOXEL_SIZE = 5e-2
+
+
 options = {
   tracking_frame = "base_link",
   pipeline = {
@@ -27,7 +29,7 @@ options = {
     {
       action = "write_xray_image",
       voxel_size = VOXEL_SIZE,
-      filename = "xray_yz.png",
+      filename = "xray_yz_all",
       transform = {
         translation = { 0., 0., 0. },
         rotation = { 0. , 0., math.pi, },
@@ -36,7 +38,7 @@ options = {
     {
       action = "write_xray_image",
       voxel_size = VOXEL_SIZE,
-      filename = "xray_xy.png",
+      filename = "xray_xy_all",
       transform = {
         translation = { 0., 0., 0. },
         rotation = { 0., -math.pi / 2., 0., },
@@ -45,7 +47,37 @@ options = {
     {
       action = "write_xray_image",
       voxel_size = VOXEL_SIZE,
-      filename = "xray_xz.png",
+      filename = "xray_xz_all",
+      transform = {
+        translation = { 0., 0., 0. },
+        rotation = { 0. , 0., -math.pi / 2, },
+      },
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      separate_floors = true,
+      filename = "xray_yz_level_",
+      transform = {
+        translation = { 0., 0., 0. },
+        rotation = { 0. , 0., math.pi, },
+      },
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      separate_floors = true,
+      filename = "xray_xy_level_",
+      transform = {
+        translation = { 0., 0., 0. },
+        rotation = { 0., -math.pi / 2., 0., },
+      },
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      separate_floors = true,
+      filename = "xray_xz_level_",
       transform = {
         translation = { 0., 0., 0. },
         rotation = { 0. , 0., -math.pi / 2, },