12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Assets pipeline demo and track Cartographer API (#253) 

Adds a more complete assets_writer_backpack_2d.lua including many
comments.
master
Holger Rapp 2017-01-24 15:57:37 +01:00 committed by GitHub
parent 0783398634
commit 2a2286deb0
4 changed files with 134 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
cartographer_ros/cartographer_ros/assets_writer_main.cc
View File

@ -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<std::unique_ptr<carto::io::PointsProcessor>> pipeline =
builder.CreatePipeline(
lua_parameter_dictionary.GetDictionary("pipeline").get());

90
cartographer_ros/configuration_files/assets_writer.lua
View File

@ -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

130
cartographer_ros/configuration_files/assets_writer_backpack_2d.lua Normal file
View File

@ -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

3
cartographer_ros/launch/assets_writer_backpack_2d.launch
View File

@ -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">