Follow googlecartographer/cartographer#288. (#338)

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

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<nav_msgs::OccupancyGrid>
 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<nav_msgs::OccupancyGrid> occupancy_grid;
   if (!trajectory_nodes.empty()) {
     occupancy_grid =

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<float>());
+    } else {
+      range_data = ::cartographer::sensor::TransformRangeData(
+          data.range_data_2d, node.pose.cast<float>());
+    }
+    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<double>() / resolution;
+  return ::cartographer::mapping_2d::MapLimits(
+      resolution, bounding_box.max().cast<double>(),
+      ::cartographer::mapping_2d::CellLimits(
+          ::cartographer::common::RoundToInt(pixel_sizes.y()),
+          ::cartographer::common::RoundToInt(pixel_sizes.x())));
+}
+
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/occupancy_grid.h

@@ -19,7 +19,10 @@
 
 #include <vector>
 
+#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_

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