Assets pipeline demo and track Cartographer API (#253)

Adds a more complete assets_writer_backpack_2d.lua including many
comments.

cartographer_ros/cartographer_ros/assets_writer_main.cc

@@ -51,14 +51,6 @@ DEFINE_string(configuration_directory, "",
 DEFINE_string(configuration_basename, "",
               "Basename, i.e. not containing any directory prefix, of the "
               "configuration file.");
-DEFINE_int32(laser_intensity_min, 0,
-             "Laser intensities are device specific. Some assets require a "
-             "useful normalized value for it though, which has to be specified "
-             "manually.");
-DEFINE_int32(laser_intensity_max, 255, "See 'laser_intensity_min'.");
-DEFINE_int32(fake_intensity, 0,
-             "If non-zero, ignore intensities in the laser scan and use this "
-             "value for all. Ignores 'laser_intensity_*'");
 DEFINE_string(
     urdf_filename, "",
     "URDF file that contains static links for your sensor configuration.");
@@ -127,17 +119,7 @@ void HandleMessage(
 
   for (int i = 0; i < point_cloud.points.size(); ++i) {
     batch->points.push_back(sensor_to_map * point_cloud.points[i]);
-    uint8_t gray;
+    batch->intensities.push_back(point_cloud.intensities[i]);
-    if (FLAGS_fake_intensity) {
-      gray = FLAGS_fake_intensity;
-    } else {
-      gray = cartographer::common::Clamp(
-                 (point_cloud.intensities[i] - FLAGS_laser_intensity_min) /
-                     (FLAGS_laser_intensity_max - FLAGS_laser_intensity_min),
-                 0.f, 1.f) *
-             255;
-    }
-    batch->colors.push_back({{gray, gray, gray}});
   }
   pipeline.back()->Process(std::move(batch));
 }
@@ -162,8 +144,8 @@ void Run(const string& trajectory_filename, const string& bag_filename,
   };
 
   carto::io::PointsProcessorPipelineBuilder builder;
-  carto::io::RegisterBuiltInPointsProcessors(trajectory_proto, file_writer_factory,
+  carto::io::RegisterBuiltInPointsProcessors(trajectory_proto,
-                                             &builder);
+                                             file_writer_factory, &builder);
   std::vector<std::unique_ptr<carto::io::PointsProcessor>> pipeline =
       builder.CreatePipeline(
           lua_parameter_dictionary.GetDictionary("pipeline").get());

cartographer_ros/configuration_files/assets_writer.lua

@@ -1,90 +0,0 @@
-VOXEL_SIZE = 5e-2
-
-options = {
-  tracking_frame = "base_link",
-  pipeline = {
-    -- {
-      -- action = "fixed_ratio_sampler",
-      -- sampling_ratio = 1.,
-    -- },
-    {
-      action = "min_max_range_filter",
-      min_range = 1.,
-      max_range = 60.,
-    },
-    -- {
-      -- action = "voxel_filter_and_remove_moving_objects",
-      -- voxel_size = VOXEL_SIZE,
-    -- },
-    {
-      action = "dump_num_points",
-    },
-    {
-      action = "write_xray_image",
-      voxel_size = VOXEL_SIZE,
-      filename = "xray_yz_all",
-      transform = {
-        translation = { 0., 0., 0. },
-        rotation = { 0. , 0., math.pi, },
-      },
-    },
-    {
-      action = "write_xray_image",
-      voxel_size = VOXEL_SIZE,
-      filename = "xray_xy_all",
-      transform = {
-        translation = { 0., 0., 0. },
-        rotation = { 0., -math.pi / 2., 0., },
-      },
-    },
-    {
-      action = "write_xray_image",
-      voxel_size = VOXEL_SIZE,
-      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, },
-      },
-    },
-    -- {
-      -- action = "write_xyz",
-      -- filename = "points.xyz",
-    -- },
-    {
-      action = "write_ply",
-      filename = "points.ply",
-    },
-  }
-}
-
-return options

cartographer_ros/configuration_files/assets_writer_backpack_2d.lua

@@ -0,0 +1,130 @@
+-- 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
+-- reduce this by writing fewer X-Rays or upping VOXEL_SIZE - which is the size
+-- of a pixel in a X-Ray.
+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,
+    },
+
+    -- We now use the intensities to color our points. We apply a linear
+    -- transform to clamp our intensity values into [0, 255] and then use this
+    -- value for RGB of our points. Every stage in the pipeline after this now
+    -- receives colored points.
+    --
+    -- We write xrays again. These now use geometry and the intensities to
+    -- color pixels - they look quite similar, just a little lighter.
+    {
+      action = "intensity_to_color",
+      min_intensity = 0.,
+      max_intensity = 4095.,
+    },
+
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_yz_all_intensity",
+      transform = YZ_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xy_all_intensity",
+      transform = XY_TRANSFORM,
+    },
+    {
+      action = "write_xray_image",
+      voxel_size = VOXEL_SIZE,
+      filename = "xray_xz_all_intensity",
+      transform = XZ_TRANSFORM,
+    },
+
+    -- We also write a PLY file at this stage, because gray points look good.
+    -- The points in the PLY can be visualized using
+    -- https://github.com/googlecartographer/point_cloud_viewer.
+    {
+      action = "write_ply",
+      filename = "points.ply",
+    },
+
+    -- 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_laser_link",
+      color = { 255., 0., 0. },
+    },
+    {
+      action = "color_points",
+      frame_id = "vertical_laser_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

cartographer_ros/launch/assets_writer_backpack_2d.launch

@@ -18,9 +18,8 @@
   <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.lua
+          -configuration_basename assets_writer_backpack_2d.lua
           -urdf_filename $(find cartographer_ros)/urdf/backpack_2d.urdf
-          -laser_intensity_max=4095
           -bag_filename $(arg bag_filenames)
           -trajectory_filename $(arg trajectory_filename)"
       output="screen">