From c86d5e48f5da23840dbb6547af68b53d11ad2faa Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Wed, 21 Dec 2016 11:39:01 +0100 Subject: [PATCH] Add support for making assets out of LaserScan and MultiEchoLaserScan. (#235) --- .../cartographer_ros/assets_writer_main.cc | 93 +++++++++++++++---- .../configuration_files/assets_writer.lua | 34 +++---- .../launch/assets_writer_backpack_2d.launch | 28 ++++++ 3 files changed, 116 insertions(+), 39 deletions(-) create mode 100644 cartographer_ros/launch/assets_writer_backpack_2d.launch diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index 3d0f83d..362fa3a 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -14,6 +14,7 @@ * limitations under the License. */ +#include #include #include #include @@ -21,8 +22,11 @@ #include "cartographer/common/configuration_file_resolver.h" #include "cartographer/common/make_unique.h" +#include "cartographer/common/math.h" #include "cartographer/io/points_processor.h" #include "cartographer/io/points_processor_pipeline_builder.h" +#include "cartographer/sensor/laser.h" +#include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/transform_interpolation_buffer.h" #include "cartographer_ros/bag_reader.h" #include "cartographer_ros/msg_conversion.h" @@ -46,6 +50,14 @@ 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."); @@ -59,13 +71,38 @@ namespace { namespace carto = ::cartographer; -void HandlePointCloud2Message( - const sensor_msgs::PointCloud2::ConstPtr& msg, const string& tracking_frame, +carto::sensor::PointCloudWithIntensities ToPointCloudWithIntensities( + const sensor_msgs::PointCloud2::ConstPtr& message) { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(*message, pcl_point_cloud); + carto::sensor::PointCloudWithIntensities point_cloud; + + // TODO(hrapp): How to get reflectivities from PCL? + for (const auto& point : pcl_point_cloud) { + point_cloud.points.emplace_back(point.x, point.y, point.z); + point_cloud.intensities.push_back(1.); + } + return point_cloud; +} + +carto::sensor::PointCloudWithIntensities ToPointCloudWithIntensities( + const sensor_msgs::MultiEchoLaserScan::ConstPtr& message) { + return carto::sensor::ToPointCloudWithIntensities(ToCartographer(*message)); +} + +carto::sensor::PointCloudWithIntensities ToPointCloudWithIntensities( + const sensor_msgs::LaserScan::ConstPtr& message) { + return carto::sensor::ToPointCloudWithIntensities(ToCartographer(*message)); +} + +template +void HandleMessage( + const T& message, const string& tracking_frame, const tf2_ros::Buffer& tf_buffer, const carto::transform::TransformInterpolationBuffer& transform_interpolation_buffer, const std::vector>& pipeline) { - const carto::common::Time time = FromRos(msg->header.stamp); + const carto::common::Time time = FromRos(message->header.stamp); if (!transform_interpolation_buffer.Has(time)) { return; } @@ -73,21 +110,33 @@ void HandlePointCloud2Message( const carto::transform::Rigid3d tracking_to_map = transform_interpolation_buffer.Lookup(time); const carto::transform::Rigid3d sensor_to_tracking = - ToRigid3d(tf_buffer.lookupTransform(tracking_frame, msg->header.frame_id, - msg->header.stamp)); + ToRigid3d(tf_buffer.lookupTransform( + tracking_frame, message->header.frame_id, message->header.stamp)); const carto::transform::Rigid3f sensor_to_map = (tracking_to_map * sensor_to_tracking).cast(); auto batch = carto::common::make_unique(); batch->time = time; batch->origin = sensor_to_map * Eigen::Vector3f::Zero(); - batch->frame_id = msg->header.frame_id; + batch->frame_id = message->header.frame_id; - pcl::PointCloud pcl_point_cloud; - pcl::fromROSMsg(*msg, pcl_point_cloud); - for (const auto& point : pcl_point_cloud) { - batch->points.push_back(sensor_to_map * - Eigen::Vector3f(point.x, point.y, point.z)); + carto::sensor::PointCloudWithIntensities point_cloud = + ToPointCloudWithIntensities(message); + CHECK(point_cloud.intensities.size() == point_cloud.points.size()); + + 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}}); } pipeline.back()->Process(std::move(batch)); } @@ -134,14 +183,24 @@ void Run(const string& trajectory_filename, const string& bag_filename, const ::ros::Time begin_time = view.getBeginTime(); const double duration_in_seconds = (view.getEndTime() - begin_time).toSec(); - for (const rosbag::MessageInstance& msg : view) { - if (msg.isType()) { - HandlePointCloud2Message(msg.instantiate(), - tracking_frame, *tf_buffer, - *transform_interpolation_buffer, pipeline); + for (const rosbag::MessageInstance& message : view) { + if (message.isType()) { + HandleMessage(message.instantiate(), + tracking_frame, *tf_buffer, + *transform_interpolation_buffer, pipeline); + } + if (message.isType()) { + HandleMessage(message.instantiate(), + tracking_frame, *tf_buffer, + *transform_interpolation_buffer, pipeline); + } + if (message.isType()) { + HandleMessage(message.instantiate(), + tracking_frame, *tf_buffer, + *transform_interpolation_buffer, pipeline); } LOG_EVERY_N(INFO, 100000) - << "Processed " << (msg.getTime() - begin_time).toSec() << " of " + << "Processed " << (message.getTime() - begin_time).toSec() << " of " << duration_in_seconds << " bag time seconds..."; } bag.close(); diff --git a/cartographer_ros/configuration_files/assets_writer.lua b/cartographer_ros/configuration_files/assets_writer.lua index 4ef0fd6..e66e465 100644 --- a/cartographer_ros/configuration_files/assets_writer.lua +++ b/cartographer_ros/configuration_files/assets_writer.lua @@ -7,24 +7,18 @@ options = { -- action = "fixed_ratio_sampler", -- sampling_ratio = 1., -- }, - -- { - -- action = "min_max_range_filter", - -- min_range = 1., - -- max_range = 150., - -- }, + { + 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 = "color_with_panos", - -- run_ids = { - -- "20120119_193543_L19220", - -- }, - -- }, + { + action = "dump_num_points", + }, { action = "write_xray_image", voxel_size = VOXEL_SIZE, @@ -86,14 +80,10 @@ options = { -- action = "write_xyz", -- filename = "points.xyz", -- }, - -- { - -- action = "write_octree", - -- directory = "octree", - -- }, - -- { - -- action = "write_ply", - -- filename = "points.ply", - -- }, + { + action = "write_ply", + filename = "points.ply", + }, } } diff --git a/cartographer_ros/launch/assets_writer_backpack_2d.launch b/cartographer_ros/launch/assets_writer_backpack_2d.launch new file mode 100644 index 0000000..41c12e1 --- /dev/null +++ b/cartographer_ros/launch/assets_writer_backpack_2d.launch @@ -0,0 +1,28 @@ + + + + + +