Fix ClangTidy warnings. (#455)

cartographer/common/thread_pool.cc

@@ -45,7 +45,7 @@ ThreadPool::~ThreadPool() {
   }
 }
 
-void ThreadPool::Schedule(std::function<void()> work_item) {
+void ThreadPool::Schedule(const std::function<void()>& work_item) {
   MutexLocker locker(&mutex_);
   CHECK(running_);
   work_queue_.push_back(work_item);

cartographer/common/thread_pool.h

@@ -40,7 +40,7 @@ class ThreadPool {
   ThreadPool(const ThreadPool&) = delete;
   ThreadPool& operator=(const ThreadPool&) = delete;
 
-  void Schedule(std::function<void()> work_item);
+  void Schedule(const std::function<void()>& work_item);
 
  private:
   void DoWork();

cartographer/io/hybrid_grid_points_processor.cc

@@ -28,7 +28,7 @@ HybridGridPointsProcessor::HybridGridPointsProcessor(
 
 std::unique_ptr<HybridGridPointsProcessor>
 HybridGridPointsProcessor::FromDictionary(
-    FileWriterFactory file_writer_factory,
+    const FileWriterFactory& file_writer_factory,
     common::LuaParameterDictionary* const dictionary,
     PointsProcessor* const next) {
   return common::make_unique<HybridGridPointsProcessor>(

cartographer/io/hybrid_grid_points_processor.h

@@ -33,7 +33,7 @@ class HybridGridPointsProcessor : public PointsProcessor {
       delete;
 
   static std::unique_ptr<HybridGridPointsProcessor> FromDictionary(
-      FileWriterFactory file_writer_factory,
+      const FileWriterFactory& file_writer_factory,
       common::LuaParameterDictionary* dictionary, PointsProcessor* next);
 
   ~HybridGridPointsProcessor() override {}

cartographer/io/outlier_removing_points_processor.cc

@@ -41,18 +41,18 @@ OutlierRemovingPointsProcessor::OutlierRemovingPointsProcessor(
 }
 
 void OutlierRemovingPointsProcessor::Process(
-    std::unique_ptr<PointsBatch> points) {
+    std::unique_ptr<PointsBatch> batch) {
   switch (state_) {
     case State::kPhase1:
-      ProcessInPhaseOne(*points);
+      ProcessInPhaseOne(*batch);
       break;
 
     case State::kPhase2:
-      ProcessInPhaseTwo(*points);
+      ProcessInPhaseTwo(*batch);
       break;
 
     case State::kPhase3:
-      ProcessInPhaseThree(std::move(points));
+      ProcessInPhaseThree(std::move(batch));
       break;
   }
 }

cartographer/io/ply_writing_points_processor.cc

@@ -71,7 +71,7 @@ void WriteBinaryPlyPointColor(const Uint8Color& color,
 
 std::unique_ptr<PlyWritingPointsProcessor>
 PlyWritingPointsProcessor::FromDictionary(
-    FileWriterFactory file_writer_factory,
+    const FileWriterFactory& file_writer_factory,
     common::LuaParameterDictionary* const dictionary,
     PointsProcessor* const next) {
   return common::make_unique<PlyWritingPointsProcessor>(

cartographer/io/ply_writing_points_processor.h

@@ -25,11 +25,11 @@ namespace io {
 class PlyWritingPointsProcessor : public PointsProcessor {
  public:
   constexpr static const char* kConfigurationFileActionName = "write_ply";
-  PlyWritingPointsProcessor(std::unique_ptr<FileWriter> file,
+  PlyWritingPointsProcessor(std::unique_ptr<FileWriter> file_writer,
                             PointsProcessor* next);
 
   static std::unique_ptr<PlyWritingPointsProcessor> FromDictionary(
-      FileWriterFactory file_writer_factory,
+      const FileWriterFactory& file_writer_factory,
       common::LuaParameterDictionary* dictionary, PointsProcessor* next);
 
   ~PlyWritingPointsProcessor() override {}

cartographer/io/points_processor_pipeline_builder.cc

@@ -48,7 +48,7 @@ void RegisterPlainPointsProcessor(
 
 template <typename PointsProcessorType>
 void RegisterFileWritingPointsProcessor(
-    FileWriterFactory file_writer_factory,
+    const FileWriterFactory& file_writer_factory,
     PointsProcessorPipelineBuilder* const builder) {
   builder->Register(
       PointsProcessorType::kConfigurationFileActionName,
@@ -63,7 +63,7 @@ void RegisterFileWritingPointsProcessor(
 template <typename PointsProcessorType>
 void RegisterFileWritingPointsProcessorWithTrajectories(
     const std::vector<mapping::proto::Trajectory>& trajectories,
-    FileWriterFactory file_writer_factory,
+    const FileWriterFactory& file_writer_factory,
     PointsProcessorPipelineBuilder* const builder) {
   builder->Register(
       PointsProcessorType::kConfigurationFileActionName,
@@ -77,7 +77,7 @@ void RegisterFileWritingPointsProcessorWithTrajectories(
 
 void RegisterBuiltInPointsProcessors(
     const std::vector<mapping::proto::Trajectory>& trajectories,
-    FileWriterFactory file_writer_factory,
+    const FileWriterFactory& file_writer_factory,
     PointsProcessorPipelineBuilder* builder) {
   RegisterPlainPointsProcessor<CountingPointsProcessor>(builder);
   RegisterPlainPointsProcessor<FixedRatioSamplingPointsProcessor>(builder);
@@ -104,7 +104,7 @@ void PointsProcessorPipelineBuilder::Register(const std::string& name,
                                               FactoryFunction factory) {
   CHECK(factories_.count(name) == 0) << "A points processor named '" << name
                                      << "' has already been registered.";
-  factories_[name] = factory;
+  factories_[name] = std::move(factory);
 }
 
 PointsProcessorPipelineBuilder::PointsProcessorPipelineBuilder() {}

cartographer/io/probability_grid_points_processor.cc

@@ -37,9 +37,9 @@ void WriteGrid(
     }
   };
   Image image(cell_limits.num_x_cells, cell_limits.num_y_cells);
-  for (auto xy_index :
+  for (const auto& xy_index :
        cartographer::mapping_2d::XYIndexRangeIterator(cell_limits)) {
-    auto index = xy_index + offset;
+    const auto index = xy_index + offset;
     uint8 value = compute_color_value(index);
     image.SetPixel(xy_index.x(), xy_index.y(), {{value, value, value}});
   }
@@ -91,7 +91,7 @@ ProbabilityGridPointsProcessor::ProbabilityGridPointsProcessor(
 std::unique_ptr<ProbabilityGridPointsProcessor>
 ProbabilityGridPointsProcessor::FromDictionary(
     const std::vector<mapping::proto::Trajectory>& trajectories,
-    FileWriterFactory file_writer_factory,
+    const FileWriterFactory& file_writer_factory,
     common::LuaParameterDictionary* const dictionary,
     PointsProcessor* const next) {
   const auto draw_trajectories = (!dictionary->HasKey("draw_trajectories") ||

cartographer/io/probability_grid_points_processor.h

@@ -39,7 +39,7 @@ class ProbabilityGridPointsProcessor : public PointsProcessor {
 
   static std::unique_ptr<ProbabilityGridPointsProcessor> FromDictionary(
       const std::vector<mapping::proto::Trajectory>& trajectories,
-      FileWriterFactory file_writer_factory,
+      const FileWriterFactory& file_writer_factory,
       common::LuaParameterDictionary* dictionary, PointsProcessor* next);
 
   ~ProbabilityGridPointsProcessor() override {}

cartographer/io/xyz_writing_points_processor.cc

@@ -27,7 +27,7 @@ XyzWriterPointsProcessor::XyzWriterPointsProcessor(
 
 std::unique_ptr<XyzWriterPointsProcessor>
 XyzWriterPointsProcessor::FromDictionary(
-    FileWriterFactory file_writer_factory,
+    const FileWriterFactory& file_writer_factory,
     common::LuaParameterDictionary* const dictionary,
     PointsProcessor* const next) {
   return common::make_unique<XyzWriterPointsProcessor>(

cartographer/io/xyz_writing_points_processor.h

@@ -36,7 +36,7 @@ class XyzWriterPointsProcessor : public PointsProcessor {
   XyzWriterPointsProcessor(std::unique_ptr<FileWriter>, PointsProcessor* next);
 
   static std::unique_ptr<XyzWriterPointsProcessor> FromDictionary(
-      FileWriterFactory file_writer_factory,
+      const FileWriterFactory& file_writer_factory,
       common::LuaParameterDictionary* dictionary, PointsProcessor* next);
 
   ~XyzWriterPointsProcessor() override {}

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -41,7 +41,7 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
       insertion_result->time, insertion_result->tracking_to_tracking_2d,
       insertion_result->range_data_in_tracking_2d,
       insertion_result->pose_estimate_2d, trajectory_id_,
-      std::move(insertion_result->insertion_submaps));
+      insertion_result->insertion_submaps);
 }
 
 void GlobalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -174,7 +174,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
   // Querying the active submaps must be done here before calling
   // InsertRangeData() since the queried values are valid for next insertion.
   std::vector<std::shared_ptr<const Submap>> insertion_submaps;
-  for (std::shared_ptr<Submap> submap : active_submaps_.submaps()) {
+  for (const std::shared_ptr<Submap>& submap : active_submaps_.submaps()) {
     insertion_submaps.push_back(submap);
   }
   active_submaps_.InsertRangeData(

cartographer/mapping_2d/local_trajectory_builder.h

@@ -58,7 +58,8 @@ class LocalTrajectoryBuilder {
   std::unique_ptr<InsertionResult> AddHorizontalRangeData(
       common::Time, const sensor::RangeData& range_data);
   void AddImuData(const sensor::ImuData& imu_data);
-  void AddOdometerData(common::Time time, const transform::Rigid3d& pose);
+  void AddOdometerData(common::Time time,
+                       const transform::Rigid3d& odometer_pose);
 
  private:
   std::unique_ptr<InsertionResult> AddAccumulatedRangeData(

cartographer/mapping_2d/scan_matching/fast_global_localizer.cc

@@ -38,7 +38,7 @@ bool PerformGlobalLocalization(
   const sensor::PointCloud filtered_point_cloud =
       voxel_filter.Filter(point_cloud);
   bool success = false;
-  if (matchers.size() == 0) {
+  if (matchers.empty()) {
     LOG(WARNING) << "Map not yet large enough to localize in!";
     return false;
   }

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -148,7 +148,7 @@ void SparsePoseGraph::AddScan(
   });
 }
 
-void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
+void SparsePoseGraph::AddWorkItem(const std::function<void()>& work_item) {
   if (scan_queue_ == nullptr) {
     work_item();
   } else {
@@ -432,6 +432,10 @@ void SparsePoseGraph::RunOptimization() {
   if (optimization_problem_.submap_data().empty()) {
     return;
   }
+
+  // No other thread is accessing the optimization_problem_, constraints_ and
+  // frozen_trajectories_ when executing the Solve. Solve is time consuming, so
+  // not taking the mutex before Solve to avoid blocking foreground processing.
   optimization_problem_.Solve(constraints_, frozen_trajectories_);
   common::MutexLocker locker(&mutex_);
 

cartographer/mapping_2d/sparse_pose_graph.h

@@ -114,7 +114,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   };
 
   // Handles a new work item.
-  void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
+  void AddWorkItem(const std::function<void()>& work_item) REQUIRES(mutex_);
 
   // Grows the optimization problem to have an entry for every element of
   // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -111,7 +111,7 @@ void ConstraintBuilder::NotifyEndOfScan() {
 }
 
 void ConstraintBuilder::WhenDone(
-    const std::function<void(const ConstraintBuilder::Result&)> callback) {
+    const std::function<void(const ConstraintBuilder::Result&)>& callback) {
   common::MutexLocker locker(&mutex_);
   CHECK(when_done_ == nullptr);
   when_done_ =
@@ -124,7 +124,7 @@ void ConstraintBuilder::WhenDone(
 
 void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
     const mapping::SubmapId& submap_id, const ProbabilityGrid* const submap,
-    const std::function<void()> work_item) {
+    const std::function<void()>& work_item) {
   if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher !=
       nullptr) {
     thread_pool_->Schedule(work_item);

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h

@@ -102,7 +102,7 @@ class ConstraintBuilder {
 
   // Registers the 'callback' to be called with the results, after all
   // computations triggered by MaybeAddConstraint() have finished.
-  void WhenDone(std::function<void(const Result&)> callback);
+  void WhenDone(const std::function<void(const Result&)>& callback);
 
   // Returns the number of consecutive finished scans.
   int GetNumFinishedScans();
@@ -121,7 +121,7 @@ class ConstraintBuilder {
   // construction and queues the 'work_item'.
   void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
       const mapping::SubmapId& submap_id, const ProbabilityGrid* submap,
-      std::function<void()> work_item) REQUIRES(mutex_);
+      const std::function<void()>& work_item) REQUIRES(mutex_);
 
   // Constructs the scan matcher for a 'submap', then schedules its work items.
   void ConstructSubmapScanMatcher(const mapping::SubmapId& submap_id,

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -158,7 +158,7 @@ class SparsePoseGraphTest : public ::testing::Test {
         point_cloud_,
         transform::Embed3D(current_pose_.inverse().cast<float>()));
     std::vector<std::shared_ptr<const Submap>> insertion_submaps;
-    for (auto submap : active_submaps_->submaps()) {
+    for (const auto& submap : active_submaps_->submaps()) {
       insertion_submaps.push_back(submap);
     }
     const sensor::RangeData range_data{
@@ -170,7 +170,7 @@ class SparsePoseGraphTest : public ::testing::Test {
 
     sparse_pose_graph_->AddScan(
         common::FromUniversal(0), transform::Rigid3d::Identity(), range_data,
-        pose_estimate, kTrajectoryId, std::move(insertion_submaps));
+        pose_estimate, kTrajectoryId, insertion_submaps);
   }
 
   void MoveRelative(const transform::Rigid2d& movement) {

cartographer/mapping_2d/submaps_test.cc

@@ -50,7 +50,7 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) {
   for (int i = 0; i != 1000; ++i) {
     submaps.InsertRangeData({Eigen::Vector3f::Zero(), {}, {}});
     // Except for the first, maps should only be returned after enough scans.
-    for (auto submap : submaps.submaps()) {
+    for (const auto& submap : submaps.submaps()) {
       all_submaps.insert(submap);
     }
     if (submaps.matching_index() != 0) {

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -44,7 +44,7 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
   sparse_pose_graph_->AddScan(
       insertion_result->time, insertion_result->range_data_in_tracking,
       insertion_result->pose_observation, trajectory_id_,
-      std::move(insertion_result->insertion_submaps));
+      insertion_result->insertion_submaps);
 }
 
 void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -190,7 +190,7 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
   // Querying the active submaps must be done here before calling
   // InsertRangeData() since the queried values are valid for next insertion.
   std::vector<std::shared_ptr<const Submap>> insertion_submaps;
-  for (std::shared_ptr<Submap> submap : active_submaps_.submaps()) {
+  for (const std::shared_ptr<Submap>& submap : active_submaps_.submaps()) {
     insertion_submaps.push_back(submap);
   }
   active_submaps_.InsertRangeData(

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -139,7 +139,7 @@ void SparsePoseGraph::AddScan(
   });
 }
 
-void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
+void SparsePoseGraph::AddWorkItem(const std::function<void()>& work_item) {
   if (scan_queue_ == nullptr) {
     work_item();
   } else {
@@ -427,6 +427,10 @@ void SparsePoseGraph::RunOptimization() {
   if (optimization_problem_.submap_data().empty()) {
     return;
   }
+
+  // No other thread is accessing the optimization_problem_, constraints_ and
+  // frozen_trajectories_ when executing the Solve. Solve is time consuming, so
+  // not taking the mutex before Solve to avoid blocking foreground processing.
   optimization_problem_.Solve(constraints_, frozen_trajectories_);
   common::MutexLocker locker(&mutex_);
 

cartographer/mapping_3d/sparse_pose_graph.h

@@ -112,7 +112,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   };
 
   // Handles a new work item.
-  void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
+  void AddWorkItem(const std::function<void()>& work_item) REQUIRES(mutex_);
 
   // Grows the optimization problem to have an entry for every element of
   // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -109,7 +109,7 @@ void ConstraintBuilder::NotifyEndOfScan() {
 }
 
 void ConstraintBuilder::WhenDone(
-    const std::function<void(const ConstraintBuilder::Result&)> callback) {
+    const std::function<void(const ConstraintBuilder::Result&)>& callback) {
   common::MutexLocker locker(&mutex_);
   CHECK(when_done_ == nullptr);
   when_done_ =
@@ -123,7 +123,7 @@ void ConstraintBuilder::WhenDone(
 void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
     const mapping::SubmapId& submap_id,
     const std::vector<mapping::TrajectoryNode>& submap_nodes,
-    const Submap* const submap, const std::function<void()> work_item) {
+    const Submap* const submap, const std::function<void()>& work_item) {
   if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher !=
       nullptr) {
     thread_pool_->Schedule(work_item);

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h

@@ -103,7 +103,7 @@ class ConstraintBuilder {
 
   // Registers the 'callback' to be called with the results, after all
   // computations triggered by MaybeAddConstraint() have finished.
-  void WhenDone(std::function<void(const Result&)> callback);
+  void WhenDone(const std::function<void(const Result&)>& callback);
 
   // Returns the number of consecutive finished scans.
   int GetNumFinishedScans();
@@ -121,7 +121,7 @@ class ConstraintBuilder {
   void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
       const mapping::SubmapId& submap_id,
       const std::vector<mapping::TrajectoryNode>& submap_nodes,
-      const Submap* submap, std::function<void()> work_item) REQUIRES(mutex_);
+      const Submap* submap, const std::function<void()>& work_item) REQUIRES(mutex_);
 
   // Constructs the scan matcher for a 'submap', then schedules its work items.
   void ConstructSubmapScanMatcher(

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -205,6 +205,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   // Add constraints based on IMU observations of angular velocities and
   // linear acceleration.
   trajectory_data_.resize(imu_data_.size());
+  CHECK_GE(trajectory_data_.size(), node_data_.size());
   for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
     const auto& node_data = node_data_[trajectory_id];

cartographer/sensor/collator.cc

@@ -22,7 +22,7 @@ namespace sensor {
 void Collator::AddTrajectory(
     const int trajectory_id,
     const std::unordered_set<string>& expected_sensor_ids,
-    const Callback callback) {
+    const Callback& callback) {
   for (const auto& sensor_id : expected_sensor_ids) {
     const auto queue_key = QueueKey{trajectory_id, sensor_id};
     queue_.AddQueue(queue_key,

cartographer/sensor/collator.h

@@ -42,7 +42,7 @@ class Collator {
   // for each collated sensor data.
   void AddTrajectory(int trajectory_id,
                      const std::unordered_set<string>& expected_sensor_ids,
-                     Callback callback);
+                     const Callback& callback);
 
   // Marks 'trajectory_id' as finished.
   void FinishTrajectory(int trajectory_id);

cartographer/sensor/range_data.h

@@ -54,7 +54,7 @@ proto::CompressedRangeData ToProto(
 CompressedRangeData FromProto(const proto::CompressedRangeData& proto);
 CompressedRangeData Compress(const RangeData& range_data);
 
-RangeData Decompress(const CompressedRangeData& compressed_range_Data);
+RangeData Decompress(const CompressedRangeData& compressed_range_data);
 
 }  // namespace sensor
 }  // namespace cartographer

cartographer/sensor/range_data_test.cc

@@ -26,7 +26,6 @@ namespace sensor {
 namespace {
 
 using ::testing::Contains;
-using ::testing::PrintToString;
 
 MATCHER(NearPointwise, std::string(negation ? "Doesn't" : "Does") + " match.") {
   return std::get<0>(arg).isApprox(std::get<1>(arg), 0.001f);

cartographer/transform/transform.cc

@@ -19,9 +19,9 @@
 namespace cartographer {
 namespace transform {
 
-Rigid2d ToRigid2(const proto::Rigid2d& pose) {
+Rigid2d ToRigid2(const proto::Rigid2d& transform) {
-  return Rigid2d({pose.translation().x(), pose.translation().y()},
+  return Rigid2d({transform.translation().x(), transform.translation().y()},
-                 pose.rotation());
+                 transform.rotation());
 }
 
 Eigen::Vector2d ToEigen(const proto::Vector2d& vector) {

cartographer/transform/transform_interpolation_buffer.cc

@@ -29,7 +29,7 @@ namespace transform {
 
 void TransformInterpolationBuffer::Push(const common::Time time,
                                         const transform::Rigid3d& transform) {
-  if (deque_.size() > 0) {
+  if (!deque_.empty()) {
     CHECK_GE(time, latest_time()) << "New transform is older than latest.";
   }
   deque_.push_back(TimestampedTransform{time, transform});