Move SubmapToProto from Submaps -> Submap. (#332)

PAIR=wohe

cartographer/mapping/map_builder.cc

@@ -123,10 +123,10 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
            " submaps in this trajectory.";
   }
 
-  Submaps* const submaps = trajectory_builders_.at(trajectory_id)->submaps();
+  const Submap* const submap =
-  response->set_submap_version(submaps->Get(submap_index)->num_range_data);
+      trajectory_builders_.at(trajectory_id)->submaps()->Get(submap_index);
-  submaps->SubmapToProto(submap_index, submap_transforms[submap_index],
+  response->set_submap_version(submap->num_range_data);
-                         response);
+  submap->ToResponseProto(submap_transforms[submap_index], response);
   return "";
 }
 

cartographer/mapping/sparse_pose_graph.cc

@@ -73,8 +73,10 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
          old_node_index != single_trajectory_nodes.size(); ++old_node_index) {
       const auto& node = single_trajectory_nodes[old_node_index];
       if (!node.trimmed()) {
-        node_id_remapping[NodeId{trajectory_id, old_node_index}] =
+        node_id_remapping[NodeId{static_cast<int>(trajectory_id),
-            NodeId{trajectory_id, trajectory_proto->node_size()};
+                                 static_cast<int>(old_node_index)}] =
+            NodeId{static_cast<int>(trajectory_id),
+                   static_cast<int>(trajectory_proto->node_size())};
         auto* node_proto = trajectory_proto->add_node();
         node_proto->set_timestamp(
             common::ToUniversal(node.constant_data->time));

cartographer/mapping/submaps.h

@@ -56,8 +56,10 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) {
 // track of how many range data were inserted into it, and sets the
 // 'finished_probability_grid' to be used for loop closing once the map no
 // longer changes.
+// TODO(hrapp): This should be a class now.
 struct Submap {
   Submap(const transform::Rigid3d& local_pose) : local_pose(local_pose) {}
+  virtual ~Submap() {}
 
   // Local SLAM pose of this submap.
   const transform::Rigid3d local_pose;
@@ -69,6 +71,11 @@ struct Submap {
   // change anymore. Otherwise, this is nullptr and the next call to
   // InsertRangeData() will change the submap.
   const mapping_2d::ProbabilityGrid* finished_probability_grid = nullptr;
+
+  // Fills data into the 'response'.
+  virtual void ToResponseProto(
+      const transform::Rigid3d& global_submap_pose,
+      proto::SubmapQuery::Response* response) const = 0;
 };
 
 // Submaps is a sequence of maps to which scans are matched and into which scans
@@ -105,11 +112,6 @@ class Submaps {
 
   // Returns the number of Submaps.
   virtual int size() const = 0;
-
-  // Fills data about the Submap with 'index' into the 'response'.
-  virtual void SubmapToProto(int index,
-                             const transform::Rigid3d& global_submap_pose,
-                             proto::SubmapQuery::Response* response) = 0;
 };
 
 }  // namespace mapping

cartographer/mapping_2d/submaps.cc

@@ -105,8 +105,9 @@ Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin)
           Eigen::Vector3d(origin.x(), origin.y(), 0.))),
       probability_grid(limits) {}
 
-void Submap::AddProbabilityGridToResponse(
+void Submap::ToResponseProto(
-    mapping::proto::SubmapQuery::Response* response) {
+    const transform::Rigid3d&,
+    mapping::proto::SubmapQuery::Response* const response) const {
   Eigen::Array2i offset;
   CellLimits limits;
   probability_grid.ComputeCroppedLimits(&offset, &limits);
@@ -177,12 +178,6 @@ const Submap* Submaps::Get(int index) const {
 
 int Submaps::size() const { return submaps_.size(); }
 
-void Submaps::SubmapToProto(
-    const int index, const transform::Rigid3d&,
-    mapping::proto::SubmapQuery::Response* const response) {
-  submaps_.at(index)->AddProbabilityGridToResponse(response);
-}
-
 void Submaps::FinishSubmap(int index) {
   // Crop the finished Submap before inserting a new Submap to reduce peak
   // memory usage a bit.

cartographer/mapping_2d/submaps.h

@@ -46,8 +46,9 @@ struct Submap : public mapping::Submap {
 
   ProbabilityGrid probability_grid;
 
-  void AddProbabilityGridToResponse(
+  void ToResponseProto(
-      mapping::proto::SubmapQuery::Response* response);
+      const transform::Rigid3d& global_submap_pose,
+      mapping::proto::SubmapQuery::Response* response) const override;
 };
 
 // A container of Submaps.
@@ -60,8 +61,6 @@ class Submaps : public mapping::Submaps {
 
   const Submap* Get(int index) const override;
   int size() const override;
-  void SubmapToProto(int index, const transform::Rigid3d& global_submap_pose,
-                     mapping::proto::SubmapQuery::Response* response) override;
 
   // Inserts 'range_data' into the Submap collection.
   void InsertRangeData(const sensor::RangeData& range_data);

cartographer/mapping_3d/submaps.cc

@@ -36,6 +36,14 @@ struct RaySegment {
   bool hit;  // Whether there is a hit at 'to'.
 };
 
+struct PixelData {
+  int min_z = INT_MAX;
+  int max_z = INT_MIN;
+  int count = 0;
+  float probability_sum = 0.f;
+  float max_probability = 0.5f;
+};
+
 // We compute a slice around the xy-plane. 'transform' is applied to the rays in
 // global map frame to allow choosing an arbitrary slice.
 void GenerateSegmentForSlice(const sensor::RangeData& range_data,
@@ -183,6 +191,102 @@ sensor::RangeData FilterRangeDataByMaxRange(const sensor::RangeData& range_data,
   return result;
 }
 
+std::vector<PixelData> AccumulatePixelData(
+    const int width, const int height, const Eigen::Array2i& min_index,
+    const Eigen::Array2i& max_index,
+    const std::vector<Eigen::Array4i>& voxel_indices_and_probabilities) {
+  std::vector<PixelData> accumulated_pixel_data(width * height);
+  for (const Eigen::Array4i& voxel_index_and_probability :
+       voxel_indices_and_probabilities) {
+    const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>();
+    if ((pixel_index < min_index).any() || (pixel_index > max_index).any()) {
+      // Out of bounds. This could happen because of floating point inaccuracy.
+      continue;
+    }
+    const int x = max_index.x() - pixel_index[0];
+    const int y = max_index.y() - pixel_index[1];
+    PixelData& pixel = accumulated_pixel_data[x * width + y];
+    ++pixel.count;
+    pixel.min_z = std::min(pixel.min_z, voxel_index_and_probability[2]);
+    pixel.max_z = std::max(pixel.max_z, voxel_index_and_probability[2]);
+    const float probability =
+        mapping::ValueToProbability(voxel_index_and_probability[3]);
+    pixel.probability_sum += probability;
+    pixel.max_probability = std::max(pixel.max_probability, probability);
+  }
+  return accumulated_pixel_data;
+}
+
+// The first three entries of each returned value are a cell_index and the
+// last is the corresponding probability value. We batch them together like
+// this to only have one vector and have better cache locality.
+std::vector<Eigen::Array4i> ExtractVoxelData(
+    const HybridGrid& hybrid_grid, const transform::Rigid3f& transform,
+    Eigen::Array2i* min_index, Eigen::Array2i* max_index) {
+  std::vector<Eigen::Array4i> voxel_indices_and_probabilities;
+  const float resolution_inverse = 1. / hybrid_grid.resolution();
+
+  constexpr double kXrayObstructedCellProbabilityLimit = 0.501;
+  for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) {
+    const uint16 probability_value = it.GetValue();
+    const float probability = mapping::ValueToProbability(probability_value);
+    if (probability < kXrayObstructedCellProbabilityLimit) {
+      // We ignore non-obstructed cells.
+      continue;
+    }
+
+    const Eigen::Vector3f cell_center_submap =
+        hybrid_grid.GetCenterOfCell(it.GetCellIndex());
+    const Eigen::Vector3f cell_center_global = transform * cell_center_submap;
+    const Eigen::Array4i voxel_index_and_probability(
+        common::RoundToInt(cell_center_global.x() * resolution_inverse),
+        common::RoundToInt(cell_center_global.y() * resolution_inverse),
+        common::RoundToInt(cell_center_global.z() * resolution_inverse),
+        probability_value);
+
+    voxel_indices_and_probabilities.push_back(voxel_index_and_probability);
+    const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>();
+    *min_index = min_index->cwiseMin(pixel_index);
+    *max_index = max_index->cwiseMax(pixel_index);
+  }
+  return voxel_indices_and_probabilities;
+}
+
+// Builds texture data containing interleaved value and alpha for the
+// visualization from 'accumulated_pixel_data'.
+string ComputePixelValues(
+    const std::vector<PixelData>& accumulated_pixel_data) {
+  string cell_data;
+  cell_data.reserve(2 * accumulated_pixel_data.size());
+  constexpr float kMinZDifference = 3.f;
+  constexpr float kFreeSpaceWeight = 0.15f;
+  for (const PixelData& pixel : accumulated_pixel_data) {
+    // TODO(whess): Take into account submap rotation.
+    // TODO(whess): Document the approach and make it more independent from the
+    // chosen resolution.
+    const float z_difference = pixel.count > 0 ? pixel.max_z - pixel.min_z : 0;
+    if (z_difference < kMinZDifference) {
+      cell_data.push_back(0);  // value
+      cell_data.push_back(0);  // alpha
+      continue;
+    }
+    const float free_space = std::max(z_difference - pixel.count, 0.f);
+    const float free_space_weight = kFreeSpaceWeight * free_space;
+    const float total_weight = pixel.count + free_space_weight;
+    const float free_space_probability = 1.f - pixel.max_probability;
+    const float average_probability = mapping::ClampProbability(
+        (pixel.probability_sum + free_space_probability * free_space_weight) /
+        total_weight);
+    const int delta =
+        128 - mapping::ProbabilityToLogOddsInteger(average_probability);
+    const uint8 alpha = delta > 0 ? 0 : -delta;
+    const uint8 value = delta > 0 ? delta : 0;
+    cell_data.push_back(value);                         // value
+    cell_data.push_back((value || alpha) ? alpha : 1);  // alpha
+  }
+  return cell_data;
+}
+
 }  // namespace
 
 void InsertIntoProbabilityGrid(
@@ -222,6 +326,39 @@ Submap::Submap(const float high_resolution, const float low_resolution,
       high_resolution_hybrid_grid(high_resolution),
       low_resolution_hybrid_grid(low_resolution) {}
 
+void Submap::ToResponseProto(
+    const transform::Rigid3d& global_submap_pose,
+    mapping::proto::SubmapQuery::Response* const response) const {
+  // Generate an X-ray view through the 'hybrid_grid', aligned to the xy-plane
+  // in the global map frame.
+  const float resolution = high_resolution_hybrid_grid.resolution();
+  response->set_resolution(resolution);
+
+  // Compute a bounding box for the texture.
+  Eigen::Array2i min_index(INT_MAX, INT_MAX);
+  Eigen::Array2i max_index(INT_MIN, INT_MIN);
+  const std::vector<Eigen::Array4i> voxel_indices_and_probabilities =
+      ExtractVoxelData(high_resolution_hybrid_grid,
+                       global_submap_pose.cast<float>(), &min_index,
+                       &max_index);
+
+  const int width = max_index.y() - min_index.y() + 1;
+  const int height = max_index.x() - min_index.x() + 1;
+  response->set_width(width);
+  response->set_height(height);
+
+  const std::vector<PixelData> accumulated_pixel_data = AccumulatePixelData(
+      width, height, min_index, max_index, voxel_indices_and_probabilities);
+  const string cell_data = ComputePixelValues(accumulated_pixel_data);
+
+  common::FastGzipString(cell_data, response->mutable_cells());
+  *response->mutable_slice_pose() = transform::ToProto(
+      global_submap_pose.inverse() *
+      transform::Rigid3d::Translation(Eigen::Vector3d(
+          max_index.x() * resolution, max_index.y() * resolution,
+          global_submap_pose.translation().z())));
+}
+
 Submaps::Submaps(const proto::SubmapsOptions& options)
     : options_(options),
       range_data_inserter_(options.range_data_inserter_options()) {
@@ -241,39 +378,6 @@ const Submap* Submaps::Get(int index) const {
 
 int Submaps::size() const { return submaps_.size(); }
 
-void Submaps::SubmapToProto(
-    int index, const transform::Rigid3d& global_submap_pose,
-    mapping::proto::SubmapQuery::Response* const response) {
-  // Generate an X-ray view through the 'hybrid_grid', aligned to the xy-plane
-  // in the global map frame.
-  const HybridGrid& hybrid_grid = Get(index)->high_resolution_hybrid_grid;
-  response->set_resolution(hybrid_grid.resolution());
-
-  // Compute a bounding box for the texture.
-  Eigen::Array2i min_index(INT_MAX, INT_MAX);
-  Eigen::Array2i max_index(INT_MIN, INT_MIN);
-  const std::vector<Eigen::Array4i> voxel_indices_and_probabilities =
-      ExtractVoxelData(hybrid_grid, global_submap_pose.cast<float>(),
-                       &min_index, &max_index);
-
-  const int width = max_index.y() - min_index.y() + 1;
-  const int height = max_index.x() - min_index.x() + 1;
-  response->set_width(width);
-  response->set_height(height);
-
-  const std::vector<PixelData> accumulated_pixel_data = AccumulatePixelData(
-      width, height, min_index, max_index, voxel_indices_and_probabilities);
-  const string cell_data = ComputePixelValues(accumulated_pixel_data);
-
-  common::FastGzipString(cell_data, response->mutable_cells());
-  *response->mutable_slice_pose() =
-      transform::ToProto(global_submap_pose.inverse() *
-                         transform::Rigid3d::Translation(Eigen::Vector3d(
-                             max_index.x() * hybrid_grid.resolution(),
-                             max_index.y() * hybrid_grid.resolution(),
-                             global_submap_pose.translation().z())));
-}
-
 void Submaps::InsertRangeData(const sensor::RangeData& range_data,
                               const Eigen::Quaterniond& gravity_alignment) {
   for (const int index : insertion_indices()) {
@@ -306,96 +410,5 @@ void Submaps::AddSubmap(const transform::Rigid3d& local_pose) {
   LOG(INFO) << "Added submap " << size();
 }
 
-std::vector<Submaps::PixelData> Submaps::AccumulatePixelData(
-    const int width, const int height, const Eigen::Array2i& min_index,
-    const Eigen::Array2i& max_index,
-    const std::vector<Eigen::Array4i>& voxel_indices_and_probabilities) const {
-  std::vector<PixelData> accumulated_pixel_data(width * height);
-  for (const Eigen::Array4i& voxel_index_and_probability :
-       voxel_indices_and_probabilities) {
-    const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>();
-    if ((pixel_index < min_index).any() || (pixel_index > max_index).any()) {
-      // Out of bounds. This could happen because of floating point inaccuracy.
-      continue;
-    }
-    const int x = max_index.x() - pixel_index[0];
-    const int y = max_index.y() - pixel_index[1];
-    PixelData& pixel = accumulated_pixel_data[x * width + y];
-    ++pixel.count;
-    pixel.min_z = std::min(pixel.min_z, voxel_index_and_probability[2]);
-    pixel.max_z = std::max(pixel.max_z, voxel_index_and_probability[2]);
-    const float probability =
-        mapping::ValueToProbability(voxel_index_and_probability[3]);
-    pixel.probability_sum += probability;
-    pixel.max_probability = std::max(pixel.max_probability, probability);
-  }
-  return accumulated_pixel_data;
-}
-
-std::vector<Eigen::Array4i> Submaps::ExtractVoxelData(
-    const HybridGrid& hybrid_grid, const transform::Rigid3f& transform,
-    Eigen::Array2i* min_index, Eigen::Array2i* max_index) const {
-  std::vector<Eigen::Array4i> voxel_indices_and_probabilities;
-  const float resolution_inverse = 1. / hybrid_grid.resolution();
-
-  constexpr double kXrayObstructedCellProbabilityLimit = 0.501;
-  for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) {
-    const uint16 probability_value = it.GetValue();
-    const float probability = mapping::ValueToProbability(probability_value);
-    if (probability < kXrayObstructedCellProbabilityLimit) {
-      // We ignore non-obstructed cells.
-      continue;
-    }
-
-    const Eigen::Vector3f cell_center_submap =
-        hybrid_grid.GetCenterOfCell(it.GetCellIndex());
-    const Eigen::Vector3f cell_center_global = transform * cell_center_submap;
-    const Eigen::Array4i voxel_index_and_probability(
-        common::RoundToInt(cell_center_global.x() * resolution_inverse),
-        common::RoundToInt(cell_center_global.y() * resolution_inverse),
-        common::RoundToInt(cell_center_global.z() * resolution_inverse),
-        probability_value);
-
-    voxel_indices_and_probabilities.push_back(voxel_index_and_probability);
-    const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>();
-    *min_index = min_index->cwiseMin(pixel_index);
-    *max_index = max_index->cwiseMax(pixel_index);
-  }
-  return voxel_indices_and_probabilities;
-}
-
-string Submaps::ComputePixelValues(
-    const std::vector<Submaps::PixelData>& accumulated_pixel_data) const {
-  string cell_data;
-  cell_data.reserve(2 * accumulated_pixel_data.size());
-  constexpr float kMinZDifference = 3.f;
-  constexpr float kFreeSpaceWeight = 0.15f;
-  for (const PixelData& pixel : accumulated_pixel_data) {
-    // TODO(whess): Take into account submap rotation.
-    // TODO(whess): Document the approach and make it more independent from the
-    // chosen resolution.
-    const float z_difference = pixel.count > 0 ? pixel.max_z - pixel.min_z : 0;
-    if (z_difference < kMinZDifference) {
-      cell_data.push_back(0);  // value
-      cell_data.push_back(0);  // alpha
-      continue;
-    }
-    const float free_space = std::max(z_difference - pixel.count, 0.f);
-    const float free_space_weight = kFreeSpaceWeight * free_space;
-    const float total_weight = pixel.count + free_space_weight;
-    const float free_space_probability = 1.f - pixel.max_probability;
-    const float average_probability = mapping::ClampProbability(
-        (pixel.probability_sum + free_space_probability * free_space_weight) /
-        total_weight);
-    const int delta =
-        128 - mapping::ProbabilityToLogOddsInteger(average_probability);
-    const uint8 alpha = delta > 0 ? 0 : -delta;
-    const uint8 value = delta > 0 ? delta : 0;
-    cell_data.push_back(value);                         // value
-    cell_data.push_back((value || alpha) ? alpha : 1);  // alpha
-  }
-  return cell_data;
-}
-
 }  // namespace mapping_3d
 }  // namespace cartographer

cartographer/mapping_3d/submaps.h

@@ -54,6 +54,10 @@ struct Submap : public mapping::Submap {
   HybridGrid high_resolution_hybrid_grid;
   HybridGrid low_resolution_hybrid_grid;
   bool finished = false;
+
+  void ToResponseProto(
+      const transform::Rigid3d& global_submap_pose,
+      mapping::proto::SubmapQuery::Response* response) const override;
 };
 
 // A container of Submaps.
@@ -66,8 +70,6 @@ class Submaps : public mapping::Submaps {
 
   const Submap* Get(int index) const override;
   int size() const override;
-  void SubmapToProto(int index, const transform::Rigid3d& global_submap_pose,
-                     mapping::proto::SubmapQuery::Response* response) override;
 
   // Inserts 'range_data' into the Submap collection. 'gravity_alignment' is
   // used for the orientation of new submaps so that the z axis approximately
@@ -76,31 +78,8 @@ class Submaps : public mapping::Submaps {
                        const Eigen::Quaterniond& gravity_alignment);
 
  private:
-  struct PixelData {
-    int min_z = INT_MAX;
-    int max_z = INT_MIN;
-    int count = 0;
-    float probability_sum = 0.f;
-    float max_probability = 0.5f;
-  };
-
   void AddSubmap(const transform::Rigid3d& local_pose);
 
-  std::vector<PixelData> AccumulatePixelData(
-      const int width, const int height, const Eigen::Array2i& min_index,
-      const Eigen::Array2i& max_index,
-      const std::vector<Eigen::Array4i>& voxel_indices_and_probabilities) const;
-  // The first three entries of each returned value are a cell_index and the
-  // last is the corresponding probability value. We batch them together like
-  // this to only have one vector and have better cache locality.
-  std::vector<Eigen::Array4i> ExtractVoxelData(
-      const HybridGrid& hybrid_grid, const transform::Rigid3f& transform,
-      Eigen::Array2i* min_index, Eigen::Array2i* max_index) const;
-  // Builds texture data containing interleaved value and alpha for the
-  // visualization from 'accumulated_pixel_data'.
-  string ComputePixelValues(
-      const std::vector<PixelData>& accumulated_pixel_data) const;
-
   const proto::SubmapsOptions options_;
 
   // 'submaps_' contains pointers, so that resizing the vector does not