Simplify 3D visualization code. (#88)

Removes the non-const Submaps accessor.
Adds -Werror=missing-braces.

CMakeLists.txt

@@ -71,7 +71,7 @@ google_combined_library(cartographer
 )
 
 get_property(CARTOGRAPHER_LIBRARY_FILE TARGET cartographer PROPERTY LOCATION)
-get_filename_component(CARTOGRAPHER_LIBRARY_FILE_BASENAME 
+get_filename_component(CARTOGRAPHER_LIBRARY_FILE_BASENAME
   ${CARTOGRAPHER_LIBRARY_FILE} NAME)
 install(
   FILES

cartographer/mapping/collated_trajectory_builder.cc

@@ -50,10 +50,6 @@ const Submaps* CollatedTrajectoryBuilder::submaps() const {
   return wrapped_trajectory_builder_->submaps();
 }
 
-Submaps* CollatedTrajectoryBuilder::submaps() {
-  return wrapped_trajectory_builder_->submaps();
-}
-
 kalman_filter::PoseTracker* CollatedTrajectoryBuilder::pose_tracker() const {
   return wrapped_trajectory_builder_->pose_tracker();
 }

cartographer/mapping/collated_trajectory_builder.h

@@ -51,7 +51,6 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilder {
       delete;
 
   const Submaps* submaps() const override;
-  Submaps* submaps() override;
   kalman_filter::PoseTracker* pose_tracker() const override;
   const PoseEstimate& pose_estimate() const override;
 

cartographer/mapping/global_trajectory_builder_interface.h

@@ -48,7 +48,6 @@ class GlobalTrajectoryBuilderInterface {
       const GlobalTrajectoryBuilderInterface&) = delete;
 
   virtual const Submaps* submaps() const = 0;
-  virtual Submaps* submaps() = 0;
   virtual kalman_filter::PoseTracker* pose_tracker() const = 0;
   virtual const PoseEstimate& pose_estimate() const = 0;
 

cartographer/mapping/sparse_pose_graph_test.cc

@@ -33,7 +33,7 @@ class FakeSubmaps : public Submaps {
 
   void SubmapToProto(int, const std::vector<mapping::TrajectoryNode>&,
                      const transform::Rigid3d&,
-                     proto::SubmapQuery::Response*) override {
+                     proto::SubmapQuery::Response*) const override {
     LOG(FATAL) << "Not implemented.";
   }
 };

cartographer/mapping/submaps.h

@@ -121,7 +121,7 @@ class Submaps {
   virtual void SubmapToProto(
       int index, const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
       const transform::Rigid3d& global_submap_pose,
-      proto::SubmapQuery::Response* response) = 0;
+      proto::SubmapQuery::Response* response) const = 0;
 
  protected:
   static void AddProbabilityGridToResponse(

cartographer/mapping/trajectory_builder.h

@@ -78,7 +78,6 @@ class TrajectoryBuilder {
   TrajectoryBuilder& operator=(const TrajectoryBuilder&) = delete;
 
   virtual const Submaps* submaps() const = 0;
-  virtual Submaps* submaps() = 0;
   virtual kalman_filter::PoseTracker* pose_tracker() const = 0;
   virtual const PoseEstimate& pose_estimate() const = 0;
 

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -32,10 +32,6 @@ const Submaps* GlobalTrajectoryBuilder::submaps() const {
   return local_trajectory_builder_.submaps();
 }
 
-Submaps* GlobalTrajectoryBuilder::submaps() {
-  return local_trajectory_builder_.submaps();
-}
-
 kalman_filter::PoseTracker* GlobalTrajectoryBuilder::pose_tracker() const {
   return local_trajectory_builder_.pose_tracker();
 }

cartographer/mapping_2d/global_trajectory_builder.h

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

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -72,8 +72,6 @@ LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
 
 const Submaps* LocalTrajectoryBuilder::submaps() const { return &submaps_; }
 
-Submaps* LocalTrajectoryBuilder::submaps() { return &submaps_; }
-
 kalman_filter::PoseTracker* LocalTrajectoryBuilder::pose_tracker() const {
   return pose_tracker_.get();
 }

cartographer/mapping_2d/local_trajectory_builder.h

@@ -71,7 +71,6 @@ class LocalTrajectoryBuilder {
                        const kalman_filter::PoseCovariance& covariance);
 
   const Submaps* submaps() const;
-  Submaps* submaps();
   kalman_filter::PoseTracker* pose_tracker() const;
 
  private:

cartographer/mapping_2d/submaps.cc

@@ -140,7 +140,7 @@ int Submaps::size() const { return submaps_.size(); }
 void Submaps::SubmapToProto(
     const int index, const std::vector<mapping::TrajectoryNode>&,
     const transform::Rigid3d&,
-    mapping::proto::SubmapQuery::Response* const response) {
+    mapping::proto::SubmapQuery::Response* const response) const {
   AddProbabilityGridToResponse(Get(index)->local_pose(),
                                Get(index)->probability_grid, response);
 }

cartographer/mapping_2d/submaps.h

@@ -61,7 +61,7 @@ class Submaps : public mapping::Submaps {
   void SubmapToProto(
       int index, const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
       const transform::Rigid3d& global_submap_pose,
-      mapping::proto::SubmapQuery::Response* response) override;
+      mapping::proto::SubmapQuery::Response* response) const override;
 
   // Inserts 'laser_fan' into the Submap collection.
   void InsertLaserFan(const sensor::LaserFan& laser_fan);

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -33,10 +33,6 @@ const mapping_3d::Submaps* GlobalTrajectoryBuilder::submaps() const {
   return local_trajectory_builder_->submaps();
 }
 
-mapping_3d::Submaps* GlobalTrajectoryBuilder::submaps() {
-  return local_trajectory_builder_->submaps();
-}
-
 kalman_filter::PoseTracker* GlobalTrajectoryBuilder::pose_tracker() const {
   return local_trajectory_builder_->pose_tracker();
 }

cartographer/mapping_3d/global_trajectory_builder.h

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

cartographer/mapping_3d/kalman_local_trajectory_builder.cc

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

cartographer/mapping_3d/kalman_local_trajectory_builder.h

@@ -55,7 +55,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
       const kalman_filter::PoseCovariance& covariance) override;
 
   void AddTrajectoryNodeIndex(int trajectory_node_index) override;
-  mapping_3d::Submaps* submaps() override;
+  const mapping_3d::Submaps* submaps() const override;
   const PoseEstimate& pose_estimate() const override;
   kalman_filter::PoseTracker* pose_tracker() const override;
 

cartographer/mapping_3d/local_trajectory_builder_interface.h

@@ -63,7 +63,7 @@ class LocalTrajectoryBuilderInterface {
   // to the latest inserted laser scan. This is used to remember which
   // trajectory node should be used to visualize a Submap.
   virtual void AddTrajectoryNodeIndex(int trajectory_node_index) = 0;
-  virtual mapping_3d::Submaps* submaps() = 0;
+  virtual const mapping_3d::Submaps* submaps() const = 0;
   virtual const PoseEstimate& pose_estimate() const = 0;
   virtual kalman_filter::PoseTracker* pose_tracker() const = 0;
 

cartographer/mapping_3d/optimizing_local_trajectory_builder.cc

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

cartographer/mapping_3d/optimizing_local_trajectory_builder.h

@@ -63,7 +63,7 @@ class OptimizingLocalTrajectoryBuilder
   kalman_filter::PoseTracker* pose_tracker() const override { return nullptr; }
 
   void AddTrajectoryNodeIndex(int trajectory_node_index) override;
-  mapping_3d::Submaps* submaps() override;
+  const mapping_3d::Submaps* submaps() const override;
   const PoseEstimate& pose_estimate() const override;
 
  private:

cartographer/mapping_3d/submaps.cc

@@ -20,7 +20,6 @@
 #include <limits>
 
 #include "cartographer/common/math.h"
-#include "cartographer/common/port.h"
 #include "cartographer/sensor/laser.h"
 #include "glog/logging.h"
 
@@ -235,7 +234,7 @@ int Submaps::size() const { return submaps_.size(); }
 void Submaps::SubmapToProto(
     int index, const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
     const transform::Rigid3d& global_submap_pose,
-    mapping::proto::SubmapQuery::Response* const response) {
+    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 HybridGrid& hybrid_grid = Get(index)->high_resolution_hybrid_grid;
@@ -244,20 +243,22 @@ void Submaps::SubmapToProto(
   // Compute a bounding box for the texture.
   Eigen::Array2i min_index(INT_MAX, INT_MAX);
   Eigen::Array2i max_index(INT_MIN, INT_MIN);
-  ExtractVoxelData(
-      hybrid_grid,
-      (global_submap_pose * Get(index)->local_pose().inverse()).cast<float>(),
-      &min_index, &max_index);
+  const std::vector<Eigen::Array4i> voxel_indices_and_probabilities =
+      ExtractVoxelData(hybrid_grid,
+                       (global_submap_pose * Get(index)->local_pose().inverse())
+                           .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);
 
-  AccumulatePixelData(width, height, min_index, max_index);
-  ComputePixelValues(width, 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(celldata_, response->mutable_cells());
+  common::FastGzipString(cell_data, response->mutable_cells());
   *response->mutable_slice_pose() =
       transform::ToProto(global_submap_pose.inverse() *
                          transform::Rigid3d::Translation(Eigen::Vector3d(
@@ -315,13 +316,13 @@ void Submaps::AddSubmap(const Eigen::Vector3f& origin) {
   num_laser_fans_in_last_submap_ = 0;
 }
 
-void Submaps::AccumulatePixelData(const int width, const int height,
-                                  const Eigen::Array2i& min_index,
-                                  const Eigen::Array2i& max_index) {
-  accumulated_pixel_data_.clear();
-  accumulated_pixel_data_.resize(width * height);
+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_) {
+       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.
@@ -329,7 +330,7 @@ void Submaps::AccumulatePixelData(const int width, const int height,
     }
     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];
+    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]);
@@ -338,13 +339,13 @@ void Submaps::AccumulatePixelData(const int width, const int height,
     pixel.probability_sum += probability;
     pixel.max_probability = std::max(pixel.max_probability, probability);
   }
+  return accumulated_pixel_data;
 }
 
-void Submaps::ExtractVoxelData(const HybridGrid& hybrid_grid,
-                               const transform::Rigid3f& transform,
-                               Eigen::Array2i* min_index,
-                               Eigen::Array2i* max_index) {
-  voxel_indices_and_probabilities_.clear();
+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;
@@ -365,29 +366,28 @@ void Submaps::ExtractVoxelData(const HybridGrid& hybrid_grid,
         common::RoundToInt(cell_center_global.z() * resolution_inverse),
         probability_value);
 
-    voxel_indices_and_probabilities_.push_back(voxel_index_and_probability);
+    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;
 }
 
-void Submaps::ComputePixelValues(const int width, const int height) {
-  celldata_.resize(2 * width * height);
+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;
-  auto it = celldata_.begin();
-  for (size_t i = 0; i < accumulated_pixel_data_.size(); ++i) {
-    const PixelData& pixel = accumulated_pixel_data_.at(i);
+  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) {
-      *it = 0;  // value
-      ++it;
-      *it = 0;  // alpha
-      ++it;
+      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);
@@ -401,11 +401,10 @@ void Submaps::ComputePixelValues(const int width, const int height) {
         128 - mapping::ProbabilityToLogOddsInteger(average_probability);
     const uint8 alpha = delta > 0 ? 0 : -delta;
     const uint8 value = delta > 0 ? delta : 0;
-    *it = value;  // value
-    ++it;
-    *it = (value || alpha) ? alpha : 1;  // alpha
-    ++it;
+    cell_data.push_back(value);                         // value
+    cell_data.push_back((value || alpha) ? alpha : 1);  // alpha
   }
+  return cell_data;
 }
 
 }  // namespace mapping_3d

cartographer/mapping_3d/submaps.h

@@ -22,6 +22,7 @@
 #include <vector>
 
 #include "Eigen/Geometry"
+#include "cartographer/common/port.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/mapping_2d/laser_fan_inserter.h"
@@ -66,7 +67,7 @@ class Submaps : public mapping::Submaps {
   void SubmapToProto(
       int index, const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
       const transform::Rigid3d& global_submap_pose,
-      mapping::proto::SubmapQuery::Response* response) override;
+      mapping::proto::SubmapQuery::Response* response) const override;
 
   // Inserts 'laser_fan' into the Submap collection.
   void InsertLaserFan(const sensor::LaserFan& laser_fan);
@@ -90,15 +91,21 @@ class Submaps : public mapping::Submaps {
   };
 
   void AddSubmap(const Eigen::Vector3f& origin);
-  void AccumulatePixelData(const int width, const int height,
-                           const Eigen::Array2i& min_index,
-                           const Eigen::Array2i& max_index);
-  void ExtractVoxelData(const HybridGrid& hybrid_grid,
-                        const transform::Rigid3f& transform,
-                        Eigen::Array2i* min_index, Eigen::Array2i* max_index);
+
+  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_' into 'celldata_'.
-  void ComputePixelValues(const int width, const int height);
+  // visualization from 'accumulated_pixel_data'.
+  string ComputePixelValues(
+      const std::vector<PixelData>& accumulated_pixel_data) const;
 
   const proto::SubmapsOptions options_;
 
@@ -110,15 +117,6 @@ class Submaps : public mapping::Submaps {
 
   // Number of LaserFans inserted since the last Submap was added.
   int num_laser_fans_in_last_submap_ = 0;
-
-  // The following members are used for visualization and kept around for
-  // performance reasons (mainly to avoid reallocations).
-  std::vector<PixelData> accumulated_pixel_data_;
-  string celldata_;
-  // The first three entries of this is 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> voxel_indices_and_probabilities_;
 };
 
 }  // namespace mapping_3d

cartographer/sensor/collator_test.cc

@@ -29,8 +29,8 @@ namespace sensor {
 namespace {
 
 TEST(Collator, Ordering) {
-  const std::array<string, 4> kSensorId = {"horizontal_laser", "vertical_laser",
-                                           "imu", "odometry"};
+  const std::array<string, 4> kSensorId = {
+      {"horizontal_laser", "vertical_laser", "imu", "odometry"}};
   Data first(common::FromUniversal(100), sensor::LaserFan{});
   Data second(common::FromUniversal(200), sensor::LaserFan{});
   Data third(common::FromUniversal(300), Data::Imu{});

cmake/functions.cmake

@@ -331,6 +331,7 @@ macro(google_initialize_cartographer_project)
 
   # Turn some warnings into errors.
   google_add_flag(GOOG_CXX_FLAGS "-Werror=format-security")
+  google_add_flag(GOOG_CXX_FLAGS "-Werror=missing-braces")
   google_add_flag(GOOG_CXX_FLAGS "-Werror=reorder")
   google_add_flag(GOOG_CXX_FLAGS "-Werror=return-type")
   google_add_flag(GOOG_CXX_FLAGS "-Werror=uninitialized")