From 53df586f691417eca9edcafbf79d8c2689661ead Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Fri, 14 Oct 2016 17:02:09 +0200 Subject: [PATCH] Write XRays for 3D runs when finalizing a trajectory. (#116) * Write XRays for 3D runs when finalizing a trajectory. --- .../cartographer_ros/CMakeLists.txt | 9 +++ .../cartographer_ros/node_main.cc | 30 ++++++---- cartographer_ros/cartographer_ros/xray.cc | 57 +++++++++++++++++++ cartographer_ros/cartographer_ros/xray.h | 35 ++++++++++++ 4 files changed, 121 insertions(+), 10 deletions(-) create mode 100644 cartographer_ros/cartographer_ros/xray.cc create mode 100644 cartographer_ros/cartographer_ros/xray.h diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index ecfc279..7ba290b 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -87,6 +87,14 @@ google_library(time_conversion time_conversion.h ) +google_library(xray + USES_CARTOGRAPHER + SRCS + xray.cc + HDRS + xray.h +) + google_catkin_test(time_conversion_test USES_CARTOGRAPHER USES_ROS @@ -113,6 +121,7 @@ google_binary(cartographer_node sensor_bridge tf_bridge time_conversion + xray ) install(TARGETS cartographer_node diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 6cbf3b8..4dd4fcf 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -55,6 +55,7 @@ #include "cartographer_ros/sensor_bridge.h" #include "cartographer_ros/tf_bridge.h" #include "cartographer_ros/time_conversion.h" +#include "cartographer_ros/xray.h" #include "cartographer_ros_msgs/FinishTrajectory.h" #include "cartographer_ros_msgs/SubmapEntry.h" #include "cartographer_ros_msgs/SubmapList.h" @@ -363,17 +364,26 @@ bool Node::HandleFinishTrajectory( // TODO(whess): Add multi-trajectory support. sensor_collator_.FinishTrajectory(kTrajectoryBuilderId); map_builder_.sparse_pose_graph()->RunFinalOptimization(); - // TODO(whess): Write X-rays in 3D. + + const auto trajectory_nodes = + map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); + if (trajectory_nodes.empty()) { + LOG(WARNING) << "Map is empty and will not be saved."; + return true; + } + if (options_.map_builder_options.use_trajectory_builder_2d()) { - const auto trajectory_nodes = - map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); - if (!trajectory_nodes.empty()) { - ::nav_msgs::OccupancyGrid occupancy_grid; - BuildOccupancyGrid(trajectory_nodes, options_, &occupancy_grid); - WriteOccupancyGridToPgmAndYaml(occupancy_grid, request.stem); - } else { - LOG(WARNING) << "Map is empty and will not be saved."; - } + ::nav_msgs::OccupancyGrid occupancy_grid; + BuildOccupancyGrid(trajectory_nodes, options_, &occupancy_grid); + WriteOccupancyGridToPgmAndYaml(occupancy_grid, request.stem); + } + + if (options_.map_builder_options.use_trajectory_builder_3d()) { + WriteXRayImages(trajectory_nodes, + options_.map_builder_options.trajectory_builder_3d_options() + .submaps_options() + .high_resolution(), + request.stem); } return true; } diff --git a/cartographer_ros/cartographer_ros/xray.cc b/cartographer_ros/cartographer_ros/xray.cc new file mode 100644 index 0000000..1d3dc0b --- /dev/null +++ b/cartographer_ros/cartographer_ros/xray.cc @@ -0,0 +1,57 @@ +/* + * 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. + */ + +#include "cartographer_ros/xray.h" + +#include "cartographer/io/null_points_processor.h" +#include "cartographer/io/points_processor.h" +#include "cartographer/io/xray_points_processor.h" + +namespace cartographer_ros { + +namespace carto = ::cartographer; + +void WriteXRayImages(const std::vector<::cartographer::mapping::TrajectoryNode>& + trajectory_nodes, + const double voxel_size, const std::string& stem) { + carto::io::NullPointsProcessor null_points_processor; + carto::io::XRayPointsProcessor xy_xray_points_processor( + voxel_size, carto::transform::Rigid3f::Rotation( + Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())), + stem + "_xray_xy.png", &null_points_processor); + carto::io::XRayPointsProcessor yz_xray_points_processor( + voxel_size, carto::transform::Rigid3f::Rotation( + Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())), + stem + "_xray_yz.png", &xy_xray_points_processor); + carto::io::XRayPointsProcessor xz_xray_points_processor( + voxel_size, carto::transform::Rigid3f::Rotation( + Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())), + stem + "_xray_xz.png", &yz_xray_points_processor); + + for (const auto& node : trajectory_nodes) { + const carto::sensor::LaserFan laser_fan = carto::sensor::TransformLaserFan( + carto::sensor::Decompress(node.constant_data->laser_fan_3d), + node.pose.cast()); + + carto::io::PointsBatch points_batch; + points_batch.origin = laser_fan.origin; + points_batch.points = laser_fan.returns; + xz_xray_points_processor.Process(points_batch); + } + xz_xray_points_processor.Flush(); +} + +} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/xray.h b/cartographer_ros/cartographer_ros/xray.h new file mode 100644 index 0000000..8c8a1f2 --- /dev/null +++ b/cartographer_ros/cartographer_ros/xray.h @@ -0,0 +1,35 @@ +/* + * 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. + */ + +#ifndef CARTOGRAPHER_ROS_XRAY_H_ +#define CARTOGRAPHER_ROS_XRAY_H_ + +#include +#include + +#include "cartographer/mapping/trajectory_node.h" + +namespace cartographer_ros { + +// Writes X-ray images from the 'trajectory_nodes'. The filenames will all start +// with 'stem'. +void WriteXRayImages(const std::vector<::cartographer::mapping::TrajectoryNode>& + trajectory_nodes, + double voxel_size, const std::string& stem); + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_XRAY_H_