From eb4415d17d9d6395b8586bf85d7f4683928c5840 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Fri, 10 Nov 2017 14:49:41 +0100 Subject: [PATCH] Introduce MatchingResult for LocalTrajectoryBuilder::AddRangeData. (#619) In preparation for #574. Depends on ~~#618~~ (merged) and ~~#617~~ (merged). --- .../mapping/global_trajectory_builder.h | 17 +++++++--- .../mapping_2d/local_trajectory_builder.cc | 12 ++++--- .../mapping_2d/local_trajectory_builder.h | 15 +++++++-- .../mapping_3d/local_trajectory_builder.cc | 31 ++++++++++--------- .../mapping_3d/local_trajectory_builder.h | 13 ++++++-- .../local_trajectory_builder_test.cc | 13 ++++---- 6 files changed, 67 insertions(+), 34 deletions(-) diff --git a/cartographer/mapping/global_trajectory_builder.h b/cartographer/mapping/global_trajectory_builder.h index 9da6c07..b585464 100644 --- a/cartographer/mapping/global_trajectory_builder.h +++ b/cartographer/mapping/global_trajectory_builder.h @@ -47,14 +47,21 @@ class GlobalTrajectoryBuilder void AddRangefinderData(const common::Time time, const Eigen::Vector3f& origin, const sensor::TimedPointCloud& ranges) override { - std::unique_ptr - insertion_result = local_trajectory_builder_.AddRangeData( + std::unique_ptr + matching_result = local_trajectory_builder_.AddRangeData( time, sensor::TimedRangeData{origin, ranges, {}}); - if (insertion_result == nullptr) { + if (matching_result == nullptr) { + // The range data has not been fully accumulated yet. return; } - sparse_pose_graph_->AddScan(insertion_result->constant_data, trajectory_id_, - insertion_result->insertion_submaps); + std::unique_ptr node_id; + if (matching_result->insertion_result != nullptr) { + node_id = ::cartographer::common::make_unique( + sparse_pose_graph_->AddScan( + matching_result->insertion_result->constant_data, trajectory_id_, + matching_result->insertion_result->insertion_submaps)); + CHECK_EQ(node_id->trajectory_id, trajectory_id_); + } } void AddSensorData(const sensor::ImuData& imu_data) override { diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index afa9e22..ba3c429 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -73,7 +73,7 @@ void LocalTrajectoryBuilder::ScanMatch( matching_submap->probability_grid(), pose_observation, &summary); } -std::unique_ptr +std::unique_ptr LocalTrajectoryBuilder::AddRangeData(const common::Time time, const sensor::TimedRangeData& range_data) { // Initialize extrapolator now if we do not ever use an IMU. @@ -126,7 +126,7 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time, return nullptr; } -std::unique_ptr +std::unique_ptr LocalTrajectoryBuilder::AddAccumulatedRangeData( const common::Time time, const sensor::RangeData& range_data) { // Transforms 'range_data' from the tracking frame into a frame where gravity @@ -158,8 +158,12 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( TransformRangeData(gravity_aligned_range_data, transform::Embed3D(pose_estimate_2d.cast())); last_pose_estimate_ = {time, pose_estimate, range_data_in_local.returns}; - return InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data, - pose_estimate, gravity_alignment.rotation()); + std::unique_ptr insertion_result = + InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data, + pose_estimate, gravity_alignment.rotation()); + return common::make_unique( + MatchingResult{time, pose_estimate, std::move(range_data_in_local), + std::move(insertion_result)}); } std::unique_ptr diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index fcf7567..a2210b0 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -45,6 +45,13 @@ class LocalTrajectoryBuilder { std::shared_ptr constant_data; std::vector> insertion_submaps; }; + struct MatchingResult { + common::Time time; + transform::Rigid3d local_pose; + sensor::RangeData range_data_in_local; + // 'nullptr' if dropped by the motion filter. + std::unique_ptr insertion_result; + }; explicit LocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options); @@ -55,14 +62,16 @@ class LocalTrajectoryBuilder { const mapping::PoseEstimate& pose_estimate() const; - // Range data must be approximately horizontal for 2D SLAM. - std::unique_ptr AddRangeData( + // Returns 'MatchingResult' when range data accumulation completed, + // otherwise 'nullptr'. Range data must be approximately horizontal + // for 2D SLAM. + std::unique_ptr AddRangeData( common::Time, const sensor::TimedRangeData& range_data); void AddImuData(const sensor::ImuData& imu_data); void AddOdometerData(const sensor::OdometryData& odometry_data); private: - std::unique_ptr AddAccumulatedRangeData( + std::unique_ptr AddAccumulatedRangeData( common::Time time, const sensor::RangeData& range_data); sensor::RangeData TransformAndFilterRangeData( const transform::Rigid3f& gravity_alignment, diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index edc5594..b7196ab 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -58,7 +58,7 @@ void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) { options_.imu_gravity_time_constant(), imu_data); } -std::unique_ptr +std::unique_ptr LocalTrajectoryBuilder::AddRangeData(const common::Time time, const sensor::TimedRangeData& range_data) { if (extrapolator_ == nullptr) { @@ -106,7 +106,7 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time, return nullptr; } -std::unique_ptr +std::unique_ptr LocalTrajectoryBuilder::AddAccumulatedRangeData( const common::Time time, const sensor::RangeData& range_data_in_tracking) { const sensor::RangeData filtered_range_data_in_tracking = { @@ -166,10 +166,13 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( filtered_range_data_in_tracking, pose_estimate.cast()); last_pose_estimate_ = {time, pose_estimate, filtered_range_data_in_local.returns}; - return InsertIntoSubmap( + 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); + return common::make_unique(MatchingResult{ + time, pose_estimate, std::move(filtered_range_data_in_local), + std::move(insertion_result)}); } void LocalTrajectoryBuilder::AddOdometerData( @@ -212,17 +215,17 @@ LocalTrajectoryBuilder::InsertIntoSubmap( filtered_range_data_in_tracking.returns, transform::Rigid3f::Rotation(gravity_alignment.cast())), options_.rotational_histogram_size()); - return std::unique_ptr(new InsertionResult{ - std::make_shared( - mapping::TrajectoryNode::Data{ - time, - gravity_alignment, - {}, // 'filtered_point_cloud' is only used in 2D. - high_resolution_point_cloud_in_tracking, - low_resolution_point_cloud_in_tracking, - rotational_scan_matcher_histogram, - pose_estimate}), - std::move(insertion_submaps)}); + return common::make_unique( + InsertionResult{std::make_shared( + mapping::TrajectoryNode::Data{ + time, + gravity_alignment, + {}, // 'filtered_point_cloud' is only used in 2D. + high_resolution_point_cloud_in_tracking, + low_resolution_point_cloud_in_tracking, + rotational_scan_matcher_histogram, + pose_estimate}), + std::move(insertion_submaps)}); } } // namespace mapping_3d diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index e6ab106..aabe851 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/cartographer/mapping_3d/local_trajectory_builder.h @@ -44,6 +44,13 @@ class LocalTrajectoryBuilder { std::shared_ptr constant_data; std::vector> insertion_submaps; }; + struct MatchingResult { + common::Time time; + transform::Rigid3d local_pose; + sensor::RangeData range_data_in_local; + // 'nullptr' if dropped by the motion filter. + std::unique_ptr insertion_result; + }; explicit LocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options); @@ -53,13 +60,15 @@ class LocalTrajectoryBuilder { LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete; void AddImuData(const sensor::ImuData& imu_data); - std::unique_ptr AddRangeData( + // Returns 'MatchingResult' when range data accumulation completed, + // otherwise 'nullptr'. + std::unique_ptr AddRangeData( common::Time time, const sensor::TimedRangeData& range_data); void AddOdometerData(const sensor::OdometryData& odometry_data); const mapping::PoseEstimate& pose_estimate() const; private: - std::unique_ptr AddAccumulatedRangeData( + std::unique_ptr AddAccumulatedRangeData( common::Time time, const sensor::RangeData& range_data_in_tracking); std::unique_ptr InsertIntoSubmap( diff --git a/cartographer/mapping_3d/local_trajectory_builder_test.cc b/cartographer/mapping_3d/local_trajectory_builder_test.cc index e5dbc81..04d7aab 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_test.cc +++ b/cartographer/mapping_3d/local_trajectory_builder_test.cc @@ -251,12 +251,13 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { for (const TrajectoryNode& node : expected_trajectory) { AddLinearOnlyImuObservation(node.time, node.pose); const auto range_data = GenerateRangeData(node.pose); - if (local_trajectory_builder_->AddRangeData( - node.time, - sensor::TimedRangeData{ - range_data.origin, range_data.returns, {}}) != nullptr) { - const auto pose_estimate = local_trajectory_builder_->pose_estimate(); - EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1)); + const std::unique_ptr + matching_result = local_trajectory_builder_->AddRangeData( + node.time, sensor::TimedRangeData{ + range_data.origin, range_data.returns, {}}); + if (matching_result != nullptr) { + EXPECT_THAT(matching_result->local_pose, + transform::IsNearly(node.pose, 1e-1)); ++num_poses; LOG(INFO) << "num_poses: " << num_poses; }