Move AddProbabilityGridToResponse to the Submap object. (#318)

Working towards a thread-safe Submap object. Related to #283.

PAIR=wohe

cartographer/mapping/collated_trajectory_builder.cc

@@ -46,7 +46,7 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
 
 CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {}
 
-const Submaps* CollatedTrajectoryBuilder::submaps() const {
+Submaps* CollatedTrajectoryBuilder::submaps() {
   return wrapped_trajectory_builder_->submaps();
 }
 

cartographer/mapping/collated_trajectory_builder.h

@@ -49,7 +49,7 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilder {
   CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) =
       delete;
 
-  const Submaps* submaps() const override;
+  Submaps* submaps() override;
   const PoseEstimate& pose_estimate() const override;
 
   void AddSensorData(const string& sensor_id,

cartographer/mapping/global_trajectory_builder_interface.h

@@ -46,7 +46,7 @@ class GlobalTrajectoryBuilderInterface {
   GlobalTrajectoryBuilderInterface& operator=(
       const GlobalTrajectoryBuilderInterface&) = delete;
 
-  virtual const Submaps* submaps() const = 0;
+  virtual Submaps* submaps() = 0;
   virtual const PoseEstimate& pose_estimate() const = 0;
 
   virtual void AddRangefinderData(common::Time time,

cartographer/mapping/map_builder.cc

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

cartographer/mapping/submaps.cc

@@ -24,8 +24,6 @@
 namespace cartographer {
 namespace mapping {
 
-constexpr uint8 Submaps::kUnknownLogOdds;
-
 Submaps::Submaps() {}
 
 Submaps::~Submaps() {}
@@ -44,50 +42,5 @@ std::vector<int> Submaps::insertion_indices() const {
   return {size() - 1};
 }
 
-void Submaps::AddProbabilityGridToResponse(
-    const transform::Rigid3d& local_submap_pose,
-    const mapping_2d::ProbabilityGrid& probability_grid,
-    proto::SubmapQuery::Response* response) {
-  Eigen::Array2i offset;
-  mapping_2d::CellLimits limits;
-  probability_grid.ComputeCroppedLimits(&offset, &limits);
-
-  string cells;
-  for (const Eigen::Array2i& xy_index :
-       mapping_2d::XYIndexRangeIterator(limits)) {
-    if (probability_grid.IsKnown(xy_index + offset)) {
-      // We would like to add 'delta' but this is not possible using a value and
-      // alpha. We use premultiplied alpha, so when 'delta' is positive we can
-      // add it by setting 'alpha' to zero. If it is negative, we set 'value' to
-      // zero, and use 'alpha' to subtract. This is only correct when the pixel
-      // is currently white, so walls will look too gray. This should be hard to
-      // detect visually for the user, though.
-      const int delta =
-          128 - ProbabilityToLogOddsInteger(
-                    probability_grid.GetProbability(xy_index + offset));
-      const uint8 alpha = delta > 0 ? 0 : -delta;
-      const uint8 value = delta > 0 ? delta : 0;
-      cells.push_back(value);
-      cells.push_back((value || alpha) ? alpha : 1);
-    } else {
-      cells.push_back(static_cast<uint8>(kUnknownLogOdds));  // value
-      cells.push_back(0);                                    // alpha
-    }
-  }
-  common::FastGzipString(cells, response->mutable_cells());
-
-  response->set_width(limits.num_x_cells);
-  response->set_height(limits.num_y_cells);
-  const double resolution = probability_grid.limits().resolution();
-  response->set_resolution(resolution);
-  const double max_x =
-      probability_grid.limits().max().x() - resolution * offset.y();
-  const double max_y =
-      probability_grid.limits().max().y() - resolution * offset.x();
-  *response->mutable_slice_pose() = transform::ToProto(
-      local_submap_pose.inverse() *
-      transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
-}
-
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/submaps.h

@@ -85,8 +85,6 @@ struct Submap {
 // a "new" submap gets inserted.
 class Submaps {
  public:
-  static constexpr uint8 kUnknownLogOdds = 0;
-
   Submaps();
   virtual ~Submaps();
 
@@ -111,13 +109,7 @@ class Submaps {
   // 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) const = 0;
+                             proto::SubmapQuery::Response* response) = 0;
-
- protected:
-  static void AddProbabilityGridToResponse(
-      const transform::Rigid3d& local_submap_pose,
-      const mapping_2d::ProbabilityGrid& probability_grid,
-      proto::SubmapQuery::Response* response);
 };
 
 }  // namespace mapping

cartographer/mapping/trajectory_builder.h

@@ -59,7 +59,7 @@ class TrajectoryBuilder {
   TrajectoryBuilder(const TrajectoryBuilder&) = delete;
   TrajectoryBuilder& operator=(const TrajectoryBuilder&) = delete;
 
-  virtual const Submaps* submaps() const = 0;
+  virtual Submaps* submaps() = 0;
   virtual const PoseEstimate& pose_estimate() const = 0;
 
   virtual void AddSensorData(const string& sensor_id,

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -28,7 +28,7 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
 
 GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
 
-const Submaps* GlobalTrajectoryBuilder::submaps() const {
+Submaps* GlobalTrajectoryBuilder::submaps() {
   return local_trajectory_builder_.submaps();
 }
 

cartographer/mapping_2d/global_trajectory_builder.h

@@ -35,7 +35,7 @@ class GlobalTrajectoryBuilder
   GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
   GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
 
-  const Submaps* submaps() const override;
+  Submaps* submaps() override;
   const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
       const override;
 

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -36,7 +36,7 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder(
 
 LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
 
-const Submaps* LocalTrajectoryBuilder::submaps() const { return &submaps_; }
+Submaps* LocalTrajectoryBuilder::submaps() { return &submaps_; }
 
 sensor::RangeData LocalTrajectoryBuilder::TransformAndFilterRangeData(
     const transform::Rigid3f& tracking_to_tracking_2d,

cartographer/mapping_2d/local_trajectory_builder.h

@@ -63,7 +63,7 @@ class LocalTrajectoryBuilder {
                   const Eigen::Vector3d& angular_velocity);
   void AddOdometerData(common::Time time, const transform::Rigid3d& pose);
 
-  const Submaps* submaps() const;
+  Submaps* submaps();
 
  private:
   sensor::RangeData TransformAndFilterRangeData(

cartographer/mapping_2d/submaps.cc

@@ -36,13 +36,12 @@ namespace {
 void WriteDebugImage(const string& filename,
                      const ProbabilityGrid& probability_grid) {
   constexpr int kUnknown = 128;
-  const mapping_2d::CellLimits& cell_limits =
+  const CellLimits& cell_limits = probability_grid.limits().cell_limits();
-      probability_grid.limits().cell_limits();
   const int width = cell_limits.num_x_cells;
   const int height = cell_limits.num_y_cells;
   std::vector<uint8_t> rgb;
-  for (const Eigen::Array2i& xy_index : mapping_2d::XYIndexRangeIterator(
+  for (const Eigen::Array2i& xy_index :
-           probability_grid.limits().cell_limits())) {
+       XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
     CHECK(probability_grid.limits().Contains(xy_index));
     const uint8_t value =
         probability_grid.IsKnown(xy_index)
@@ -106,6 +105,49 @@ Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin)
           Eigen::Vector3d(origin.x(), origin.y(), 0.))),
       probability_grid(limits) {}
 
+void Submap::AddProbabilityGridToResponse(
+    mapping::proto::SubmapQuery::Response* response) {
+  Eigen::Array2i offset;
+  CellLimits limits;
+  probability_grid.ComputeCroppedLimits(&offset, &limits);
+
+  string cells;
+  for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) {
+    if (probability_grid.IsKnown(xy_index + offset)) {
+      // We would like to add 'delta' but this is not possible using a value and
+      // alpha. We use premultiplied alpha, so when 'delta' is positive we can
+      // add it by setting 'alpha' to zero. If it is negative, we set 'value' to
+      // zero, and use 'alpha' to subtract. This is only correct when the pixel
+      // is currently white, so walls will look too gray. This should be hard to
+      // detect visually for the user, though.
+      const int delta =
+          128 - mapping::ProbabilityToLogOddsInteger(
+                    probability_grid.GetProbability(xy_index + offset));
+      const uint8 alpha = delta > 0 ? 0 : -delta;
+      const uint8 value = delta > 0 ? delta : 0;
+      cells.push_back(value);
+      cells.push_back((value || alpha) ? alpha : 1);
+    } else {
+      constexpr uint8 kUnknownLogOdds = 0;
+      cells.push_back(static_cast<uint8>(kUnknownLogOdds));  // value
+      cells.push_back(0);                                    // alpha
+    }
+  }
+  common::FastGzipString(cells, response->mutable_cells());
+
+  response->set_width(limits.num_x_cells);
+  response->set_height(limits.num_y_cells);
+  const double resolution = probability_grid.limits().resolution();
+  response->set_resolution(resolution);
+  const double max_x =
+      probability_grid.limits().max().x() - resolution * offset.y();
+  const double max_y =
+      probability_grid.limits().max().y() - resolution * offset.x();
+  *response->mutable_slice_pose() = transform::ToProto(
+      local_pose.inverse() *
+      transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
+}
+
 Submaps::Submaps(const proto::SubmapsOptions& options)
     : options_(options),
       range_data_inserter_(options.range_data_inserter_options()) {
@@ -137,9 +179,8 @@ int Submaps::size() const { return submaps_.size(); }
 
 void Submaps::SubmapToProto(
     const int index, const transform::Rigid3d&,
-    mapping::proto::SubmapQuery::Response* const response) const {
+    mapping::proto::SubmapQuery::Response* const response) {
-  AddProbabilityGridToResponse(Get(index)->local_pose,
+  submaps_.at(index)->AddProbabilityGridToResponse(response);
-                               Get(index)->probability_grid, response);
 }
 
 void Submaps::FinishSubmap(int index) {

cartographer/mapping_2d/submaps.h

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

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -30,7 +30,7 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
 
 GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
 
-const mapping_3d::Submaps* GlobalTrajectoryBuilder::submaps() const {
+mapping_3d::Submaps* GlobalTrajectoryBuilder::submaps() {
   return local_trajectory_builder_->submaps();
 }
 

cartographer/mapping_3d/global_trajectory_builder.h

@@ -38,7 +38,7 @@ class GlobalTrajectoryBuilder
   GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
   GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
 
-  const mapping_3d::Submaps* submaps() const override;
+  mapping_3d::Submaps* submaps() override;
   void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity) override;
   void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,

cartographer/mapping_3d/kalman_local_trajectory_builder.cc

@@ -46,7 +46,7 @@ KalmanLocalTrajectoryBuilder::KalmanLocalTrajectoryBuilder(
 
 KalmanLocalTrajectoryBuilder::~KalmanLocalTrajectoryBuilder() {}
 
-const mapping_3d::Submaps* KalmanLocalTrajectoryBuilder::submaps() const {
+mapping_3d::Submaps* KalmanLocalTrajectoryBuilder::submaps() {
   return submaps_.get();
 }
 

cartographer/mapping_3d/kalman_local_trajectory_builder.h

@@ -53,7 +53,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
       const sensor::PointCloud& ranges) override;
   void AddOdometerData(common::Time time,
                        const transform::Rigid3d& pose) override;
-  const mapping_3d::Submaps* submaps() const override;
+  mapping_3d::Submaps* submaps() override;
   const PoseEstimate& pose_estimate() const override;
 
  private:

cartographer/mapping_3d/local_trajectory_builder_interface.h

@@ -57,7 +57,7 @@ class LocalTrajectoryBuilderInterface {
   virtual void AddOdometerData(common::Time time,
                                const transform::Rigid3d& pose) = 0;
 
-  virtual const mapping_3d::Submaps* submaps() const = 0;
+  virtual mapping_3d::Submaps* submaps() = 0;
   virtual const PoseEstimate& pose_estimate() const = 0;
 
  protected:

cartographer/mapping_3d/optimizing_local_trajectory_builder.cc

@@ -112,7 +112,7 @@ OptimizingLocalTrajectoryBuilder::OptimizingLocalTrajectoryBuilder(
 
 OptimizingLocalTrajectoryBuilder::~OptimizingLocalTrajectoryBuilder() {}
 
-const mapping_3d::Submaps* OptimizingLocalTrajectoryBuilder::submaps() const {
+mapping_3d::Submaps* OptimizingLocalTrajectoryBuilder::submaps() {
   return submaps_.get();
 }
 

cartographer/mapping_3d/optimizing_local_trajectory_builder.h

@@ -56,7 +56,7 @@ class OptimizingLocalTrajectoryBuilder
       const sensor::PointCloud& ranges) override;
   void AddOdometerData(common::Time time,
                        const transform::Rigid3d& pose) override;
-  const mapping_3d::Submaps* submaps() const override;
+  mapping_3d::Submaps* submaps() override;
   const PoseEstimate& pose_estimate() const override;
 
  private:

cartographer/mapping_3d/submaps.cc

@@ -243,7 +243,7 @@ int Submaps::size() const { return submaps_.size(); }
 
 void Submaps::SubmapToProto(
     int index, const transform::Rigid3d& global_submap_pose,
-    mapping::proto::SubmapQuery::Response* const response) const {
+    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;

cartographer/mapping_3d/submaps.h

@@ -66,9 +66,8 @@ class Submaps : public mapping::Submaps {
 
   const Submap* Get(int index) const override;
   int size() const override;
-  void SubmapToProto(
+  void SubmapToProto(int index, const transform::Rigid3d& global_submap_pose,
-      int index, const transform::Rigid3d& global_submap_pose,
+                     mapping::proto::SubmapQuery::Response* response) override;
-      mapping::proto::SubmapQuery::Response* response) const override;
 
   // Inserts 'range_data' into the Submap collection. 'gravity_alignment' is
   // used for the orientation of new submaps so that the z axis approximately