From 0156e6b8ce3132786fb49c5a19cf0a9b95455842 Mon Sep 17 00:00:00 2001 From: gaschler Date: Wed, 28 Feb 2018 16:32:40 +0100 Subject: [PATCH] Instrument metrics in local trajectory builders. (#946) RFC=[0014](https://github.com/googlecartographer/rfcs/blob/master/text/0014-monitoring.md) --- .../2d/local_trajectory_builder_2d.cc | 55 ++++++++++++++++++- .../internal/2d/local_trajectory_builder_2d.h | 5 ++ .../3d/local_trajectory_builder_3d.cc | 52 +++++++++++++++++- .../internal/3d/local_trajectory_builder_3d.h | 5 ++ cartographer/metrics/register.cc | 4 ++ 5 files changed, 119 insertions(+), 2 deletions(-) diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc index 1579158..54eb614 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc @@ -20,11 +20,19 @@ #include #include "cartographer/common/make_unique.h" +#include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/range_data.h" namespace cartographer { namespace mapping { +static auto* kLocalSlamLatencyMetric = metrics::Gauge::Null(); +static auto* kFastCorrelativeScanMatcherScoreMetric = + metrics::Histogram::Null(); +static auto* kCeresScanMatcherCostMetric = metrics::Histogram::Null(); +static auto* kScanMatcherResidualDistanceMetric = metrics::Histogram::Null(); +static auto* kScanMatcherResidualAngleMetric = metrics::Histogram::Null(); + LocalTrajectoryBuilder2D::LocalTrajectoryBuilder2D( const proto::LocalTrajectoryBuilderOptions2D& options) : options_(options), @@ -66,9 +74,10 @@ std::unique_ptr LocalTrajectoryBuilder2D::ScanMatch( return nullptr; } if (options_.use_online_correlative_scan_matching()) { - real_time_correlative_scan_matcher_.Match( + double score = real_time_correlative_scan_matcher_.Match( pose_prediction, filtered_gravity_aligned_point_cloud, matching_submap->probability_grid(), &initial_ceres_pose); + kFastCorrelativeScanMatcherScoreMetric->Observe(score); } auto pose_observation = common::make_unique(); @@ -77,6 +86,16 @@ std::unique_ptr LocalTrajectoryBuilder2D::ScanMatch( filtered_gravity_aligned_point_cloud, matching_submap->probability_grid(), pose_observation.get(), &summary); + if (pose_observation) { + kCeresScanMatcherCostMetric->Observe(summary.final_cost); + double residual_distance = + (pose_observation->translation() - pose_prediction.translation()) + .norm(); + kScanMatcherResidualDistanceMetric->Observe(residual_distance); + double residual_angle = std::abs(pose_observation->rotation().angle() - + pose_prediction.rotation().angle()); + kScanMatcherResidualAngleMetric->Observe(residual_angle); + } return pose_observation; } @@ -104,6 +123,10 @@ LocalTrajectoryBuilder2D::AddRangeData( return nullptr; } + if (num_accumulated_ == 0) { + accumulation_started_ = std::chrono::steady_clock::now(); + } + std::vector range_data_poses; range_data_poses.reserve(range_data.returns.size()); bool warned = false; @@ -198,6 +221,9 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData( std::unique_ptr insertion_result = InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data, pose_estimate, gravity_alignment.rotation()); + auto duration = std::chrono::steady_clock::now() - accumulation_started_; + kLocalSlamLatencyMetric->Set( + std::chrono::duration_cast(duration).count()); return common::make_unique( MatchingResult{time, pose_estimate, std::move(range_data_in_local), std::move(insertion_result)}); @@ -269,5 +295,32 @@ void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) { extrapolator_->AddPose(time, transform::Rigid3d::Identity()); } +void LocalTrajectoryBuilder2D::RegisterMetrics( + metrics::FamilyFactory* family_factory) { + auto* latency = family_factory->NewGaugeFamily( + "/mapping/internal/2d/local_trajectory_builder/latency", + "Duration from first incoming point cloud in accumulation to local slam " + "result"); + kLocalSlamLatencyMetric = latency->Add({}); + auto score_boundaries = metrics::Histogram::FixedWidth(0.05, 20); + auto* scores = family_factory->NewHistogramFamily( + "/mapping/internal/2d/local_trajectory_builder/scores", + "Local scan matcher scores", score_boundaries); + kFastCorrelativeScanMatcherScoreMetric = + scores->Add({{"scan_matcher", "fast_correlative"}}); + auto cost_boundaries = metrics::Histogram::ScaledPowersOf(2, 0.01, 100); + auto* costs = family_factory->NewHistogramFamily( + "/mapping/internal/2d/local_trajectory_builder/costs", + "Local scan matcher costs", cost_boundaries); + kCeresScanMatcherCostMetric = costs->Add({{"scan_matcher", "ceres"}}); + auto distance_boundaries = metrics::Histogram::ScaledPowersOf(2, 0.01, 10); + auto* residuals = family_factory->NewHistogramFamily( + "/mapping/internal/2d/local_trajectory_builder/residuals", + "Local scan matcher residuals", distance_boundaries); + kScanMatcherResidualDistanceMetric = + residuals->Add({{"component", "distance"}}); + kScanMatcherResidualAngleMetric = residuals->Add({{"component", "angle"}}); +} + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h index 509c99f..a6ced91 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h @@ -17,6 +17,7 @@ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_H_ +#include #include #include "cartographer/common/time.h" @@ -26,6 +27,7 @@ #include "cartographer/mapping/2d/submap_2d.h" #include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/mapping/pose_extrapolator.h" +#include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/range_data.h" @@ -69,6 +71,8 @@ class LocalTrajectoryBuilder2D { void AddImuData(const sensor::ImuData& imu_data); void AddOdometryData(const sensor::OdometryData& odometry_data); + static void RegisterMetrics(metrics::FamilyFactory* family_factory); + private: std::unique_ptr AddAccumulatedRangeData( common::Time time, const sensor::RangeData& gravity_aligned_range_data, @@ -104,6 +108,7 @@ class LocalTrajectoryBuilder2D { int num_accumulated_ = 0; sensor::RangeData accumulated_range_data_; + std::chrono::steady_clock::time_point accumulation_started_; }; } // namespace mapping diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index 6d1bc7d..c630d5d 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -30,6 +30,13 @@ namespace cartographer { namespace mapping { +static auto* kLocalSlamLatencyMetric = metrics::Gauge::Null(); +static auto* kRealTimeCorrelativeScanMatcherScoreMetric = + metrics::Histogram::Null(); +static auto* kCeresScanMatcherCostMetric = metrics::Histogram::Null(); +static auto* kScanMatcherResidualDistanceMetric = metrics::Histogram::Null(); +static auto* kScanMatcherResidualAngleMetric = metrics::Histogram::Null(); + LocalTrajectoryBuilder3D::LocalTrajectoryBuilder3D( const mapping::proto::LocalTrajectoryBuilderOptions3D& options) : options_(options), @@ -78,6 +85,10 @@ LocalTrajectoryBuilder3D::AddRangeData( return nullptr; } + if (num_accumulated_ == 0) { + accumulation_started_ = std::chrono::steady_clock::now(); + } + sensor::TimedPointCloud hits = sensor::VoxelFilter(0.5f * options_.voxel_filter_size()) .Filter(range_data.returns); @@ -168,9 +179,10 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData( if (options_.use_online_correlative_scan_matching()) { // We take a copy since we use 'initial_ceres_pose' as an output argument. const transform::Rigid3d initial_pose = initial_ceres_pose; - real_time_correlative_scan_matcher_->Match( + double score = real_time_correlative_scan_matcher_->Match( initial_pose, high_resolution_point_cloud_in_tracking, matching_submap->high_resolution_hybrid_grid(), &initial_ceres_pose); + kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score); } transform::Rigid3d pose_observation_in_submap; @@ -193,6 +205,14 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData( {&low_resolution_point_cloud_in_tracking, &matching_submap->low_resolution_hybrid_grid()}}, &pose_observation_in_submap, &summary); + kCeresScanMatcherCostMetric->Observe(summary.final_cost); + double residual_distance = (pose_observation_in_submap.translation() - + initial_ceres_pose.translation()) + .norm(); + kScanMatcherResidualDistanceMetric->Observe(residual_distance); + double residual_angle = pose_observation_in_submap.rotation().angularDistance( + initial_ceres_pose.rotation()); + kScanMatcherResidualAngleMetric->Observe(residual_angle); const transform::Rigid3d pose_estimate = matching_submap->local_pose() * pose_observation_in_submap; extrapolator_->AddPose(time, pose_estimate); @@ -205,6 +225,9 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData( time, filtered_range_data_in_local, filtered_range_data_in_tracking, high_resolution_point_cloud_in_tracking, low_resolution_point_cloud_in_tracking, pose_estimate, gravity_alignment); + auto duration = std::chrono::steady_clock::now() - accumulation_started_; + kLocalSlamLatencyMetric->Set( + std::chrono::duration_cast(duration).count()); return common::make_unique(MatchingResult{ time, pose_estimate, std::move(filtered_range_data_in_local), std::move(insertion_result)}); @@ -260,5 +283,32 @@ LocalTrajectoryBuilder3D::InsertIntoSubmap( std::move(insertion_submaps)}); } +void LocalTrajectoryBuilder3D::RegisterMetrics( + metrics::FamilyFactory* family_factory) { + auto* latency = family_factory->NewGaugeFamily( + "/mapping/internal/3d/local_trajectory_builder/latency", + "Duration from first incoming point cloud in accumulation to local slam " + "result"); + kLocalSlamLatencyMetric = latency->Add({}); + auto score_boundaries = metrics::Histogram::FixedWidth(0.05, 20); + auto* scores = family_factory->NewHistogramFamily( + "/mapping/internal/3d/local_trajectory_builder/scores", + "Local scan matcher scores", score_boundaries); + kRealTimeCorrelativeScanMatcherScoreMetric = + scores->Add({{"scan_matcher", "real_time_correlative"}}); + auto cost_boundaries = metrics::Histogram::ScaledPowersOf(2, 0.01, 100); + auto* costs = family_factory->NewHistogramFamily( + "/mapping/internal/3d/local_trajectory_builder/costs", + "Local scan matcher costs", cost_boundaries); + kCeresScanMatcherCostMetric = costs->Add({{"scan_matcher", "ceres"}}); + auto distance_boundaries = metrics::Histogram::ScaledPowersOf(2, 0.01, 10); + auto* residuals = family_factory->NewHistogramFamily( + "/mapping/internal/3d/local_trajectory_builder/residuals", + "Local scan matcher residuals", distance_boundaries); + kScanMatcherResidualDistanceMetric = + residuals->Add({{"component", "distance"}}); + kScanMatcherResidualAngleMetric = residuals->Add({{"component", "angle"}}); +} + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h index 06bfc5b..83127a5 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h @@ -17,6 +17,7 @@ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_TRAJECTORY_BUILDER_3D_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_TRAJECTORY_BUILDER_3D_H_ +#include #include #include "cartographer/common/time.h" @@ -26,6 +27,7 @@ #include "cartographer/mapping/3d/submap_3d.h" #include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/mapping/pose_extrapolator.h" +#include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/range_data.h" @@ -67,6 +69,8 @@ class LocalTrajectoryBuilder3D { common::Time time, const sensor::TimedRangeData& range_data); void AddOdometryData(const sensor::OdometryData& odometry_data); + static void RegisterMetrics(metrics::FamilyFactory* family_factory); + private: std::unique_ptr AddAccumulatedRangeData( common::Time time, @@ -92,6 +96,7 @@ class LocalTrajectoryBuilder3D { int num_accumulated_ = 0; sensor::RangeData accumulated_range_data_; + std::chrono::steady_clock::time_point accumulation_started_; }; } // namespace mapping diff --git a/cartographer/metrics/register.cc b/cartographer/metrics/register.cc index 9b7e1a3..c2e49a8 100644 --- a/cartographer/metrics/register.cc +++ b/cartographer/metrics/register.cc @@ -18,6 +18,8 @@ #include "cartographer/mapping/2d/pose_graph/constraint_builder_2d.h" #include "cartographer/mapping/3d/pose_graph/constraint_builder_3d.h" +#include "cartographer/mapping/internal/2d/local_trajectory_builder_2d.h" +#include "cartographer/mapping/internal/3d/local_trajectory_builder_3d.h" namespace cartographer { namespace metrics { @@ -25,6 +27,8 @@ namespace metrics { void RegisterAllMetrics(FamilyFactory* registry) { mapping::pose_graph::ConstraintBuilder2D::RegisterMetrics(registry); mapping::pose_graph::ConstraintBuilder3D::RegisterMetrics(registry); + mapping::LocalTrajectoryBuilder2D::RegisterMetrics(registry); + mapping::LocalTrajectoryBuilder3D::RegisterMetrics(registry); } } // namespace metrics