Adding metrics (#1240)
to measure fraction of sensor time that is taken up by voxel filter, scan matcher, and inserting into submap.master
parent
aa3ac7e837
commit
3ee7c6710f
|
@ -31,6 +31,9 @@ namespace cartographer {
|
||||||
namespace mapping {
|
namespace mapping {
|
||||||
|
|
||||||
static auto* kLocalSlamLatencyMetric = metrics::Gauge::Null();
|
static auto* kLocalSlamLatencyMetric = metrics::Gauge::Null();
|
||||||
|
static auto* kLocalSlamVoxelFilterFraction = metrics::Gauge::Null();
|
||||||
|
static auto* kLocalSlamScanMatcherFraction = metrics::Gauge::Null();
|
||||||
|
static auto* kLocalSlamInsertIntoSubmapFraction = metrics::Gauge::Null();
|
||||||
static auto* kRealTimeCorrelativeScanMatcherScoreMetric =
|
static auto* kRealTimeCorrelativeScanMatcherScoreMetric =
|
||||||
metrics::Histogram::Null();
|
metrics::Histogram::Null();
|
||||||
static auto* kCeresScanMatcherCostMetric = metrics::Histogram::Null();
|
static auto* kCeresScanMatcherCostMetric = metrics::Histogram::Null();
|
||||||
|
@ -108,6 +111,7 @@ LocalTrajectoryBuilder3D::AddRangeData(
|
||||||
std::vector<transform::Rigid3f> hits_poses;
|
std::vector<transform::Rigid3f> hits_poses;
|
||||||
hits_poses.reserve(hits.size());
|
hits_poses.reserve(hits.size());
|
||||||
bool warned = false;
|
bool warned = false;
|
||||||
|
|
||||||
for (const auto& hit : hits) {
|
for (const auto& hit : hits) {
|
||||||
common::Time time_point = time + common::FromSeconds(hit.point_time[3]);
|
common::Time time_point = time + common::FromSeconds(hit.point_time[3]);
|
||||||
if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
|
if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
|
||||||
|
@ -150,18 +154,40 @@ LocalTrajectoryBuilder3D::AddRangeData(
|
||||||
++num_accumulated_;
|
++num_accumulated_;
|
||||||
|
|
||||||
if (num_accumulated_ >= options_.num_accumulated_range_data()) {
|
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;
|
num_accumulated_ = 0;
|
||||||
|
|
||||||
transform::Rigid3f current_pose =
|
transform::Rigid3f current_pose =
|
||||||
extrapolator_->ExtrapolatePose(time).cast<float>();
|
extrapolator_->ExtrapolatePose(time).cast<float>();
|
||||||
|
|
||||||
|
const auto voxel_filter_start = std::chrono::steady_clock::now();
|
||||||
const sensor::RangeData filtered_range_data = {
|
const sensor::RangeData filtered_range_data = {
|
||||||
current_pose.translation(),
|
current_pose.translation(),
|
||||||
sensor::VoxelFilter(options_.voxel_filter_size())
|
sensor::VoxelFilter(options_.voxel_filter_size())
|
||||||
.Filter(accumulated_range_data_.returns),
|
.Filter(accumulated_range_data_.returns),
|
||||||
sensor::VoxelFilter(options_.voxel_filter_size())
|
sensor::VoxelFilter(options_.voxel_filter_size())
|
||||||
.Filter(accumulated_range_data_.misses)};
|
.Filter(accumulated_range_data_.misses)};
|
||||||
|
const auto voxel_filter_stop = std::chrono::steady_clock::now();
|
||||||
|
const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start;
|
||||||
|
|
||||||
|
if (sensor_duration.has_value()) {
|
||||||
|
const double voxel_filter_fraction =
|
||||||
|
std::chrono::duration_cast<std::chrono::duration<double>>(
|
||||||
|
voxel_filter_duration)
|
||||||
|
.count() /
|
||||||
|
common::ToSeconds(sensor_duration.value());
|
||||||
|
kLocalSlamVoxelFilterFraction->Set(voxel_filter_fraction);
|
||||||
|
}
|
||||||
|
|
||||||
return AddAccumulatedRangeData(
|
return AddAccumulatedRangeData(
|
||||||
time, sensor::TransformRangeData(filtered_range_data,
|
time,
|
||||||
current_pose.inverse()));
|
sensor::TransformRangeData(filtered_range_data, current_pose.inverse()),
|
||||||
|
sensor_duration);
|
||||||
}
|
}
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
@ -169,7 +195,8 @@ LocalTrajectoryBuilder3D::AddRangeData(
|
||||||
std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
|
std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
|
||||||
LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
|
LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
|
||||||
const common::Time time,
|
const common::Time time,
|
||||||
const sensor::RangeData& filtered_range_data_in_tracking) {
|
const sensor::RangeData& filtered_range_data_in_tracking,
|
||||||
|
const common::optional<common::Duration>& sensor_duration) {
|
||||||
if (filtered_range_data_in_tracking.returns.empty()) {
|
if (filtered_range_data_in_tracking.returns.empty()) {
|
||||||
LOG(WARNING) << "Dropped empty range data.";
|
LOG(WARNING) << "Dropped empty range data.";
|
||||||
return nullptr;
|
return nullptr;
|
||||||
|
@ -178,6 +205,8 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
|
||||||
const transform::Rigid3d pose_prediction =
|
const transform::Rigid3d pose_prediction =
|
||||||
extrapolator_->ExtrapolatePose(time);
|
extrapolator_->ExtrapolatePose(time);
|
||||||
|
|
||||||
|
const auto scan_matcher_start = std::chrono::steady_clock::now();
|
||||||
|
|
||||||
std::shared_ptr<const mapping::Submap3D> matching_submap =
|
std::shared_ptr<const mapping::Submap3D> matching_submap =
|
||||||
active_submaps_.submaps().front();
|
active_submaps_.submaps().front();
|
||||||
transform::Rigid3d initial_ceres_pose =
|
transform::Rigid3d initial_ceres_pose =
|
||||||
|
@ -219,6 +248,18 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
|
||||||
{&low_resolution_point_cloud_in_tracking,
|
{&low_resolution_point_cloud_in_tracking,
|
||||||
&matching_submap->low_resolution_hybrid_grid()}},
|
&matching_submap->low_resolution_hybrid_grid()}},
|
||||||
&pose_observation_in_submap, &summary);
|
&pose_observation_in_submap, &summary);
|
||||||
|
|
||||||
|
const auto scan_matcher_stop = std::chrono::steady_clock::now();
|
||||||
|
const auto scan_matcher_duration = scan_matcher_stop - scan_matcher_start;
|
||||||
|
if (sensor_duration.has_value()) {
|
||||||
|
const double scan_matcher_fraction =
|
||||||
|
std::chrono::duration_cast<std::chrono::duration<double>>(
|
||||||
|
scan_matcher_duration)
|
||||||
|
.count() /
|
||||||
|
common::ToSeconds(sensor_duration.value());
|
||||||
|
kLocalSlamScanMatcherFraction->Set(scan_matcher_fraction);
|
||||||
|
}
|
||||||
|
|
||||||
kCeresScanMatcherCostMetric->Observe(summary.final_cost);
|
kCeresScanMatcherCostMetric->Observe(summary.final_cost);
|
||||||
double residual_distance = (pose_observation_in_submap.translation() -
|
double residual_distance = (pose_observation_in_submap.translation() -
|
||||||
initial_ceres_pose.translation())
|
initial_ceres_pose.translation())
|
||||||
|
@ -235,10 +276,26 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
|
||||||
|
|
||||||
sensor::RangeData filtered_range_data_in_local = sensor::TransformRangeData(
|
sensor::RangeData filtered_range_data_in_local = sensor::TransformRangeData(
|
||||||
filtered_range_data_in_tracking, pose_estimate.cast<float>());
|
filtered_range_data_in_tracking, pose_estimate.cast<float>());
|
||||||
|
|
||||||
|
const auto insert_into_submap_start = std::chrono::steady_clock::now();
|
||||||
std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
|
std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
|
||||||
time, filtered_range_data_in_local, filtered_range_data_in_tracking,
|
time, filtered_range_data_in_local, filtered_range_data_in_tracking,
|
||||||
high_resolution_point_cloud_in_tracking,
|
high_resolution_point_cloud_in_tracking,
|
||||||
low_resolution_point_cloud_in_tracking, pose_estimate, gravity_alignment);
|
low_resolution_point_cloud_in_tracking, pose_estimate, gravity_alignment);
|
||||||
|
|
||||||
|
const auto insert_into_submap_stop = std::chrono::steady_clock::now();
|
||||||
|
|
||||||
|
const auto insert_into_submap_duration =
|
||||||
|
insert_into_submap_stop - insert_into_submap_start;
|
||||||
|
if (sensor_duration.has_value()) {
|
||||||
|
double insert_into_submap_fraction =
|
||||||
|
std::chrono::duration_cast<std::chrono::duration<double>>(
|
||||||
|
insert_into_submap_duration)
|
||||||
|
.count() /
|
||||||
|
common::ToSeconds(sensor_duration.value());
|
||||||
|
kLocalSlamInsertIntoSubmapFraction->Set(insert_into_submap_fraction);
|
||||||
|
}
|
||||||
|
|
||||||
auto duration = std::chrono::steady_clock::now() - accumulation_started_;
|
auto duration = std::chrono::steady_clock::now() - accumulation_started_;
|
||||||
kLocalSlamLatencyMetric->Set(
|
kLocalSlamLatencyMetric->Set(
|
||||||
std::chrono::duration_cast<std::chrono::duration<double>>(duration)
|
std::chrono::duration_cast<std::chrono::duration<double>>(duration)
|
||||||
|
@ -304,6 +361,23 @@ void LocalTrajectoryBuilder3D::RegisterMetrics(
|
||||||
"mapping_3d_local_trajectory_builder_latency",
|
"mapping_3d_local_trajectory_builder_latency",
|
||||||
"Duration from first incoming point cloud in accumulation to local slam "
|
"Duration from first incoming point cloud in accumulation to local slam "
|
||||||
"result");
|
"result");
|
||||||
|
|
||||||
|
auto* voxel_filter_fraction = family_factory->NewGaugeFamily(
|
||||||
|
"mapping_3d_local_trajectory_builder_voxel_filter_fraction",
|
||||||
|
"Fraction of total sensor time taken up by voxel filter.");
|
||||||
|
kLocalSlamVoxelFilterFraction = voxel_filter_fraction->Add({});
|
||||||
|
|
||||||
|
auto* scan_matcher_fraction = family_factory->NewGaugeFamily(
|
||||||
|
"mapping_3d_local_trajectory_builder_scan_matcher_fraction",
|
||||||
|
"Fraction of total sensor time taken up by scan matcher.");
|
||||||
|
kLocalSlamScanMatcherFraction = scan_matcher_fraction->Add({});
|
||||||
|
|
||||||
|
auto* insert_into_submap_fraction = family_factory->NewGaugeFamily(
|
||||||
|
"mapping_3d_local_trajectory_builder"
|
||||||
|
"insert_into_submap_fraction",
|
||||||
|
"Fraction of total sensor time taken up by inserting into submap.");
|
||||||
|
kLocalSlamInsertIntoSubmapFraction = insert_into_submap_fraction->Add({});
|
||||||
|
|
||||||
kLocalSlamLatencyMetric = latency->Add({});
|
kLocalSlamLatencyMetric = latency->Add({});
|
||||||
auto score_boundaries = metrics::Histogram::FixedWidth(0.05, 20);
|
auto score_boundaries = metrics::Histogram::FixedWidth(0.05, 20);
|
||||||
auto* scores = family_factory->NewHistogramFamily(
|
auto* scores = family_factory->NewHistogramFamily(
|
||||||
|
|
|
@ -77,7 +77,8 @@ class LocalTrajectoryBuilder3D {
|
||||||
private:
|
private:
|
||||||
std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
|
std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
|
||||||
common::Time time,
|
common::Time time,
|
||||||
const sensor::RangeData& filtered_range_data_in_tracking);
|
const sensor::RangeData& filtered_range_data_in_tracking,
|
||||||
|
const common::optional<common::Duration>& sensor_duration);
|
||||||
|
|
||||||
std::unique_ptr<InsertionResult> InsertIntoSubmap(
|
std::unique_ptr<InsertionResult> InsertIntoSubmap(
|
||||||
common::Time time, const sensor::RangeData& filtered_range_data_in_local,
|
common::Time time, const sensor::RangeData& filtered_range_data_in_local,
|
||||||
|
@ -102,6 +103,8 @@ class LocalTrajectoryBuilder3D {
|
||||||
std::chrono::steady_clock::time_point accumulation_started_;
|
std::chrono::steady_clock::time_point accumulation_started_;
|
||||||
|
|
||||||
RangeDataCollator range_data_collator_;
|
RangeDataCollator range_data_collator_;
|
||||||
|
|
||||||
|
common::optional<common::Time> last_sensor_time_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace mapping
|
} // namespace mapping
|
||||||
|
|
Loading…
Reference in New Issue