12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Instrument metrics in local trajectory builders. (#946) 

RFC=[0014](https://github.com/googlecartographer/rfcs/blob/master/text/0014-monitoring.md)
master
gaschler 2018-02-28 16:32:40 +01:00 committed by Wally B. Feed
parent df1ee4bb29
commit 0156e6b8ce
5 changed files with 119 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
View File

@ -20,11 +20,19 @@
#include <memory>
#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<transform::Rigid2d> 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<transform::Rigid2d>();
@ -77,6 +86,16 @@ std::unique_ptr<transform::Rigid2d> 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<transform::Rigid3f> range_data_poses;
range_data_poses.reserve(range_data.returns.size());
bool warned = false;
@ -198,6 +221,9 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
std::unique_ptr<InsertionResult> 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<std::chrono::seconds>(duration).count());
return common::make_unique<MatchingResult>(
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

5
cartographer/mapping/internal/2d/local_trajectory_builder_2d.h
View File

@ -17,6 +17,7 @@
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_H_
#include <chrono>
#include <memory>
#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<MatchingResult> 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

52
cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc
View File

@ -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<std::chrono::seconds>(duration).count());
return common::make_unique<MatchingResult>(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

5
cartographer/mapping/internal/3d/local_trajectory_builder_3d.h
View File

@ -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 <chrono>
#include <memory>
#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<MatchingResult> 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

4
cartographer/metrics/register.cc
View File

@ -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