Add support for making assets out of LaserScan and MultiEchoLaserScan. (#235)

master
Holger Rapp 2016-12-21 11:39:01 +01:00 committed by GitHub
parent 9311be0f7a
commit c86d5e48f5
3 changed files with 116 additions and 39 deletions

View File

@ -14,6 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include <algorithm>
#include <fstream> #include <fstream>
#include <iostream> #include <iostream>
#include <string> #include <string>
@ -21,8 +22,11 @@
#include "cartographer/common/configuration_file_resolver.h" #include "cartographer/common/configuration_file_resolver.h"
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h"
#include "cartographer/io/points_processor.h" #include "cartographer/io/points_processor.h"
#include "cartographer/io/points_processor_pipeline_builder.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/transform/transform_interpolation_buffer.h"
#include "cartographer_ros/bag_reader.h" #include "cartographer_ros/bag_reader.h"
#include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/msg_conversion.h"
@ -46,6 +50,14 @@ DEFINE_string(configuration_directory, "",
DEFINE_string(configuration_basename, "", DEFINE_string(configuration_basename, "",
"Basename, i.e. not containing any directory prefix, of the " "Basename, i.e. not containing any directory prefix, of the "
"configuration file."); "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( DEFINE_string(
urdf_filename, "", urdf_filename, "",
"URDF file that contains static links for your sensor configuration."); "URDF file that contains static links for your sensor configuration.");
@ -59,13 +71,38 @@ namespace {
namespace carto = ::cartographer; namespace carto = ::cartographer;
void HandlePointCloud2Message( carto::sensor::PointCloudWithIntensities ToPointCloudWithIntensities(
const sensor_msgs::PointCloud2::ConstPtr& msg, const string& tracking_frame, const sensor_msgs::PointCloud2::ConstPtr& message) {
pcl::PointCloud<pcl::PointXYZ> 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 <typename T>
void HandleMessage(
const T& message, const string& tracking_frame,
const tf2_ros::Buffer& tf_buffer, const tf2_ros::Buffer& tf_buffer,
const carto::transform::TransformInterpolationBuffer& const carto::transform::TransformInterpolationBuffer&
transform_interpolation_buffer, transform_interpolation_buffer,
const std::vector<std::unique_ptr<carto::io::PointsProcessor>>& pipeline) { const std::vector<std::unique_ptr<carto::io::PointsProcessor>>& 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)) { if (!transform_interpolation_buffer.Has(time)) {
return; return;
} }
@ -73,21 +110,33 @@ void HandlePointCloud2Message(
const carto::transform::Rigid3d tracking_to_map = const carto::transform::Rigid3d tracking_to_map =
transform_interpolation_buffer.Lookup(time); transform_interpolation_buffer.Lookup(time);
const carto::transform::Rigid3d sensor_to_tracking = const carto::transform::Rigid3d sensor_to_tracking =
ToRigid3d(tf_buffer.lookupTransform(tracking_frame, msg->header.frame_id, ToRigid3d(tf_buffer.lookupTransform(
msg->header.stamp)); tracking_frame, message->header.frame_id, message->header.stamp));
const carto::transform::Rigid3f sensor_to_map = const carto::transform::Rigid3f sensor_to_map =
(tracking_to_map * sensor_to_tracking).cast<float>(); (tracking_to_map * sensor_to_tracking).cast<float>();
auto batch = carto::common::make_unique<carto::io::PointsBatch>(); auto batch = carto::common::make_unique<carto::io::PointsBatch>();
batch->time = time; batch->time = time;
batch->origin = sensor_to_map * Eigen::Vector3f::Zero(); 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::PointXYZ> pcl_point_cloud; carto::sensor::PointCloudWithIntensities point_cloud =
pcl::fromROSMsg(*msg, pcl_point_cloud); ToPointCloudWithIntensities(message);
for (const auto& point : pcl_point_cloud) { CHECK(point_cloud.intensities.size() == point_cloud.points.size());
batch->points.push_back(sensor_to_map *
Eigen::Vector3f(point.x, point.y, point.z)); 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)); 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 ::ros::Time begin_time = view.getBeginTime();
const double duration_in_seconds = (view.getEndTime() - begin_time).toSec(); const double duration_in_seconds = (view.getEndTime() - begin_time).toSec();
for (const rosbag::MessageInstance& msg : view) { for (const rosbag::MessageInstance& message : view) {
if (msg.isType<sensor_msgs::PointCloud2>()) { if (message.isType<sensor_msgs::PointCloud2>()) {
HandlePointCloud2Message(msg.instantiate<sensor_msgs::PointCloud2>(), HandleMessage(message.instantiate<sensor_msgs::PointCloud2>(),
tracking_frame, *tf_buffer,
*transform_interpolation_buffer, pipeline);
}
if (message.isType<sensor_msgs::MultiEchoLaserScan>()) {
HandleMessage(message.instantiate<sensor_msgs::MultiEchoLaserScan>(),
tracking_frame, *tf_buffer,
*transform_interpolation_buffer, pipeline);
}
if (message.isType<sensor_msgs::LaserScan>()) {
HandleMessage(message.instantiate<sensor_msgs::LaserScan>(),
tracking_frame, *tf_buffer, tracking_frame, *tf_buffer,
*transform_interpolation_buffer, pipeline); *transform_interpolation_buffer, pipeline);
} }
LOG_EVERY_N(INFO, 100000) 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..."; << duration_in_seconds << " bag time seconds...";
} }
bag.close(); bag.close();

View File

@ -7,24 +7,18 @@ options = {
-- action = "fixed_ratio_sampler", -- action = "fixed_ratio_sampler",
-- sampling_ratio = 1., -- sampling_ratio = 1.,
-- }, -- },
-- { {
-- action = "min_max_range_filter", action = "min_max_range_filter",
-- min_range = 1., min_range = 1.,
-- max_range = 150., max_range = 60.,
-- }, },
-- { -- {
-- action = "voxel_filter_and_remove_moving_objects", -- action = "voxel_filter_and_remove_moving_objects",
-- voxel_size = VOXEL_SIZE, -- voxel_size = VOXEL_SIZE,
-- }, -- },
-- { {
-- action = "dump_num_points", action = "dump_num_points",
-- }, },
-- {
-- action = "color_with_panos",
-- run_ids = {
-- "20120119_193543_L19220",
-- },
-- },
{ {
action = "write_xray_image", action = "write_xray_image",
voxel_size = VOXEL_SIZE, voxel_size = VOXEL_SIZE,
@ -86,14 +80,10 @@ options = {
-- action = "write_xyz", -- action = "write_xyz",
-- filename = "points.xyz", -- filename = "points.xyz",
-- }, -- },
-- { {
-- action = "write_octree", action = "write_ply",
-- directory = "octree", filename = "points.ply",
-- }, },
-- {
-- action = "write_ply",
-- filename = "points.ply",
-- },
} }
} }

View File

@ -0,0 +1,28 @@
<!--
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.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">
</node>
</launch>