Track googlecartographer/cartographer#333. (#373)

cartographer_ros/cartographer_ros/color.cc

@@ -27,7 +27,7 @@ namespace {
 constexpr float kInitialHue = 0.69f;
 constexpr float kSaturation = 0.85f;
 constexpr float kValue = 0.77f;
-constexpr float kGoldenRatioConjugate = (std::sqrt(5) - 1) / 2.f;
+constexpr float kGoldenRatioConjugate = (std::sqrt(5.f) - 1.f) / 2.f;
 
 ::cartographer_ros::ColorRgb HsvToRgb(const float h, const float s,
                                       const float v) {

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -71,18 +71,18 @@ void MapBuilderBridge::SerializeState(const std::string& stem) {
 }
 
 void MapBuilderBridge::WriteAssets(const string& stem) {
-  const auto trajectory_nodes =
+  const auto all_trajectory_nodes =
       map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
   // TODO(yutakaoka): Add multi-trajectory support.
   CHECK_EQ(trajectory_options_.count(0), 1);
-  CHECK_EQ(trajectory_nodes.size(), 1);
+  CHECK_EQ(all_trajectory_nodes.size(), 1);
-  if (trajectory_nodes[0].empty()) {
+  if (all_trajectory_nodes[0].empty()) {
     LOG(WARNING) << "No data was collected and no assets will be written.";
   } else {
     LOG(INFO) << "Writing assets with stem '" << stem << "'...";
     if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
       Write2DAssets(
-          trajectory_nodes[0], node_options_.map_frame,
+          all_trajectory_nodes[0], node_options_.map_frame,
           trajectory_options_[0]
               .trajectory_builder_options.trajectory_builder_2d_options()
               .submaps_options(),
@@ -91,7 +91,7 @@ void MapBuilderBridge::WriteAssets(const string& stem) {
 
     if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
       Write3DAssets(
-          trajectory_nodes[0],
+          all_trajectory_nodes[0],
           trajectory_options_[0]
               .trajectory_builder_options.trajectory_builder_3d_options()
               .submaps_options()
@@ -140,7 +140,7 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
     for (size_t submap_index = 0; submap_index != submap_transforms.size();
          ++submap_index) {
       cartographer_ros_msgs::SubmapEntry submap_entry;
-      submap_entry.submap_version = submaps->Get(submap_index)->num_range_data;
+      submap_entry.submap_version = submaps->Get(submap_index)->num_range_data();
       submap_entry.pose = ToGeometryMsgPose(submap_transforms[submap_index]);
       trajectory_submap_list.submap.push_back(submap_entry);
     }
@@ -153,19 +153,20 @@ std::unique_ptr<nav_msgs::OccupancyGrid>
 MapBuilderBridge::BuildOccupancyGrid() {
   CHECK(node_options_.map_builder_options.use_trajectory_builder_2d())
       << "Publishing OccupancyGrids for 3D data is not yet supported";
-  std::vector<::cartographer::mapping::TrajectoryNode> trajectory_nodes;
+  std::vector<::cartographer::mapping::TrajectoryNode> flat_trajectory_nodes;
-  for (const auto& single_trajectory :
+  for (const auto& single_trajectory_nodes :
        map_builder_.sparse_pose_graph()->GetTrajectoryNodes()) {
-    trajectory_nodes.insert(trajectory_nodes.end(), single_trajectory.begin(),
+    flat_trajectory_nodes.insert(flat_trajectory_nodes.end(),
-                            single_trajectory.end());
+                                 single_trajectory_nodes.begin(),
+                                 single_trajectory_nodes.end());
   }
   std::unique_ptr<nav_msgs::OccupancyGrid> occupancy_grid;
-  if (!trajectory_nodes.empty()) {
+  if (!flat_trajectory_nodes.empty()) {
     occupancy_grid =
         cartographer::common::make_unique<nav_msgs::OccupancyGrid>();
     CHECK_EQ(trajectory_options_.count(0), 1);
     BuildOccupancyGrid2D(
-        trajectory_nodes, node_options_.map_frame,
+        flat_trajectory_nodes, node_options_.map_frame,
         trajectory_options_[0]
             .trajectory_builder_options.trajectory_builder_2d_options()
             .submaps_options(),
@@ -204,13 +205,13 @@ MapBuilderBridge::GetTrajectoryStates() {
 
 visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodesList() {
   visualization_msgs::MarkerArray trajectory_nodes_list;
-  const auto trajectory_nodes =
+  const auto all_trajectory_nodes =
       map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
   int marker_id = 0;
   for (int trajectory_id = 0;
-       trajectory_id < static_cast<int>(trajectory_nodes.size());
+       trajectory_id < static_cast<int>(all_trajectory_nodes.size());
        ++trajectory_id) {
-    const auto& single_trajectory = trajectory_nodes[trajectory_id];
+    const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id];
     visualization_msgs::Marker marker;
     marker.id = marker_id++;
     marker.type = visualization_msgs::Marker::LINE_STRIP;
@@ -223,7 +224,10 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodesList() {
     marker.color.a = 1.0;
     marker.scale.x = kTrajectoryLineStripMarkerScale;
     marker.pose.orientation.w = 1.0;
-    for (const auto& node : single_trajectory) {
+    for (const auto& node : single_trajectory_nodes) {
+      if (node.trimmed()) {
+        continue;
+      }
       const ::geometry_msgs::Point node_point = ToGeometryMsgPoint(
           (node.pose * node.constant_data->tracking_to_pose).translation());
       marker.points.push_back(node_point);

cartographer_ros/cartographer_ros/occupancy_grid.cc

@@ -29,6 +29,9 @@ Eigen::AlignedBox2f ComputeMapBoundingBox(
         trajectory_nodes) {
   Eigen::AlignedBox2f bounding_box(Eigen::Vector2f::Zero());
   for (const auto& node : trajectory_nodes) {
+    if (node.trimmed()) {
+      continue;
+    }
     const auto& data = *node.constant_data;
     ::cartographer::sensor::RangeData range_data;
     if (!data.range_data_3d.returns.empty()) {
@@ -68,6 +71,9 @@ void BuildOccupancyGrid2D(
   carto::mapping_2d::RangeDataInserter range_data_inserter(
       submaps_options.range_data_inserter_options());
   for (const auto& node : trajectory_nodes) {
+    if (node.trimmed()) {
+      continue;
+    }
     CHECK(node.constant_data->range_data_3d.returns.empty());  // No 3D yet.
     range_data_inserter.Insert(
         carto::sensor::TransformRangeData(node.constant_data->range_data_2d,