From 8c56b44bdd6b18d2f76a22b96c64bca12dcb3592 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 16 May 2017 11:40:06 +0200 Subject: [PATCH] Follow googlecartographer/cartographer#288. (#338) --- .../cartographer_ros/assets_writer.cc | 1 + .../cartographer_ros/map_builder_bridge.cc | 16 ++++-- .../cartographer_ros/occupancy_grid.cc | 56 ++++++++++++++++++- .../cartographer_ros/occupancy_grid.h | 10 ++++ .../cartographer_ros/occupancy_grid_test.cc | 52 +++++++++++++++++ 5 files changed, 128 insertions(+), 7 deletions(-) create mode 100644 cartographer_ros/cartographer_ros/occupancy_grid_test.cc diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index 60b6f84..d91e02d 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -39,6 +39,7 @@ void WriteTrajectory(const std::vector<::cartographer::mapping::TrajectoryNode>& trajectory_nodes, const std::string& stem) { carto::mapping::proto::Trajectory trajectory; + // TODO(whess): Add multi-trajectory support. for (const auto& node : trajectory_nodes) { const auto& data = *node.constant_data; auto* node_proto = trajectory.add_node(); diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 3cd8e69..3e8cf0c 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -54,8 +54,12 @@ void MapBuilderBridge::FinishTrajectory(const int trajectory_id) { } void MapBuilderBridge::WriteAssets(const string& stem) { - const auto trajectory_nodes = - map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); + std::vector<::cartographer::mapping::TrajectoryNode> trajectory_nodes; + for (const auto& single_trajectory : + map_builder_.sparse_pose_graph()->GetTrajectoryNodes()) { + trajectory_nodes.insert(trajectory_nodes.end(), single_trajectory.begin(), + single_trajectory.end()); + } if (trajectory_nodes.empty()) { LOG(WARNING) << "No data was collected and no assets will be written."; } else { @@ -131,8 +135,12 @@ std::unique_ptr MapBuilderBridge::BuildOccupancyGrid() { CHECK(options_.map_builder_options.use_trajectory_builder_2d()) << "Publishing OccupancyGrids for 3D data is not yet supported"; - const auto trajectory_nodes = - map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); + std::vector<::cartographer::mapping::TrajectoryNode> trajectory_nodes; + for (const auto& single_trajectory : + map_builder_.sparse_pose_graph()->GetTrajectoryNodes()) { + trajectory_nodes.insert(trajectory_nodes.end(), single_trajectory.begin(), + single_trajectory.end()); + } std::unique_ptr occupancy_grid; if (!trajectory_nodes.empty()) { occupancy_grid = diff --git a/cartographer_ros/cartographer_ros/occupancy_grid.cc b/cartographer_ros/cartographer_ros/occupancy_grid.cc index 5d358a8..b4c58af 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid.cc +++ b/cartographer_ros/cartographer_ros/occupancy_grid.cc @@ -16,12 +16,42 @@ #include "cartographer_ros/occupancy_grid.h" -#include "cartographer/mapping_2d/map_limits.h" +#include "cartographer/common/port.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/range_data_inserter.h" #include "cartographer_ros/time_conversion.h" #include "glog/logging.h" +namespace { + +Eigen::AlignedBox2f ComputeMapBoundingBox( + const std::vector<::cartographer::mapping::TrajectoryNode>& + trajectory_nodes) { + Eigen::AlignedBox2f bounding_box(Eigen::Vector2f::Zero()); + for (const auto& node : trajectory_nodes) { + const auto& data = *node.constant_data; + ::cartographer::sensor::RangeData range_data; + if (!data.range_data_3d.returns.empty()) { + range_data = ::cartographer::sensor::TransformRangeData( + ::cartographer::sensor::Decompress(data.range_data_3d), + node.pose.cast()); + } else { + range_data = ::cartographer::sensor::TransformRangeData( + data.range_data_2d, node.pose.cast()); + } + bounding_box.extend(range_data.origin.head<2>()); + for (const Eigen::Vector3f& hit : range_data.returns) { + bounding_box.extend(hit.head<2>()); + } + for (const Eigen::Vector3f& miss : range_data.misses) { + bounding_box.extend(miss.head<2>()); + } + } + return bounding_box; +} + +} // namespace + namespace cartographer_ros { void BuildOccupancyGrid2D( @@ -30,10 +60,10 @@ void BuildOccupancyGrid2D( const string& map_frame, const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options, ::nav_msgs::OccupancyGrid* const occupancy_grid) { + CHECK(!trajectory_nodes.empty()); namespace carto = ::cartographer; const carto::mapping_2d::MapLimits map_limits = - carto::mapping_2d::MapLimits::ComputeMapLimits( - submaps_options.resolution(), trajectory_nodes); + ComputeMapLimits(submaps_options.resolution(), trajectory_nodes); carto::mapping_2d::ProbabilityGrid probability_grid(map_limits); carto::mapping_2d::RangeDataInserter range_data_inserter( submaps_options.range_data_inserter_options()); @@ -45,6 +75,7 @@ void BuildOccupancyGrid2D( &probability_grid); } + // TODO(whess): Compute the latest time of in 'trajectory_nodes'. occupancy_grid->header.stamp = ToRos(trajectory_nodes.back().time()); occupancy_grid->header.frame_id = map_frame; occupancy_grid->info.map_load_time = occupancy_grid->header.stamp; @@ -89,4 +120,23 @@ void BuildOccupancyGrid2D( } } +::cartographer::mapping_2d::MapLimits ComputeMapLimits( + const double resolution, + const std::vector<::cartographer::mapping::TrajectoryNode>& + trajectory_nodes) { + Eigen::AlignedBox2f bounding_box = ComputeMapBoundingBox(trajectory_nodes); + // Add some padding to ensure all rays are still contained in the map after + // discretization. + const float kPadding = 3.f * resolution; + bounding_box.min() -= kPadding * Eigen::Vector2f::Ones(); + bounding_box.max() += kPadding * Eigen::Vector2f::Ones(); + const Eigen::Vector2d pixel_sizes = + bounding_box.sizes().cast() / resolution; + return ::cartographer::mapping_2d::MapLimits( + resolution, bounding_box.max().cast(), + ::cartographer::mapping_2d::CellLimits( + ::cartographer::common::RoundToInt(pixel_sizes.y()), + ::cartographer::common::RoundToInt(pixel_sizes.x()))); +} + } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/occupancy_grid.h b/cartographer_ros/cartographer_ros/occupancy_grid.h index c6d6f6e..0b27be2 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid.h +++ b/cartographer_ros/cartographer_ros/occupancy_grid.h @@ -19,7 +19,10 @@ #include +#include "Eigen/Core" +#include "Eigen/Geometry" #include "cartographer/mapping/trajectory_node.h" +#include "cartographer/mapping_2d/map_limits.h" #include "cartographer/mapping_2d/proto/submaps_options.pb.h" #include "nav_msgs/OccupancyGrid.h" @@ -32,6 +35,13 @@ void BuildOccupancyGrid2D( const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options, ::nav_msgs::OccupancyGrid* const occupancy_grid); +// Computes MapLimits that contain the origin, and all rays (both returns and +// misses) in the 'trajectory_nodes'. +::cartographer::mapping_2d::MapLimits ComputeMapLimits( + double resolution, + const std::vector<::cartographer::mapping::TrajectoryNode>& + trajectory_nodes); + } // namespace cartographer_ros #endif // CARTOGRAPHER_ROS_OCCUPANCY_GRID_H_ diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_test.cc b/cartographer_ros/cartographer_ros/occupancy_grid_test.cc new file mode 100644 index 0000000..51674b2 --- /dev/null +++ b/cartographer_ros/cartographer_ros/occupancy_grid_test.cc @@ -0,0 +1,52 @@ +/* + * 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/occupancy_grid.h" + +#include "cartographer/common/time.h" +#include "cartographer/sensor/range_data.h" +#include "gtest/gtest.h" +#include "ros/ros.h" + +namespace cartographer_ros { +namespace { + +TEST(OccupancyGridTest, ComputeMapLimits) { + const ::cartographer::mapping::TrajectoryNode::ConstantData constant_data{ + ::cartographer::common::FromUniversal(52), + ::cartographer::sensor::RangeData{ + Eigen::Vector3f::Zero(), + {Eigen::Vector3f(-30.f, 1.f, 0.f), Eigen::Vector3f(50.f, -10.f, 0.f)}, + {}}, + ::cartographer::sensor::Compress( + ::cartographer::sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), + nullptr, ::cartographer::transform::Rigid3d::Identity()}; + const ::cartographer::mapping::TrajectoryNode trajectory_node{ + &constant_data, ::cartographer::transform::Rigid3d::Identity()}; + constexpr double kResolution = 0.05; + const ::cartographer::mapping_2d::MapLimits limits = + ComputeMapLimits(kResolution, {trajectory_node}); + constexpr float kPaddingAwareTolerance = 5 * kResolution; + EXPECT_NEAR(50.f, limits.max().x(), kPaddingAwareTolerance); + EXPECT_NEAR(1.f, limits.max().y(), kPaddingAwareTolerance); + EXPECT_LT(200, limits.cell_limits().num_x_cells); + EXPECT_LT(1600, limits.cell_limits().num_y_cells); + EXPECT_GT(400, limits.cell_limits().num_x_cells); + EXPECT_GT(2000, limits.cell_limits().num_y_cells); +} + +} // namespace +} // namespace cartographer_ros