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

Measure accumulation_duration from last accumulation stop (#1251) 

[PR 946](https://github.com/googlecartographer/cartographer/pull/946) introduced metrics in local trajectory builder. The accumulation duration was measured from accumulation_start to accumulation_stop. 

This PR changes this to measure the entire time elapsed between two accumulations (i.e. from accumulation_stop to the next accumulation_stop). 

Mostly the two measurements result in roughly the same, with the new way measuring slightly larger durations, as expected. But occasionally the measurements differ significantly. This is probably due to a lock contention somewhere outside of what was measured previously. Since we are interested in real time metrics, we need to track the entire time passed.
master
Susanne Pielawa 2018-07-10 16:24:57 +02:00 committed by Wally B. Feed
parent a04b6cbc48
commit 120c216c47
4 changed files with 18 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -134,10 +134,6 @@ LocalTrajectoryBuilder2D::AddRangeData(
return nullptr; return nullptr;
} }
if (num_accumulated_ == 0) {
accumulation_started_ = std::chrono::steady_clock::now();
}
std::vector<transform::Rigid3f> range_data_poses; std::vector<transform::Rigid3f> range_data_poses;
range_data_poses.reserve(synchronized_data.ranges.size()); range_data_poses.reserve(synchronized_data.ranges.size());
bool warned = false; bool warned = false;
@ -241,9 +237,13 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap( std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
time, range_data_in_local, filtered_gravity_aligned_point_cloud, time, range_data_in_local, filtered_gravity_aligned_point_cloud,
pose_estimate, gravity_alignment.rotation()); pose_estimate, gravity_alignment.rotation());
const auto accumulation_duration = const auto accumulation_stop = std::chrono::steady_clock::now();
std::chrono::steady_clock::now() - accumulation_started_; if (last_accumulation_stop_.has_value()) {
kLocalSlamLatencyMetric->Set(common::ToSeconds(accumulation_duration)); const auto accumulation_duration =
accumulation_stop - last_accumulation_stop_.value();
kLocalSlamLatencyMetric->Set(common::ToSeconds(accumulation_duration));
}
last_accumulation_stop_ = accumulation_stop;
return common::make_unique<MatchingResult>( return common::make_unique<MatchingResult>(
MatchingResult{time, pose_estimate, std::move(range_data_in_local), MatchingResult{time, pose_estimate, std::move(range_data_in_local),
std::move(insertion_result)}); std::move(insertion_result)});

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

@ -110,7 +110,8 @@ class LocalTrajectoryBuilder2D {
int num_accumulated_ = 0; int num_accumulated_ = 0;
sensor::RangeData accumulated_range_data_; sensor::RangeData accumulated_range_data_;
std::chrono::steady_clock::time_point accumulation_started_; common::optional<std::chrono::steady_clock::time_point>
last_accumulation_stop_;
RangeDataCollator range_data_collator_; RangeDataCollator range_data_collator_;
}; };

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

@ -140,10 +140,6 @@ LocalTrajectoryBuilder3D::AddRangeData(
return nullptr; return nullptr;
} }
if (num_accumulated_ == 0) {
accumulation_started_ = std::chrono::steady_clock::now();
}
std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> hits = std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> hits =
sensor::VoxelFilter(0.5f * options_.voxel_filter_size()) sensor::VoxelFilter(0.5f * options_.voxel_filter_size())
.Filter(synchronized_data.ranges); .Filter(synchronized_data.ranges);
@ -302,9 +298,13 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
common::ToSeconds(sensor_duration.value()); common::ToSeconds(sensor_duration.value());
kLocalSlamInsertIntoSubmapFraction->Set(insert_into_submap_fraction); kLocalSlamInsertIntoSubmapFraction->Set(insert_into_submap_fraction);
} }
const auto accumulation_duration = const auto accumulation_stop = std::chrono::steady_clock::now();
std::chrono::steady_clock::now() - accumulation_started_; if (last_accumulation_stop_.has_value()) {
kLocalSlamLatencyMetric->Set(common::ToSeconds(accumulation_duration)); const auto accumulation_duration =
accumulation_stop - last_accumulation_stop_.value();
kLocalSlamLatencyMetric->Set(common::ToSeconds(accumulation_duration));
}
last_accumulation_stop_ = accumulation_stop;
return common::make_unique<MatchingResult>(MatchingResult{ return common::make_unique<MatchingResult>(MatchingResult{
time, *pose_estimate, std::move(filtered_range_data_in_local), time, *pose_estimate, std::move(filtered_range_data_in_local),
std::move(insertion_result)}); std::move(insertion_result)});

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

@ -107,7 +107,8 @@ class LocalTrajectoryBuilder3D {
int num_accumulated_ = 0; int num_accumulated_ = 0;
sensor::RangeData accumulated_range_data_; sensor::RangeData accumulated_range_data_;
std::chrono::steady_clock::time_point accumulation_started_; common::optional<std::chrono::steady_clock::time_point>
last_accumulation_stop_;
RangeDataCollator range_data_collator_; RangeDataCollator range_data_collator_;