From 5aa968748e9f8ce85ec76f942e6cefd07bf83330 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Tue, 16 May 2017 14:24:49 +0200 Subject: [PATCH] Sparse pose graph no longer uses Submaps* for trajectory identification. (#290) Related to #256. PAIR=wohe --- cartographer/mapping/map_builder.cc | 18 ++------ cartographer/mapping/map_builder.h | 4 -- cartographer/mapping/sparse_pose_graph.h | 9 +--- .../mapping_2d/global_trajectory_builder.cc | 10 ++-- .../mapping_2d/global_trajectory_builder.h | 3 +- .../mapping_2d/local_trajectory_builder.cc | 5 +- .../mapping_2d/local_trajectory_builder.h | 2 - cartographer/mapping_2d/sparse_pose_graph.cc | 46 ++++++++----------- cartographer/mapping_2d/sparse_pose_graph.h | 13 ++---- .../mapping_2d/sparse_pose_graph_test.cc | 3 +- .../mapping_3d/global_trajectory_builder.cc | 11 +++-- .../mapping_3d/global_trajectory_builder.h | 2 + .../kalman_local_trajectory_builder.cc | 2 +- .../local_trajectory_builder_interface.h | 1 - .../optimizing_local_trajectory_builder.cc | 6 +-- cartographer/mapping_3d/sparse_pose_graph.cc | 46 ++++++++----------- cartographer/mapping_3d/sparse_pose_graph.h | 12 ++--- 17 files changed, 74 insertions(+), 119 deletions(-) diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 8d4b1ba..3eb7ebd 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -78,7 +78,7 @@ int MapBuilder::AddTrajectoryBuilder( &sensor_collator_, trajectory_id, expected_sensor_ids, common::make_unique( trajectory_options.trajectory_builder_3d_options(), - sparse_pose_graph_3d_.get()))); + trajectory_id, sparse_pose_graph_3d_.get()))); } else { CHECK(trajectory_options.has_trajectory_builder_2d_options()); trajectory_builders_.push_back( @@ -86,10 +86,8 @@ int MapBuilder::AddTrajectoryBuilder( &sensor_collator_, trajectory_id, expected_sensor_ids, common::make_unique( trajectory_options.trajectory_builder_2d_options(), - sparse_pose_graph_2d_.get()))); + trajectory_id, sparse_pose_graph_2d_.get()))); } - trajectory_ids_.emplace(trajectory_builders_.back()->submaps(), - trajectory_id); return trajectory_id; } @@ -106,12 +104,6 @@ int MapBuilder::GetBlockingTrajectoryId() const { return sensor_collator_.GetBlockingTrajectoryId(); } -int MapBuilder::GetTrajectoryId(const Submaps* const trajectory) const { - const auto trajectory_id = trajectory_ids_.find(trajectory); - CHECK(trajectory_id != trajectory_ids_.end()); - return trajectory_id->second; -} - proto::TrajectoryConnectivity MapBuilder::GetTrajectoryConnectivity() { return ToProto(sparse_pose_graph_->GetConnectedTrajectories()); } @@ -125,10 +117,8 @@ string MapBuilder::SubmapToProto(const int trajectory_id, " trajectories."; } - const Submaps* const submaps = - trajectory_builders_.at(trajectory_id)->submaps(); const std::vector submap_transforms = - sparse_pose_graph_->GetSubmapTransforms(submaps); + sparse_pose_graph_->GetSubmapTransforms(trajectory_id); if (submap_index < 0 || static_cast(submap_index) >= submap_transforms.size()) { return "Requested submap " + std::to_string(submap_index) + @@ -137,6 +127,8 @@ string MapBuilder::SubmapToProto(const int trajectory_id, " submaps in this trajectory."; } + const Submaps* const submaps = + trajectory_builders_.at(trajectory_id)->submaps(); response->set_submap_version(submaps->Get(submap_index)->num_range_data); submaps->SubmapToProto(submap_index, submap_transforms[submap_index], response); diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index 35e61bb..d6bce5e 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -71,9 +71,6 @@ class MapBuilder { // unblocked. int GetBlockingTrajectoryId() const; - // Returns the trajectory ID for 'trajectory'. - int GetTrajectoryId(const mapping::Submaps* trajectory) const; - // Returns the trajectory connectivity. proto::TrajectoryConnectivity GetTrajectoryConnectivity(); @@ -97,7 +94,6 @@ class MapBuilder { sensor::Collator sensor_collator_; std::vector> trajectory_builders_; - std::unordered_map trajectory_ids_; }; } // namespace mapping diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 66782b7..280172b 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -26,7 +26,6 @@ #include "cartographer/mapping/id.h" #include "cartographer/mapping/proto/sparse_pose_graph.pb.h" #include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h" -#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_node.h" #include "cartographer/transform/rigid_transform.h" @@ -71,11 +70,6 @@ class SparsePoseGraph { // Get the current trajectory clusters. virtual std::vector> GetConnectedTrajectories() = 0; - // Returns the current optimized transforms for the given 'trajectory'. - // TODO(hrapp): Remove this version. - virtual std::vector GetSubmapTransforms( - const Submaps* trajectory) = 0; - // Returns the current optimized transforms for the given 'trajectory_id'. virtual std::vector GetSubmapTransforms( int trajectory_id) = 0; @@ -83,8 +77,7 @@ class SparsePoseGraph { // Returns the transform converting data in the local map frame (i.e. the // continuous, non-loop-closed frame) into the global map frame (i.e. the // discontinuous, loop-closed frame). - virtual transform::Rigid3d GetLocalToGlobalTransform( - const Submaps* submaps) = 0; + virtual transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) = 0; // Returns the current optimized trajectories. virtual std::vector> GetTrajectoryNodes() = 0; diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index e9aa38a..64aa15a 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -21,8 +21,8 @@ namespace mapping_2d { GlobalTrajectoryBuilder::GlobalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options, - SparsePoseGraph* sparse_pose_graph) - : options_(options), + const int trajectory_id, SparsePoseGraph* sparse_pose_graph) + : trajectory_id_(trajectory_id), sparse_pose_graph_(sparse_pose_graph), local_trajectory_builder_(options) {} @@ -44,7 +44,7 @@ void GlobalTrajectoryBuilder::AddRangefinderData( insertion_result->range_data_in_tracking_2d, insertion_result->pose_estimate_2d, kalman_filter::Project2D(insertion_result->covariance_estimate), - insertion_result->submaps, insertion_result->matching_submap, + trajectory_id_, insertion_result->matching_submap, insertion_result->insertion_submaps); } } @@ -54,8 +54,8 @@ void GlobalTrajectoryBuilder::AddImuData( const Eigen::Vector3d& angular_velocity) { local_trajectory_builder_.AddImuData(time, linear_acceleration, angular_velocity); - sparse_pose_graph_->AddImuData(local_trajectory_builder_.submaps(), time, - linear_acceleration, angular_velocity); + sparse_pose_graph_->AddImuData(trajectory_id_, time, linear_acceleration, + angular_velocity); } void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time, diff --git a/cartographer/mapping_2d/global_trajectory_builder.h b/cartographer/mapping_2d/global_trajectory_builder.h index dcd83ad..6928d51 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.h +++ b/cartographer/mapping_2d/global_trajectory_builder.h @@ -28,6 +28,7 @@ class GlobalTrajectoryBuilder : public mapping::GlobalTrajectoryBuilderInterface { public: GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options, + int trajectory_id, SparsePoseGraph* sparse_pose_graph); ~GlobalTrajectoryBuilder() override; @@ -48,7 +49,7 @@ class GlobalTrajectoryBuilder const transform::Rigid3d& pose) override; private: - const proto::LocalTrajectoryBuilderOptions options_; + const int trajectory_id_; SparsePoseGraph* const sparse_pose_graph_; LocalTrajectoryBuilder local_trajectory_builder_; }; diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 0831f24..4054489 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -236,9 +236,8 @@ LocalTrajectoryBuilder::AddHorizontalRangeData( transform::Embed3D(pose_estimate_2d.cast()))); return common::make_unique(InsertionResult{ - time, &submaps_, matching_submap, insertion_submaps, - tracking_to_tracking_2d, tracking_2d_to_map, range_data_in_tracking_2d, - pose_estimate_2d, covariance_observation}); + time, matching_submap, insertion_submaps, tracking_to_tracking_2d, + range_data_in_tracking_2d, pose_estimate_2d, covariance_observation}); } const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index eba303c..cb29143 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -45,11 +45,9 @@ class LocalTrajectoryBuilder { public: struct InsertionResult { common::Time time; - const mapping::Submaps* submaps; const mapping::Submap* matching_submap; std::vector insertion_submaps; transform::Rigid3d tracking_to_tracking_2d; - transform::Rigid3d tracking_2d_to_map; sensor::RangeData range_data_in_tracking_2d; transform::Rigid2d pose_estimate_2d; kalman_filter::PoseCovariance covariance_estimate; diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 4f4549f..c633da8 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -90,16 +90,13 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded( void SparsePoseGraph::AddScan( common::Time time, const transform::Rigid3d& tracking_to_pose, const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose, - const kalman_filter::Pose2DCovariance& covariance, - const mapping::Submaps* const trajectory, + const kalman_filter::Pose2DCovariance& covariance, const int trajectory_id, const mapping::Submap* const matching_submap, const std::vector& insertion_submaps) { const transform::Rigid3d optimized_pose( - GetLocalToGlobalTransform(trajectory) * transform::Embed3D(pose)); + GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose)); common::MutexLocker locker(&mutex_); - trajectory_ids_.emplace(trajectory, trajectory_ids_.size()); - const int trajectory_id = trajectory_ids_.at(trajectory); const int flat_scan_index = trajectory_nodes_.size(); CHECK_LT(flat_scan_index, std::numeric_limits::max()); @@ -150,15 +147,13 @@ void SparsePoseGraph::AddWorkItem(std::function work_item) { } } -void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory, - common::Time time, +void SparsePoseGraph::AddImuData(const int trajectory_id, common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { common::MutexLocker locker(&mutex_); - trajectory_ids_.emplace(trajectory, trajectory_ids_.size()); AddWorkItem([=]() REQUIRES(mutex_) { - optimization_problem_.AddImuData(trajectory_ids_.at(trajectory), time, - linear_acceleration, angular_velocity); + optimization_problem_.AddImuData(trajectory_id, time, linear_acceleration, + angular_velocity); }); } @@ -173,7 +168,6 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, .at(node_id.trajectory_id) .at(node_id.node_index) .point_cloud_pose; - const int scan_trajectory_id = trajectory_nodes_[scan_index].constant_data->trajectory_id; @@ -423,9 +417,10 @@ void SparsePoseGraph::RunOptimization() { std::vector> SparsePoseGraph::GetTrajectoryNodes() { common::MutexLocker locker(&mutex_); - std::vector> result( - trajectory_ids_.size()); + std::vector> result; for (const auto& node : trajectory_nodes_) { + result.resize( + std::max(result.size(), node.constant_data->trajectory_id + 1)); result.at(node.constant_data->trajectory_id).push_back(node); } return result; @@ -437,11 +432,18 @@ std::vector SparsePoseGraph::constraints() { } transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( - const mapping::Submaps* const submaps) { - const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps); + const int trajectory_id) { + common::MutexLocker locker(&mutex_); + if (submap_states_.size() <= static_cast(trajectory_id) || + submap_states_.at(trajectory_id).empty()) { + return transform::Rigid3d::Identity(); + } + const auto extrapolated_submap_transforms = + ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id); return extrapolated_submap_transforms.back() * - submaps->Get(extrapolated_submap_transforms.size() - 1) - ->local_pose() + submap_states_.at(trajectory_id) + .at(extrapolated_submap_transforms.size() - 1) + .submap->local_pose() .inverse(); } @@ -450,16 +452,6 @@ std::vector> SparsePoseGraph::GetConnectedTrajectories() { return connected_components_; } -std::vector SparsePoseGraph::GetSubmapTransforms( - const mapping::Submaps* const trajectory) { - common::MutexLocker locker(&mutex_); - if (trajectory_ids_.count(trajectory) == 0) { - return {transform::Rigid3d::Identity()}; - } - return ExtrapolateSubmapTransforms(optimized_submap_transforms_, - trajectory_ids_.at(trajectory)); -} - std::vector SparsePoseGraph::GetSubmapTransforms( const int trajectory_id) { common::MutexLocker locker(&mutex_); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 444665c..68cd5b4 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -70,23 +70,20 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose, const kalman_filter::Pose2DCovariance& pose_covariance, - const mapping::Submaps* trajectory, - const mapping::Submap* matching_submap, + int trajectory_id, const mapping::Submap* matching_submap, const std::vector& insertion_submaps) EXCLUDES(mutex_); // Adds new IMU data to be used in the optimization. - void AddImuData(const mapping::Submaps* trajectory, common::Time time, + void AddImuData(int trajectory_id, common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() override; - std::vector GetSubmapTransforms( - const mapping::Submaps* trajectory) EXCLUDES(mutex_) override; std::vector GetSubmapTransforms(int trajectory_id) EXCLUDES(mutex_) override; - transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps) + transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) EXCLUDES(mutex_) override; std::vector> GetTrajectoryNodes() override EXCLUDES(mutex_); @@ -209,10 +206,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Current submap transforms used for displaying data. std::vector> optimized_submap_transforms_ GUARDED_BY(mutex_); - - // Map from submap pointers to trajectory IDs. - std::unordered_map trajectory_ids_ - GUARDED_BY(mutex_); }; } // namespace mapping_2d diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index ea47234..77bc740 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -158,11 +158,12 @@ class SparsePoseGraphTest : public ::testing::Test { const sensor::RangeData range_data{ Eigen::Vector3f::Zero(), new_point_cloud, {}}; const transform::Rigid2d pose_estimate = noise * current_pose_; + constexpr int kTrajectoryId = 0; submaps_->InsertRangeData(TransformRangeData( range_data, transform::Embed3D(pose_estimate.cast()))); sparse_pose_graph_->AddScan(common::FromUniversal(0), transform::Rigid3d::Identity(), range_data, - pose_estimate, covariance, submaps_.get(), + pose_estimate, covariance, kTrajectoryId, matching_submap, insertion_submaps); } diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index 1736d49..3fdfebc 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/cartographer/mapping_3d/global_trajectory_builder.cc @@ -23,8 +23,9 @@ namespace mapping_3d { GlobalTrajectoryBuilder::GlobalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options, - SparsePoseGraph* sparse_pose_graph) - : sparse_pose_graph_(sparse_pose_graph), + const int trajectory_id, SparsePoseGraph* sparse_pose_graph) + : trajectory_id_(trajectory_id), + sparse_pose_graph_(sparse_pose_graph), local_trajectory_builder_(CreateLocalTrajectoryBuilder(options)) {} GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {} @@ -38,8 +39,8 @@ void GlobalTrajectoryBuilder::AddImuData( const Eigen::Vector3d& angular_velocity) { local_trajectory_builder_->AddImuData(time, linear_acceleration, angular_velocity); - sparse_pose_graph_->AddImuData(local_trajectory_builder_->submaps(), time, - linear_acceleration, angular_velocity); + sparse_pose_graph_->AddImuData(trajectory_id_, time, linear_acceleration, + angular_velocity); } void GlobalTrajectoryBuilder::AddRangefinderData( @@ -55,7 +56,7 @@ void GlobalTrajectoryBuilder::AddRangefinderData( sparse_pose_graph_->AddScan( insertion_result->time, insertion_result->range_data_in_tracking, insertion_result->pose_observation, insertion_result->covariance_estimate, - insertion_result->submaps, insertion_result->matching_submap, + trajectory_id_, insertion_result->matching_submap, insertion_result->insertion_submaps); } diff --git a/cartographer/mapping_3d/global_trajectory_builder.h b/cartographer/mapping_3d/global_trajectory_builder.h index 28efb95..7be3fa8 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.h +++ b/cartographer/mapping_3d/global_trajectory_builder.h @@ -31,6 +31,7 @@ class GlobalTrajectoryBuilder : public mapping::GlobalTrajectoryBuilderInterface { public: GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options, + int trajectory_id, mapping_3d::SparsePoseGraph* sparse_pose_graph); ~GlobalTrajectoryBuilder() override; @@ -47,6 +48,7 @@ class GlobalTrajectoryBuilder const PoseEstimate& pose_estimate() const override; private: + const int trajectory_id_; mapping_3d::SparsePoseGraph* const sparse_pose_graph_; std::unique_ptr local_trajectory_builder_; }; diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index df6b7c9..030f6b6 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc @@ -230,7 +230,7 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap( trajectory_node_index); return std::unique_ptr(new InsertionResult{ time, range_data_in_tracking, pose_observation, covariance_estimate, - submaps_.get(), matching_submap, insertion_submaps}); + matching_submap, insertion_submaps}); } } // namespace mapping_3d diff --git a/cartographer/mapping_3d/local_trajectory_builder_interface.h b/cartographer/mapping_3d/local_trajectory_builder_interface.h index 85b9a2e..3291afa 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_interface.h +++ b/cartographer/mapping_3d/local_trajectory_builder_interface.h @@ -38,7 +38,6 @@ class LocalTrajectoryBuilderInterface { sensor::RangeData range_data_in_tracking; transform::Rigid3d pose_observation; kalman_filter::PoseCovariance covariance_estimate; - const Submaps* submaps; const Submap* matching_submap; std::vector insertion_submaps; }; diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc index b2b48e2..0fecffb 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc @@ -406,9 +406,9 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap( const kalman_filter::PoseCovariance kCovariance = 1e-7 * kalman_filter::PoseCovariance::Identity(); - return std::unique_ptr(new InsertionResult{ - time, range_data_in_tracking, pose_observation, kCovariance, - submaps_.get(), matching_submap, insertion_submaps}); + return std::unique_ptr( + new InsertionResult{time, range_data_in_tracking, pose_observation, + kCovariance, matching_submap, insertion_submaps}); } OptimizingLocalTrajectoryBuilder::State diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 135159e..2750eff 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -89,15 +89,13 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded( void SparsePoseGraph::AddScan( common::Time time, const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose, - const kalman_filter::PoseCovariance& covariance, - const Submaps* const trajectory, const Submap* const matching_submap, + const kalman_filter::PoseCovariance& covariance, const int trajectory_id, + const Submap* const matching_submap, const std::vector& insertion_submaps) { const transform::Rigid3d optimized_pose( - GetLocalToGlobalTransform(trajectory) * pose); + GetLocalToGlobalTransform(trajectory_id) * pose); common::MutexLocker locker(&mutex_); - trajectory_ids_.emplace(trajectory, trajectory_ids_.size()); - const int trajectory_id = trajectory_ids_.at(trajectory); const int flat_scan_index = trajectory_nodes_.size(); CHECK_LT(flat_scan_index, std::numeric_limits::max()); @@ -150,15 +148,13 @@ void SparsePoseGraph::AddWorkItem(std::function work_item) { } } -void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory, - common::Time time, +void SparsePoseGraph::AddImuData(const int trajectory_id, common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { common::MutexLocker locker(&mutex_); - trajectory_ids_.emplace(trajectory, trajectory_ids_.size()); AddWorkItem([=]() REQUIRES(mutex_) { - optimization_problem_.AddImuData(trajectory_ids_.at(trajectory), time, - linear_acceleration, angular_velocity); + optimization_problem_.AddImuData(trajectory_id, time, linear_acceleration, + angular_velocity); }); } @@ -413,9 +409,10 @@ void SparsePoseGraph::RunOptimization() { std::vector> SparsePoseGraph::GetTrajectoryNodes() { common::MutexLocker locker(&mutex_); - std::vector> result( - trajectory_ids_.size()); + std::vector> result; for (const auto& node : trajectory_nodes_) { + result.resize( + std::max(result.size(), node.constant_data->trajectory_id + 1)); result.at(node.constant_data->trajectory_id).push_back(node); } return result; @@ -427,11 +424,18 @@ std::vector SparsePoseGraph::constraints() { } transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( - const mapping::Submaps* const submaps) { - const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps); + const int trajectory_id) { + common::MutexLocker locker(&mutex_); + if (submap_states_.size() <= static_cast(trajectory_id) || + submap_states_.at(trajectory_id).empty()) { + return transform::Rigid3d::Identity(); + } + const auto extrapolated_submap_transforms = + ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id); return extrapolated_submap_transforms.back() * - submaps->Get(extrapolated_submap_transforms.size() - 1) - ->local_pose() + submap_states_.at(trajectory_id) + .at(extrapolated_submap_transforms.size() - 1) + .submap->local_pose() .inverse(); } @@ -440,16 +444,6 @@ std::vector> SparsePoseGraph::GetConnectedTrajectories() { return connected_components_; } -std::vector SparsePoseGraph::GetSubmapTransforms( - const mapping::Submaps* const trajectory) { - common::MutexLocker locker(&mutex_); - if (trajectory_ids_.count(trajectory) == 0) { - return {transform::Rigid3d::Identity()}; - } - return ExtrapolateSubmapTransforms(optimized_submap_transforms_, - trajectory_ids_.at(trajectory)); -} - std::vector SparsePoseGraph::GetSubmapTransforms( const int trajectory_id) { common::MutexLocker locker(&mutex_); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index a05f103..5f2dd13 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -69,7 +69,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& pose_covariance, - const Submaps* trajectory, const Submap* matching_submap, + int trajectory_id, const Submap* matching_submap, const std::vector& insertion_submaps) EXCLUDES(mutex_); @@ -78,17 +78,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { int GetNextTrajectoryNodeIndex() EXCLUDES(mutex_); // Adds new IMU data to be used in the optimization. - void AddImuData(const mapping::Submaps* trajectory, common::Time time, + void AddImuData(int trajectory_id, common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() override; - std::vector GetSubmapTransforms( - const mapping::Submaps* trajectory) EXCLUDES(mutex_) override; std::vector GetSubmapTransforms(int trajectory_id) EXCLUDES(mutex_) override; - transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps) + transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) EXCLUDES(mutex_) override; std::vector> GetTrajectoryNodes() override EXCLUDES(mutex_); @@ -209,10 +207,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Current submap transforms used for displaying data. std::vector> optimized_submap_transforms_ GUARDED_BY(mutex_); - - // Map from submap pointers to trajectory IDs. - std::unordered_map trajectory_ids_ - GUARDED_BY(mutex_); }; } // namespace mapping_3d