Write XRays for 3D runs when finalizing a trajectory. (#116)

* Write XRays for 3D runs when finalizing a trajectory.
master
Holger Rapp 2016-10-14 17:02:09 +02:00 committed by GitHub
parent cfdab98cf3
commit 53df586f69
4 changed files with 121 additions and 10 deletions

View File

@ -87,6 +87,14 @@ google_library(time_conversion
time_conversion.h time_conversion.h
) )
google_library(xray
USES_CARTOGRAPHER
SRCS
xray.cc
HDRS
xray.h
)
google_catkin_test(time_conversion_test google_catkin_test(time_conversion_test
USES_CARTOGRAPHER USES_CARTOGRAPHER
USES_ROS USES_ROS
@ -113,6 +121,7 @@ google_binary(cartographer_node
sensor_bridge sensor_bridge
tf_bridge tf_bridge
time_conversion time_conversion
xray
) )
install(TARGETS cartographer_node install(TARGETS cartographer_node

View File

@ -55,6 +55,7 @@
#include "cartographer_ros/sensor_bridge.h" #include "cartographer_ros/sensor_bridge.h"
#include "cartographer_ros/tf_bridge.h" #include "cartographer_ros/tf_bridge.h"
#include "cartographer_ros/time_conversion.h" #include "cartographer_ros/time_conversion.h"
#include "cartographer_ros/xray.h"
#include "cartographer_ros_msgs/FinishTrajectory.h" #include "cartographer_ros_msgs/FinishTrajectory.h"
#include "cartographer_ros_msgs/SubmapEntry.h" #include "cartographer_ros_msgs/SubmapEntry.h"
#include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapList.h"
@ -363,17 +364,26 @@ bool Node::HandleFinishTrajectory(
// TODO(whess): Add multi-trajectory support. // TODO(whess): Add multi-trajectory support.
sensor_collator_.FinishTrajectory(kTrajectoryBuilderId); sensor_collator_.FinishTrajectory(kTrajectoryBuilderId);
map_builder_.sparse_pose_graph()->RunFinalOptimization(); 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()) { if (options_.map_builder_options.use_trajectory_builder_2d()) {
const auto trajectory_nodes = ::nav_msgs::OccupancyGrid occupancy_grid;
map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); BuildOccupancyGrid(trajectory_nodes, options_, &occupancy_grid);
if (!trajectory_nodes.empty()) { WriteOccupancyGridToPgmAndYaml(occupancy_grid, request.stem);
::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()) {
} else { WriteXRayImages(trajectory_nodes,
LOG(WARNING) << "Map is empty and will not be saved."; options_.map_builder_options.trajectory_builder_3d_options()
} .submaps_options()
.high_resolution(),
request.stem);
} }
return true; return true;
} }

View File

@ -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<float>());
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

View File

@ -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 <string>
#include <vector>
#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_