diff --git a/cartographer/common/thread_pool.cc b/cartographer/common/thread_pool.cc index 12e84d5..fdda166 100644 --- a/cartographer/common/thread_pool.cc +++ b/cartographer/common/thread_pool.cc @@ -45,7 +45,7 @@ ThreadPool::~ThreadPool() { } } -void ThreadPool::Schedule(std::function work_item) { +void ThreadPool::Schedule(const std::function& work_item) { MutexLocker locker(&mutex_); CHECK(running_); work_queue_.push_back(work_item); diff --git a/cartographer/common/thread_pool.h b/cartographer/common/thread_pool.h index b93da78..ff31321 100644 --- a/cartographer/common/thread_pool.h +++ b/cartographer/common/thread_pool.h @@ -40,7 +40,7 @@ class ThreadPool { ThreadPool(const ThreadPool&) = delete; ThreadPool& operator=(const ThreadPool&) = delete; - void Schedule(std::function work_item); + void Schedule(const std::function& work_item); private: void DoWork(); diff --git a/cartographer/io/hybrid_grid_points_processor.cc b/cartographer/io/hybrid_grid_points_processor.cc index 708f789..aba503f 100644 --- a/cartographer/io/hybrid_grid_points_processor.cc +++ b/cartographer/io/hybrid_grid_points_processor.cc @@ -28,7 +28,7 @@ HybridGridPointsProcessor::HybridGridPointsProcessor( std::unique_ptr HybridGridPointsProcessor::FromDictionary( - FileWriterFactory file_writer_factory, + const FileWriterFactory& file_writer_factory, common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { return common::make_unique( diff --git a/cartographer/io/hybrid_grid_points_processor.h b/cartographer/io/hybrid_grid_points_processor.h index bc25a8d..800bbb7 100644 --- a/cartographer/io/hybrid_grid_points_processor.h +++ b/cartographer/io/hybrid_grid_points_processor.h @@ -33,7 +33,7 @@ class HybridGridPointsProcessor : public PointsProcessor { delete; static std::unique_ptr FromDictionary( - FileWriterFactory file_writer_factory, + const FileWriterFactory& file_writer_factory, common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~HybridGridPointsProcessor() override {} diff --git a/cartographer/io/outlier_removing_points_processor.cc b/cartographer/io/outlier_removing_points_processor.cc index 4e21021..469b87f 100644 --- a/cartographer/io/outlier_removing_points_processor.cc +++ b/cartographer/io/outlier_removing_points_processor.cc @@ -41,18 +41,18 @@ OutlierRemovingPointsProcessor::OutlierRemovingPointsProcessor( } void OutlierRemovingPointsProcessor::Process( - std::unique_ptr points) { + std::unique_ptr 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; } } diff --git a/cartographer/io/ply_writing_points_processor.cc b/cartographer/io/ply_writing_points_processor.cc index d549fd6..cd63b05 100644 --- a/cartographer/io/ply_writing_points_processor.cc +++ b/cartographer/io/ply_writing_points_processor.cc @@ -71,7 +71,7 @@ void WriteBinaryPlyPointColor(const Uint8Color& color, std::unique_ptr PlyWritingPointsProcessor::FromDictionary( - FileWriterFactory file_writer_factory, + const FileWriterFactory& file_writer_factory, common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { return common::make_unique( diff --git a/cartographer/io/ply_writing_points_processor.h b/cartographer/io/ply_writing_points_processor.h index aa79359..2753b1c 100644 --- a/cartographer/io/ply_writing_points_processor.h +++ b/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 file, + PlyWritingPointsProcessor(std::unique_ptr file_writer, PointsProcessor* next); static std::unique_ptr FromDictionary( - FileWriterFactory file_writer_factory, + const FileWriterFactory& file_writer_factory, common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~PlyWritingPointsProcessor() override {} diff --git a/cartographer/io/points_processor_pipeline_builder.cc b/cartographer/io/points_processor_pipeline_builder.cc index 5f7f1d4..09697ae 100644 --- a/cartographer/io/points_processor_pipeline_builder.cc +++ b/cartographer/io/points_processor_pipeline_builder.cc @@ -48,7 +48,7 @@ void RegisterPlainPointsProcessor( template 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 void RegisterFileWritingPointsProcessorWithTrajectories( const std::vector& 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& trajectories, - FileWriterFactory file_writer_factory, + const FileWriterFactory& file_writer_factory, PointsProcessorPipelineBuilder* builder) { RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(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() {} diff --git a/cartographer/io/probability_grid_points_processor.cc b/cartographer/io/probability_grid_points_processor.cc index 9b81bff..230f153 100644 --- a/cartographer/io/probability_grid_points_processor.cc +++ b/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::FromDictionary( const std::vector& 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") || diff --git a/cartographer/io/probability_grid_points_processor.h b/cartographer/io/probability_grid_points_processor.h index 3d42f80..5e5dec8 100644 --- a/cartographer/io/probability_grid_points_processor.h +++ b/cartographer/io/probability_grid_points_processor.h @@ -39,7 +39,7 @@ class ProbabilityGridPointsProcessor : public PointsProcessor { static std::unique_ptr FromDictionary( const std::vector& trajectories, - FileWriterFactory file_writer_factory, + const FileWriterFactory& file_writer_factory, common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~ProbabilityGridPointsProcessor() override {} diff --git a/cartographer/io/xyz_writing_points_processor.cc b/cartographer/io/xyz_writing_points_processor.cc index 38d1522..f23f0e4 100644 --- a/cartographer/io/xyz_writing_points_processor.cc +++ b/cartographer/io/xyz_writing_points_processor.cc @@ -27,7 +27,7 @@ XyzWriterPointsProcessor::XyzWriterPointsProcessor( std::unique_ptr XyzWriterPointsProcessor::FromDictionary( - FileWriterFactory file_writer_factory, + const FileWriterFactory& file_writer_factory, common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { return common::make_unique( diff --git a/cartographer/io/xyz_writing_points_processor.h b/cartographer/io/xyz_writing_points_processor.h index 46ac55d..c0e0c64 100644 --- a/cartographer/io/xyz_writing_points_processor.h +++ b/cartographer/io/xyz_writing_points_processor.h @@ -36,7 +36,7 @@ class XyzWriterPointsProcessor : public PointsProcessor { XyzWriterPointsProcessor(std::unique_ptr, PointsProcessor* next); static std::unique_ptr FromDictionary( - FileWriterFactory file_writer_factory, + const FileWriterFactory& file_writer_factory, common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~XyzWriterPointsProcessor() override {} diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index a61b5a4..4ff0a91 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/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) { diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 1b0e5f5..d25209b 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/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> insertion_submaps; - for (std::shared_ptr submap : active_submaps_.submaps()) { + for (const std::shared_ptr& submap : active_submaps_.submaps()) { insertion_submaps.push_back(submap); } active_submaps_.InsertRangeData( diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index a984fd7..15e6251 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -58,7 +58,8 @@ class LocalTrajectoryBuilder { std::unique_ptr 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 AddAccumulatedRangeData( diff --git a/cartographer/mapping_2d/scan_matching/fast_global_localizer.cc b/cartographer/mapping_2d/scan_matching/fast_global_localizer.cc index 1399eeb..fa245c2 100644 --- a/cartographer/mapping_2d/scan_matching/fast_global_localizer.cc +++ b/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; } diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 567f03a..0396cf6 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -148,7 +148,7 @@ void SparsePoseGraph::AddScan( }); } -void SparsePoseGraph::AddWorkItem(std::function work_item) { +void SparsePoseGraph::AddWorkItem(const std::function& 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_); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index fedeed1..347a34b 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/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 work_item) REQUIRES(mutex_); + void AddWorkItem(const std::function& 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'. diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index 46b663c..0d54ee9 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -111,7 +111,7 @@ void ConstraintBuilder::NotifyEndOfScan() { } void ConstraintBuilder::WhenDone( - const std::function callback) { + const std::function& 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 work_item) { + const std::function& work_item) { if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher != nullptr) { thread_pool_->Schedule(work_item); diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h index 023a2fb..6f9e1c0 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h +++ b/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 callback); + void WhenDone(const std::function& 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 work_item) REQUIRES(mutex_); + const std::function& work_item) REQUIRES(mutex_); // Constructs the scan matcher for a 'submap', then schedules its work items. void ConstructSubmapScanMatcher(const mapping::SubmapId& submap_id, diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index aec849c..a718258 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/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())); std::vector> 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) { diff --git a/cartographer/mapping_2d/submaps_test.cc b/cartographer/mapping_2d/submaps_test.cc index 682ddf7..b8468a2 100644 --- a/cartographer/mapping_2d/submaps_test.cc +++ b/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) { diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index b987bb5..0c2d815 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/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, diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index 2653595..55de4de 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/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> insertion_submaps; - for (std::shared_ptr submap : active_submaps_.submaps()) { + for (const std::shared_ptr& submap : active_submaps_.submaps()) { insertion_submaps.push_back(submap); } active_submaps_.InsertRangeData( diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index beaff04..74e13db 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -139,7 +139,7 @@ void SparsePoseGraph::AddScan( }); } -void SparsePoseGraph::AddWorkItem(std::function work_item) { +void SparsePoseGraph::AddWorkItem(const std::function& 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_); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index d27b688..1dbadfc 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/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 work_item) REQUIRES(mutex_); + void AddWorkItem(const std::function& 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'. diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index 2ab1aa1..5fe33f3 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -109,7 +109,7 @@ void ConstraintBuilder::NotifyEndOfScan() { } void ConstraintBuilder::WhenDone( - const std::function callback) { + const std::function& 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& submap_nodes, - const Submap* const submap, const std::function work_item) { + const Submap* const submap, const std::function& work_item) { if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher != nullptr) { thread_pool_->Schedule(work_item); diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h index 8689397..0f7e999 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h +++ b/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 callback); + void WhenDone(const std::function& 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& submap_nodes, - const Submap* submap, std::function work_item) REQUIRES(mutex_); + const Submap* submap, const std::function& work_item) REQUIRES(mutex_); // Constructs the scan matcher for a 'submap', then schedules its work items. void ConstructSubmapScanMatcher( diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index 28fe16d..ffeac7b 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -205,6 +205,7 @@ void OptimizationProblem::Solve(const std::vector& 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]; diff --git a/cartographer/sensor/collator.cc b/cartographer/sensor/collator.cc index 0d62a65..d6f7820 100644 --- a/cartographer/sensor/collator.cc +++ b/cartographer/sensor/collator.cc @@ -22,7 +22,7 @@ namespace sensor { void Collator::AddTrajectory( const int trajectory_id, const std::unordered_set& 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, diff --git a/cartographer/sensor/collator.h b/cartographer/sensor/collator.h index b159a15..447daaf 100644 --- a/cartographer/sensor/collator.h +++ b/cartographer/sensor/collator.h @@ -42,7 +42,7 @@ class Collator { // for each collated sensor data. void AddTrajectory(int trajectory_id, const std::unordered_set& expected_sensor_ids, - Callback callback); + const Callback& callback); // Marks 'trajectory_id' as finished. void FinishTrajectory(int trajectory_id); diff --git a/cartographer/sensor/range_data.h b/cartographer/sensor/range_data.h index 85e769f..4f069da 100644 --- a/cartographer/sensor/range_data.h +++ b/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 diff --git a/cartographer/sensor/range_data_test.cc b/cartographer/sensor/range_data_test.cc index 24c52e3..72d57bf 100644 --- a/cartographer/sensor/range_data_test.cc +++ b/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); diff --git a/cartographer/transform/transform.cc b/cartographer/transform/transform.cc index 7b7f7b7..722f460 100644 --- a/cartographer/transform/transform.cc +++ b/cartographer/transform/transform.cc @@ -19,9 +19,9 @@ namespace cartographer { namespace transform { -Rigid2d ToRigid2(const proto::Rigid2d& pose) { - return Rigid2d({pose.translation().x(), pose.translation().y()}, - pose.rotation()); +Rigid2d ToRigid2(const proto::Rigid2d& transform) { + return Rigid2d({transform.translation().x(), transform.translation().y()}, + transform.rotation()); } Eigen::Vector2d ToEigen(const proto::Vector2d& vector) { diff --git a/cartographer/transform/transform_interpolation_buffer.cc b/cartographer/transform/transform_interpolation_buffer.cc index c8e9244..4c1e8aa 100644 --- a/cartographer/transform/transform_interpolation_buffer.cc +++ b/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});