Added metrics RealTimeRatio and CpuRealTimeRatio to 2d. (#1324)

Added the metrics RealTimeRatio and CpuRealTimeRatio also to local_trajectory_builder_2d.

This is similar to[ PR 1275](https://github.com/googlecartographer/cartographer/pull/1275) and resolves a TODO.
master
Susanne Pielawa 2018-07-23 18:23:41 +02:00 committed by Wally B. Feed
parent fc78288ef8
commit 25f79cb6eb
3 changed files with 47 additions and 13 deletions

View File

@ -27,6 +27,8 @@ namespace cartographer {
namespace mapping {
static auto* kLocalSlamLatencyMetric = metrics::Gauge::Null();
static auto* kLocalSlamRealTimeRatio = metrics::Gauge::Null();
static auto* kLocalSlamCpuRealTimeRatio = metrics::Gauge::Null();
static auto* kRealTimeCorrelativeScanMatcherScoreMetric =
metrics::Histogram::Null();
static auto* kCeresScanMatcherCostMetric = metrics::Histogram::Null();
@ -184,6 +186,12 @@ LocalTrajectoryBuilder2D::AddRangeData(
++num_accumulated_;
if (num_accumulated_ >= options_.num_accumulated_range_data()) {
const common::Time current_sensor_time = synchronized_data.time;
common::optional<common::Duration> sensor_duration;
if (last_sensor_time_.has_value()) {
sensor_duration = current_sensor_time - last_sensor_time_.value();
}
last_sensor_time_ = current_sensor_time;
num_accumulated_ = 0;
const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
extrapolator_->EstimateGravityOrientation(time));
@ -195,7 +203,7 @@ LocalTrajectoryBuilder2D::AddRangeData(
TransformToGravityAlignedFrameAndFilter(
gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
accumulated_range_data_),
gravity_alignment);
gravity_alignment, sensor_duration);
}
return nullptr;
}
@ -204,7 +212,8 @@ std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
const common::Time time,
const sensor::RangeData& gravity_aligned_range_data,
const transform::Rigid3d& gravity_alignment) {
const transform::Rigid3d& gravity_alignment,
const common::optional<common::Duration>& sensor_duration) {
if (gravity_aligned_range_data.returns.empty()) {
LOG(WARNING) << "Dropped empty horizontal range data.";
return nullptr;
@ -240,13 +249,28 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
time, range_data_in_local, filtered_gravity_aligned_point_cloud,
pose_estimate, gravity_alignment.rotation());
const auto accumulation_stop = std::chrono::steady_clock::now();
if (last_accumulation_stop_.has_value()) {
const auto accumulation_duration =
accumulation_stop - last_accumulation_stop_.value();
kLocalSlamLatencyMetric->Set(common::ToSeconds(accumulation_duration));
const auto wall_time = std::chrono::steady_clock::now();
if (last_wall_time_.has_value()) {
const auto wall_time_duration = wall_time - last_wall_time_.value();
kLocalSlamLatencyMetric->Set(common::ToSeconds(wall_time_duration));
if (sensor_duration.has_value()) {
kLocalSlamRealTimeRatio->Set(common::ToSeconds(sensor_duration.value()) /
common::ToSeconds(wall_time_duration));
}
}
last_accumulation_stop_ = accumulation_stop;
const double thread_cpu_time_seconds = common::GetThreadCpuTimeSeconds();
if (last_thread_cpu_time_seconds_.has_value()) {
const double thread_cpu_duration_seconds =
thread_cpu_time_seconds - last_thread_cpu_time_seconds_.value();
if (sensor_duration.has_value()) {
kLocalSlamCpuRealTimeRatio->Set(
common::ToSeconds(sensor_duration.value()) /
thread_cpu_duration_seconds);
}
}
last_wall_time_ = wall_time;
last_thread_cpu_time_seconds_ = thread_cpu_time_seconds;
return common::make_unique<MatchingResult>(
MatchingResult{time, pose_estimate, std::move(range_data_in_local),
std::move(insertion_result)});
@ -313,6 +337,15 @@ void LocalTrajectoryBuilder2D::RegisterMetrics(
"Duration from first incoming point cloud in accumulation to local slam "
"result");
kLocalSlamLatencyMetric = latency->Add({});
auto* real_time_ratio = family_factory->NewGaugeFamily(
"mapping_2d_local_trajectory_builder_real_time_ratio",
"sensor duration / wall clock duration.");
kLocalSlamRealTimeRatio = real_time_ratio->Add({});
auto* cpu_real_time_ratio = family_factory->NewGaugeFamily(
"mapping_2d_local_trajectory_builder_cpu_real_time_ratio",
"sensor duration / cpu duration.");
kLocalSlamCpuRealTimeRatio = cpu_real_time_ratio->Add({});
auto score_boundaries = metrics::Histogram::FixedWidth(0.05, 20);
auto* scores = family_factory->NewHistogramFamily(
"mapping_2d_local_trajectory_builder_scores", "Local scan matcher scores",

View File

@ -79,7 +79,8 @@ class LocalTrajectoryBuilder2D {
private:
std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
common::Time time, const sensor::RangeData& gravity_aligned_range_data,
const transform::Rigid3d& gravity_alignment);
const transform::Rigid3d& gravity_alignment,
const common::optional<common::Duration>& sensor_duration);
sensor::RangeData TransformToGravityAlignedFrameAndFilter(
const transform::Rigid3f& transform_to_gravity_aligned_frame,
const sensor::RangeData& range_data) const;
@ -110,8 +111,10 @@ class LocalTrajectoryBuilder2D {
int num_accumulated_ = 0;
sensor::RangeData accumulated_range_data_;
common::optional<std::chrono::steady_clock::time_point>
last_accumulation_stop_;
common::optional<std::chrono::steady_clock::time_point> last_wall_time_;
common::optional<double> last_thread_cpu_time_seconds_;
common::optional<common::Time> last_sensor_time_;
RangeDataCollator range_data_collator_;
};

View File

@ -36,8 +36,6 @@ static auto* kLocalSlamLatencyMetric = metrics::Gauge::Null();
static auto* kLocalSlamVoxelFilterFraction = metrics::Gauge::Null();
static auto* kLocalSlamScanMatcherFraction = metrics::Gauge::Null();
static auto* kLocalSlamInsertIntoSubmapFraction = metrics::Gauge::Null();
// TODO(spielawa): Add the following two metrics also to
// local_trajectory_builder_2d
static auto* kLocalSlamRealTimeRatio = metrics::Gauge::Null();
static auto* kLocalSlamCpuRealTimeRatio = metrics::Gauge::Null();
static auto* kRealTimeCorrelativeScanMatcherScoreMetric =