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
parent
a04b6cbc48
commit
120c216c47
|
@ -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_stop = std::chrono::steady_clock::now();
|
||||||
|
if (last_accumulation_stop_.has_value()) {
|
||||||
const auto accumulation_duration =
|
const auto accumulation_duration =
|
||||||
std::chrono::steady_clock::now() - accumulation_started_;
|
accumulation_stop - last_accumulation_stop_.value();
|
||||||
kLocalSlamLatencyMetric->Set(common::ToSeconds(accumulation_duration));
|
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)});
|
||||||
|
|
|
@ -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_;
|
||||||
};
|
};
|
||||||
|
|
|
@ -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_stop = std::chrono::steady_clock::now();
|
||||||
|
if (last_accumulation_stop_.has_value()) {
|
||||||
const auto accumulation_duration =
|
const auto accumulation_duration =
|
||||||
std::chrono::steady_clock::now() - accumulation_started_;
|
accumulation_stop - last_accumulation_stop_.value();
|
||||||
kLocalSlamLatencyMetric->Set(common::ToSeconds(accumulation_duration));
|
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)});
|
||||||
|
|
|
@ -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_;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue