From 5b16f4bcb6e97e6a903b6fa010a8a8f1c13b57f6 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 20 Oct 2016 11:29:12 +0200 Subject: [PATCH] Simplify 3D visualization code. (#88) Removes the non-const Submaps accessor. Adds -Werror=missing-braces. --- CMakeLists.txt | 2 +- .../mapping/collated_trajectory_builder.cc | 4 -- .../mapping/collated_trajectory_builder.h | 1 - .../global_trajectory_builder_interface.h | 1 - .../mapping/sparse_pose_graph_test.cc | 2 +- cartographer/mapping/submaps.h | 2 +- cartographer/mapping/trajectory_builder.h | 1 - .../mapping_2d/global_trajectory_builder.cc | 4 -- .../mapping_2d/global_trajectory_builder.h | 1 - .../mapping_2d/local_trajectory_builder.cc | 2 - .../mapping_2d/local_trajectory_builder.h | 1 - cartographer/mapping_2d/submaps.cc | 2 +- cartographer/mapping_2d/submaps.h | 2 +- .../mapping_3d/global_trajectory_builder.cc | 4 -- .../mapping_3d/global_trajectory_builder.h | 1 - .../kalman_local_trajectory_builder.cc | 2 +- .../kalman_local_trajectory_builder.h | 2 +- .../local_trajectory_builder_interface.h | 2 +- .../optimizing_local_trajectory_builder.cc | 2 +- .../optimizing_local_trajectory_builder.h | 2 +- cartographer/mapping_3d/submaps.cc | 69 +++++++++---------- cartographer/mapping_3d/submaps.h | 34 +++++---- cartographer/sensor/collator_test.cc | 4 +- cmake/functions.cmake | 1 + 24 files changed, 63 insertions(+), 85 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4308394..423f62b 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/cartographer/mapping/collated_trajectory_builder.cc b/cartographer/mapping/collated_trajectory_builder.cc index e1a6b37..46fca74 100644 --- a/cartographer/mapping/collated_trajectory_builder.cc +++ b/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(); } diff --git a/cartographer/mapping/collated_trajectory_builder.h b/cartographer/mapping/collated_trajectory_builder.h index 3ce7c83..3d15ac7 100644 --- a/cartographer/mapping/collated_trajectory_builder.h +++ b/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; diff --git a/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/mapping/global_trajectory_builder_interface.h index f29e4d8..a268076 100644 --- a/cartographer/mapping/global_trajectory_builder_interface.h +++ b/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; diff --git a/cartographer/mapping/sparse_pose_graph_test.cc b/cartographer/mapping/sparse_pose_graph_test.cc index 681afe3..e0fb635 100644 --- a/cartographer/mapping/sparse_pose_graph_test.cc +++ b/cartographer/mapping/sparse_pose_graph_test.cc @@ -33,7 +33,7 @@ class FakeSubmaps : public Submaps { void SubmapToProto(int, const std::vector&, const transform::Rigid3d&, - proto::SubmapQuery::Response*) override { + proto::SubmapQuery::Response*) const override { LOG(FATAL) << "Not implemented."; } }; diff --git a/cartographer/mapping/submaps.h b/cartographer/mapping/submaps.h index ae3f698..9ac4307 100644 --- a/cartographer/mapping/submaps.h +++ b/cartographer/mapping/submaps.h @@ -121,7 +121,7 @@ class Submaps { virtual void SubmapToProto( int index, const std::vector& trajectory_nodes, const transform::Rigid3d& global_submap_pose, - proto::SubmapQuery::Response* response) = 0; + proto::SubmapQuery::Response* response) const = 0; protected: static void AddProbabilityGridToResponse( diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h index 19b0175..66cb0f9 100644 --- a/cartographer/mapping/trajectory_builder.h +++ b/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; diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index ff6242b..a67ce0d 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/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(); } diff --git a/cartographer/mapping_2d/global_trajectory_builder.h b/cartographer/mapping_2d/global_trajectory_builder.h index 136a6db..6e5aab6 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.h +++ b/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; diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 77101c2..5f2553c 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/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(); } diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index a7c6829..4f0893a 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/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: diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc index a56df15..e1362c6 100644 --- a/cartographer/mapping_2d/submaps.cc +++ b/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&, 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); } diff --git a/cartographer/mapping_2d/submaps.h b/cartographer/mapping_2d/submaps.h index d035ea9..2e8de6e 100644 --- a/cartographer/mapping_2d/submaps.h +++ b/cartographer/mapping_2d/submaps.h @@ -61,7 +61,7 @@ class Submaps : public mapping::Submaps { void SubmapToProto( int index, const std::vector& 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); diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index 05e209c..50776c6 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/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(); } diff --git a/cartographer/mapping_3d/global_trajectory_builder.h b/cartographer/mapping_3d/global_trajectory_builder.h index 3a15d2e..2545fed 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.h +++ b/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; diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index adebe96..290c096 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/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(); } diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.h b/cartographer/mapping_3d/kalman_local_trajectory_builder.h index 7006063..5a6f057 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.h +++ b/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; diff --git a/cartographer/mapping_3d/local_trajectory_builder_interface.h b/cartographer/mapping_3d/local_trajectory_builder_interface.h index 8755f96..4fbbc9e 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_interface.h +++ b/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; diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc index 1cca04f..6e94ce1 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc +++ b/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(); } diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h index c20f4f0..c2b0e9c 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h +++ b/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: diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submaps.cc index 0e532c9..7a4f94d 100644 --- a/cartographer/mapping_3d/submaps.cc +++ b/cartographer/mapping_3d/submaps.cc @@ -20,7 +20,6 @@ #include #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& 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(), - &min_index, &max_index); + const std::vector voxel_indices_and_probabilities = + ExtractVoxelData(hybrid_grid, + (global_submap_pose * Get(index)->local_pose().inverse()) + .cast(), + &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 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::AccumulatePixelData( + const int width, const int height, const Eigen::Array2i& min_index, + const Eigen::Array2i& max_index, + const std::vector& voxel_indices_and_probabilities) const { + std::vector 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 Submaps::ExtractVoxelData( + const HybridGrid& hybrid_grid, const transform::Rigid3f& transform, + Eigen::Array2i* min_index, Eigen::Array2i* max_index) const { + std::vector 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& 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 diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h index 961946f..b6ac10a 100644 --- a/cartographer/mapping_3d/submaps.h +++ b/cartographer/mapping_3d/submaps.h @@ -22,6 +22,7 @@ #include #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& 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 AccumulatePixelData( + const int width, const int height, const Eigen::Array2i& min_index, + const Eigen::Array2i& max_index, + const std::vector& 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 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& 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 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 voxel_indices_and_probabilities_; }; } // namespace mapping_3d diff --git a/cartographer/sensor/collator_test.cc b/cartographer/sensor/collator_test.cc index 19157ef..f7b7cce 100644 --- a/cartographer/sensor/collator_test.cc +++ b/cartographer/sensor/collator_test.cc @@ -29,8 +29,8 @@ namespace sensor { namespace { TEST(Collator, Ordering) { - const std::array kSensorId = {"horizontal_laser", "vertical_laser", - "imu", "odometry"}; + const std::array 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{}); diff --git a/cmake/functions.cmake b/cmake/functions.cmake index 431ab08..6aa807c 100644 --- a/cmake/functions.cmake +++ b/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")