diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index 7b54b15..682b066 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -31,6 +31,9 @@ namespace cartographer { namespace mapping { 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 = metrics::Histogram::Null(); static auto* kCeresScanMatcherCostMetric = metrics::Histogram::Null(); @@ -108,6 +111,7 @@ LocalTrajectoryBuilder3D::AddRangeData( std::vector hits_poses; hits_poses.reserve(hits.size()); bool warned = false; + for (const auto& hit : hits) { common::Time time_point = time + common::FromSeconds(hit.point_time[3]); if (time_point < extrapolator_->GetLastExtrapolatedTime()) { @@ -150,18 +154,40 @@ LocalTrajectoryBuilder3D::AddRangeData( ++num_accumulated_; if (num_accumulated_ >= options_.num_accumulated_range_data()) { + const common::Time current_sensor_time = synchronized_data.time; + common::optional 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; + transform::Rigid3f current_pose = extrapolator_->ExtrapolatePose(time).cast(); + + const auto voxel_filter_start = std::chrono::steady_clock::now(); const sensor::RangeData filtered_range_data = { current_pose.translation(), sensor::VoxelFilter(options_.voxel_filter_size()) .Filter(accumulated_range_data_.returns), sensor::VoxelFilter(options_.voxel_filter_size()) .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>( + voxel_filter_duration) + .count() / + common::ToSeconds(sensor_duration.value()); + kLocalSlamVoxelFilterFraction->Set(voxel_filter_fraction); + } + return AddAccumulatedRangeData( - time, sensor::TransformRangeData(filtered_range_data, - current_pose.inverse())); + time, + sensor::TransformRangeData(filtered_range_data, current_pose.inverse()), + sensor_duration); } return nullptr; } @@ -169,7 +195,8 @@ LocalTrajectoryBuilder3D::AddRangeData( std::unique_ptr LocalTrajectoryBuilder3D::AddAccumulatedRangeData( const common::Time time, - const sensor::RangeData& filtered_range_data_in_tracking) { + const sensor::RangeData& filtered_range_data_in_tracking, + const common::optional& sensor_duration) { if (filtered_range_data_in_tracking.returns.empty()) { LOG(WARNING) << "Dropped empty range data."; return nullptr; @@ -178,6 +205,8 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData( const transform::Rigid3d pose_prediction = extrapolator_->ExtrapolatePose(time); + const auto scan_matcher_start = std::chrono::steady_clock::now(); + std::shared_ptr matching_submap = active_submaps_.submaps().front(); transform::Rigid3d initial_ceres_pose = @@ -219,6 +248,18 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData( {&low_resolution_point_cloud_in_tracking, &matching_submap->low_resolution_hybrid_grid()}}, &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>( + scan_matcher_duration) + .count() / + common::ToSeconds(sensor_duration.value()); + kLocalSlamScanMatcherFraction->Set(scan_matcher_fraction); + } + kCeresScanMatcherCostMetric->Observe(summary.final_cost); double residual_distance = (pose_observation_in_submap.translation() - initial_ceres_pose.translation()) @@ -235,10 +276,26 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData( sensor::RangeData filtered_range_data_in_local = sensor::TransformRangeData( filtered_range_data_in_tracking, pose_estimate.cast()); + + const auto insert_into_submap_start = std::chrono::steady_clock::now(); std::unique_ptr insertion_result = InsertIntoSubmap( 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); + + 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>( + 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_; kLocalSlamLatencyMetric->Set( std::chrono::duration_cast>(duration) @@ -304,6 +361,23 @@ void LocalTrajectoryBuilder3D::RegisterMetrics( "mapping_3d_local_trajectory_builder_latency", "Duration from first incoming point cloud in accumulation to local slam " "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({}); auto score_boundaries = metrics::Histogram::FixedWidth(0.05, 20); auto* scores = family_factory->NewHistogramFamily( diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h index 02fdeef..71dc5fb 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h @@ -77,7 +77,8 @@ class LocalTrajectoryBuilder3D { private: std::unique_ptr AddAccumulatedRangeData( common::Time time, - const sensor::RangeData& filtered_range_data_in_tracking); + const sensor::RangeData& filtered_range_data_in_tracking, + const common::optional& sensor_duration); std::unique_ptr InsertIntoSubmap( 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_; RangeDataCollator range_data_collator_; + + common::optional last_sensor_time_; }; } // namespace mapping