From 2a2286deb05c0ea5a35fffd49e24dfed6ffd0559 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Tue, 24 Jan 2017 15:57:37 +0100 Subject: [PATCH] Assets pipeline demo and track Cartographer API (#253) Adds a more complete assets_writer_backpack_2d.lua including many comments. --- .../cartographer_ros/assets_writer_main.cc | 24 +--- .../configuration_files/assets_writer.lua | 90 ------------ .../assets_writer_backpack_2d.lua | 130 ++++++++++++++++++ .../launch/assets_writer_backpack_2d.launch | 3 +- 4 files changed, 134 insertions(+), 113 deletions(-) delete mode 100644 cartographer_ros/configuration_files/assets_writer.lua create mode 100644 cartographer_ros/configuration_files/assets_writer_backpack_2d.lua diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index 9ed021a..dcd8fff 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/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; - 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}}); + batch->intensities.push_back(point_cloud.intensities[i]); } 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, - &builder); + carto::io::RegisterBuiltInPointsProcessors(trajectory_proto, + file_writer_factory, &builder); std::vector> pipeline = builder.CreatePipeline( lua_parameter_dictionary.GetDictionary("pipeline").get()); diff --git a/cartographer_ros/configuration_files/assets_writer.lua b/cartographer_ros/configuration_files/assets_writer.lua deleted file mode 100644 index e66e465..0000000 --- a/cartographer_ros/configuration_files/assets_writer.lua +++ /dev/null @@ -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 diff --git a/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua b/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua new file mode 100644 index 0000000..0b853e4 --- /dev/null +++ b/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 diff --git a/cartographer_ros/launch/assets_writer_backpack_2d.launch b/cartographer_ros/launch/assets_writer_backpack_2d.launch index 41c12e1..4f0e605 100644 --- a/cartographer_ros/launch/assets_writer_backpack_2d.launch +++ b/cartographer_ros/launch/assets_writer_backpack_2d.launch @@ -18,9 +18,8 @@