diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h index d550694..9c7f8db 100644 --- a/cartographer/mapping/id.h +++ b/cartographer/mapping/id.h @@ -17,8 +17,10 @@ #ifndef CARTOGRAPHER_MAPPING_ID_H_ #define CARTOGRAPHER_MAPPING_ID_H_ +#include #include #include +#include namespace cartographer { namespace mapping { @@ -55,6 +57,40 @@ inline std::ostream& operator<<(std::ostream& os, const SubmapId& v) { return os << "(" << v.trajectory_id << ", " << v.submap_index << ")"; } +template +class NestedVectorsById { + public: + // Appends data to a trajectory, creating trajectories as needed. + IdType Append(int trajectory_id, const ValueType& value) { + data_.resize(std::max(data_.size(), trajectory_id + 1)); + const IdType id{trajectory_id, + static_cast(data_[trajectory_id].size())}; + data_[trajectory_id].push_back(value); + return id; + } + + const ValueType& at(const IdType& id) const { + return data_.at(id.trajectory_id).at(GetIndex(id)); + } + ValueType& at(const IdType& id) { + return data_.at(id.trajectory_id).at(GetIndex(id)); + } + + int num_trajectories() const { return static_cast(data_.size()); } + int num_indices(int trajectory_id) const { + return static_cast(data_.at(trajectory_id).size()); + } + + // TODO(whess): Remove once no longer needed. + const std::vector> data() const { return data_; } + + private: + static int GetIndex(const NodeId& id) { return id.node_index; } + static int GetIndex(const SubmapId& id) { return id.submap_index; } + + std::vector> data_; +}; + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 1e237c2..56b3664 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -101,24 +101,18 @@ void SparsePoseGraph::AddScan( time, range_data_in_pose, Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), trajectory_id, tracking_to_pose}); - trajectory_nodes_.resize( - std::max(trajectory_nodes_.size(), trajectory_id + 1)); - trajectory_nodes_[trajectory_id].push_back(mapping::TrajectoryNode{ - &constant_node_data_.back(), optimized_pose, - }); + trajectory_nodes_.Append(trajectory_id, + mapping::TrajectoryNode{ + &constant_node_data_.back(), optimized_pose, + }); ++num_trajectory_nodes_; trajectory_connectivity_.Add(trajectory_id); if (submap_ids_.count(insertion_submaps.back()) == 0) { - submap_states_.resize( - std::max(submap_states_.size(), trajectory_id + 1)); - auto& trajectory_submap_states = submap_states_.at(trajectory_id); - submap_ids_.emplace( - insertion_submaps.back(), - mapping::SubmapId{trajectory_id, - static_cast(trajectory_submap_states.size())}); - trajectory_submap_states.emplace_back(); - trajectory_submap_states.back().submap = insertion_submaps.back(); + const mapping::SubmapId submap_id = + submap_states_.Append(trajectory_id, SubmapState()); + submap_ids_.emplace(insertion_submaps.back(), submap_id); + submap_states_.at(submap_id).submap = insertion_submaps.back(); } const mapping::Submap* const finished_submap = insertion_submaps.front()->finished_probability_grid != nullptr @@ -162,14 +156,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, if (node_id.trajectory_id != submap_id.trajectory_id && global_localization_samplers_[node_id.trajectory_id]->Pulse()) { constraint_builder_.MaybeAddGlobalConstraint( - submap_id, - submap_states_.at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .submap, - node_id, - &trajectory_nodes_.at(node_id.trajectory_id) - .at(node_id.node_index) - .constant_data->range_data_2d.returns, + submap_id, submap_states_.at(submap_id).submap, node_id, + &trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns, &trajectory_connectivity_); } else { const bool scan_and_submap_trajectories_connected = @@ -190,14 +178,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, .point_cloud_pose; constraint_builder_.MaybeAddConstraint( - submap_id, - submap_states_.at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .submap, - node_id, - &trajectory_nodes_.at(node_id.trajectory_id) - .at(node_id.node_index) - .constant_data->range_data_2d.returns, + submap_id, submap_states_.at(submap_id).submap, node_id, + &trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns, initial_relative_pose); } } @@ -206,8 +188,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, void SparsePoseGraph::ComputeConstraintsForOldScans( const mapping::Submap* submap) { const auto submap_id = GetSubmapId(submap); - const auto& submap_state = - submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index); + const auto& submap_state = submap_states_.at(submap_id); const auto& node_data = optimization_problem_.node_data(); for (size_t trajectory_id = 0; trajectory_id != node_data.size(); @@ -245,20 +226,14 @@ void SparsePoseGraph::ComputeConstraintsForScan( .size()) : 0}; const mapping::TrajectoryNode::ConstantData* const scan_data = - trajectory_nodes_.at(node_id.trajectory_id) - .at(node_id.node_index) - .constant_data; + trajectory_nodes_.at(node_id).constant_data; CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id); optimization_problem_.AddTrajectoryNode( matching_id.trajectory_id, scan_data->time, pose, optimized_pose); for (const mapping::Submap* submap : insertion_submaps) { const mapping::SubmapId submap_id = GetSubmapId(submap); - CHECK(!submap_states_.at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .finished); - submap_states_.at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .node_ids.emplace(node_id); + CHECK(!submap_states_.at(submap_id).finished); + submap_states_.at(submap_id).node_ids.emplace(node_id); // Unchanged covariance as (submap <- map) is a translation. const transform::Rigid2d constraint_transform = sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose; @@ -276,28 +251,22 @@ void SparsePoseGraph::ComputeConstraintsForScan( Constraint::INTRA_SUBMAP}); } - for (size_t trajectory_id = 0; trajectory_id < submap_states_.size(); + for (int trajectory_id = 0; trajectory_id < submap_states_.num_trajectories(); ++trajectory_id) { - for (size_t submap_index = 0; - submap_index < submap_states_.at(trajectory_id).size(); + for (int submap_index = 0; + submap_index < submap_states_.num_indices(trajectory_id); ++submap_index) { - if (submap_states_.at(trajectory_id).at(submap_index).finished) { - CHECK_EQ(submap_states_.at(trajectory_id) - .at(submap_index) - .node_ids.count(node_id), - 0); - ComputeConstraint(node_id, - mapping::SubmapId{static_cast(trajectory_id), - static_cast(submap_index)}); + const mapping::SubmapId submap_id{trajectory_id, submap_index}; + if (submap_states_.at(submap_id).finished) { + CHECK_EQ(submap_states_.at(submap_id).node_ids.count(node_id), 0); + ComputeConstraint(node_id, submap_id); } } } if (finished_submap != nullptr) { const mapping::SubmapId finished_submap_id = GetSubmapId(finished_submap); - SubmapState& finished_submap_state = - submap_states_.at(finished_submap_id.trajectory_id) - .at(finished_submap_id.submap_index); + SubmapState& finished_submap_state = submap_states_.at(finished_submap_id); CHECK(!finished_submap_state.finished); // We have a new completed submap, so we look into adding constraints for // old scans. @@ -391,12 +360,14 @@ void SparsePoseGraph::RunOptimization() { common::MutexLocker locker(&mutex_); const auto& node_data = optimization_problem_.node_data(); - for (size_t trajectory_id = 0; trajectory_id != node_data.size(); - ++trajectory_id) { - size_t node_index = 0; - const size_t num_nodes = trajectory_nodes_.at(trajectory_id).size(); - for (; node_index != node_data[trajectory_id].size(); ++node_index) { - trajectory_nodes_[trajectory_id][node_index].pose = transform::Embed3D( + for (int trajectory_id = 0; + trajectory_id != static_cast(node_data.size()); ++trajectory_id) { + int node_index = 0; + const int num_nodes = trajectory_nodes_.num_indices(trajectory_id); + for (; node_index != static_cast(node_data[trajectory_id].size()); + ++node_index) { + const mapping::NodeId node_id{trajectory_id, node_index}; + trajectory_nodes_.at(node_id).pose = transform::Embed3D( node_data[trajectory_id][node_index].point_cloud_pose); } // Extrapolate all point cloud poses that were added later. @@ -408,9 +379,9 @@ void SparsePoseGraph::RunOptimization() { const transform::Rigid3d extrapolation_transform = new_submap_transforms.back() * old_submap_transforms.back().inverse(); for (; node_index < num_nodes; ++node_index) { - trajectory_nodes_[trajectory_id][node_index].pose = - extrapolation_transform * - trajectory_nodes_[trajectory_id][node_index].pose; + const mapping::NodeId node_id{trajectory_id, node_index}; + trajectory_nodes_.at(node_id).pose = + extrapolation_transform * trajectory_nodes_.at(node_id).pose; } } optimized_submap_transforms_ = optimization_problem_.submap_data(); @@ -426,7 +397,7 @@ void SparsePoseGraph::RunOptimization() { std::vector> SparsePoseGraph::GetTrajectoryNodes() { common::MutexLocker locker(&mutex_); - return trajectory_nodes_; + return trajectory_nodes_.data(); } std::vector SparsePoseGraph::constraints() { @@ -437,15 +408,17 @@ std::vector SparsePoseGraph::constraints() { transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( const int trajectory_id) { common::MutexLocker locker(&mutex_); - if (submap_states_.size() <= static_cast(trajectory_id) || - submap_states_.at(trajectory_id).empty()) { + if (submap_states_.num_trajectories() <= trajectory_id || + submap_states_.num_indices(trajectory_id) == 0) { return transform::Rigid3d::Identity(); } const auto extrapolated_submap_transforms = ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id); return extrapolated_submap_transforms.back() * - submap_states_.at(trajectory_id) - .at(extrapolated_submap_transforms.size() - 1) + submap_states_ + .at(mapping::SubmapId{ + trajectory_id, + static_cast(extrapolated_submap_transforms.size()) - 1}) .submap->local_pose.inverse(); } @@ -464,15 +437,19 @@ std::vector SparsePoseGraph::GetSubmapTransforms( std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( const std::vector>& submap_transforms, - const size_t trajectory_id) const { - if (trajectory_id >= submap_states_.size()) { + const int trajectory_id) const { + if (trajectory_id >= submap_states_.num_trajectories()) { return {transform::Rigid3d::Identity()}; } // Submaps for which we have optimized poses. std::vector result; - for (const auto& state : submap_states_.at(trajectory_id)) { - if (trajectory_id < submap_transforms.size() && + for (int submap_index = 0; + submap_index != submap_states_.num_indices(trajectory_id); + ++submap_index) { + const mapping::SubmapId submap_id{trajectory_id, submap_index}; + const auto& state = submap_states_.at(submap_id); + if (static_cast(trajectory_id) < submap_transforms.size() && result.size() < submap_transforms.at(trajectory_id).size()) { // Submaps for which we have optimized poses. result.push_back( @@ -482,11 +459,12 @@ std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( } else { // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps // is okay, since the member is const. - result.push_back(result.back() * - submap_states_.at(trajectory_id) - .at(result.size() - 1) - .submap->local_pose.inverse() * - state.submap->local_pose); + const mapping::SubmapId previous_submap_id{ + trajectory_id, static_cast(result.size()) - 1}; + result.push_back( + result.back() * + submap_states_.at(previous_submap_id).submap->local_pose.inverse() * + state.submap->local_pose); } } diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index a6291df..2c32f6d 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -152,7 +152,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::vector ExtrapolateSubmapTransforms( const std::vector>& submap_transforms, - size_t trajectory_id) const REQUIRES(mutex_); + int trajectory_id) const REQUIRES(mutex_); const mapping::proto::SparsePoseGraphOptions options_; common::Mutex mutex_; @@ -184,7 +184,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // before they take part in the background computations. std::map submap_ids_ GUARDED_BY(mutex_); - std::vector> submap_states_ GUARDED_BY(mutex_); + mapping::NestedVectorsById submap_states_ + GUARDED_BY(mutex_); // Connectivity structure of our trajectories by IDs. std::vector> connected_components_; @@ -196,8 +197,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Deque to keep references valid for the background computation when adding // new data. std::deque constant_node_data_; - std::vector> trajectory_nodes_ - GUARDED_BY(mutex_); + mapping::NestedVectorsById + trajectory_nodes_ GUARDED_BY(mutex_); int num_trajectory_nodes_ = 0 GUARDED_BY(mutex_); // Current submap transforms used for displaying data. diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index cce3a7b..286ae02 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -100,23 +100,17 @@ void SparsePoseGraph::AddScan( time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}, sensor::Compress(range_data_in_tracking), trajectory_id, transform::Rigid3d::Identity()}); - trajectory_nodes_.resize( - std::max(trajectory_nodes_.size(), trajectory_id + 1)); - trajectory_nodes_[trajectory_id].push_back( + trajectory_nodes_.Append( + trajectory_id, mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose}); ++num_trajectory_nodes_; trajectory_connectivity_.Add(trajectory_id); if (submap_ids_.count(insertion_submaps.back()) == 0) { - submap_states_.resize( - std::max(submap_states_.size(), trajectory_id + 1)); - auto& trajectory_submap_states = submap_states_.at(trajectory_id); - submap_ids_.emplace( - insertion_submaps.back(), - mapping::SubmapId{trajectory_id, - static_cast(trajectory_submap_states.size())}); - trajectory_submap_states.emplace_back(); - trajectory_submap_states.back().submap = insertion_submaps.back(); + const mapping::SubmapId submap_id = + submap_states_.Append(trajectory_id, SubmapState()); + submap_ids_.emplace(insertion_submaps.back(), submap_id); + submap_states_.at(submap_id).submap = insertion_submaps.back(); } const Submap* const finished_submap = insertion_submaps.front()->finished ? insertion_submaps.front() : nullptr; @@ -168,16 +162,10 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, std::vector submap_nodes; for (const mapping::NodeId& submap_node_id : - submap_states_.at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .node_ids) { + submap_states_.at(submap_id).node_ids) { submap_nodes.push_back(mapping::TrajectoryNode{ - trajectory_nodes_.at(submap_node_id.trajectory_id) - .at(submap_node_id.node_index) - .constant_data, - inverse_submap_pose * trajectory_nodes_.at(submap_node_id.trajectory_id) - .at(submap_node_id.node_index) - .pose}); + trajectory_nodes_.at(submap_node_id).constant_data, + inverse_submap_pose * trajectory_nodes_.at(submap_node_id).pose}); } // Only globally match against submaps not in this trajectory. @@ -196,14 +184,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, // yaw component will be handled by the matching procedure in the // FastCorrelativeScanMatcher, and the given yaw is essentially ignored. constraint_builder_.MaybeAddGlobalConstraint( - submap_id, - submap_states_.at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .submap, - node_id, - &trajectory_nodes_.at(node_id.trajectory_id) - .at(node_id.node_index) - .constant_data->range_data_3d.returns, + submap_id, submap_states_.at(submap_id).submap, node_id, + &trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns, submap_nodes, initial_relative_pose.rotation(), &trajectory_connectivity_); } else { @@ -215,14 +197,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, if (node_id.trajectory_id == submap_id.trajectory_id || scan_and_submap_trajectories_connected) { constraint_builder_.MaybeAddConstraint( - submap_id, - submap_states_.at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .submap, - node_id, - &trajectory_nodes_.at(node_id.trajectory_id) - .at(node_id.node_index) - .constant_data->range_data_3d.returns, + submap_id, submap_states_.at(submap_id).submap, node_id, + &trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns, submap_nodes, initial_relative_pose); } } @@ -230,8 +206,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) { const auto submap_id = GetSubmapId(submap); - const auto& submap_state = - submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index); + const auto& submap_state = submap_states_.at(submap_id); const auto& node_data = optimization_problem_.node_data(); for (size_t trajectory_id = 0; trajectory_id != node_data.size(); @@ -268,20 +243,14 @@ void SparsePoseGraph::ComputeConstraintsForScan( .size()) : 0}; const mapping::TrajectoryNode::ConstantData* const scan_data = - trajectory_nodes_.at(node_id.trajectory_id) - .at(node_id.node_index) - .constant_data; + trajectory_nodes_.at(node_id).constant_data; CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id); optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id, scan_data->time, optimized_pose); for (const Submap* submap : insertion_submaps) { const mapping::SubmapId submap_id = GetSubmapId(submap); - CHECK(!submap_states_.at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .finished); - submap_states_.at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .node_ids.emplace(node_id); + CHECK(!submap_states_.at(submap_id).finished); + submap_states_.at(submap_id).node_ids.emplace(node_id); // Unchanged covariance as (submap <- map) is a translation. const transform::Rigid3d constraint_transform = submap->local_pose.inverse() * pose; @@ -295,28 +264,22 @@ void SparsePoseGraph::ComputeConstraintsForScan( Constraint::INTRA_SUBMAP}); } - for (size_t trajectory_id = 0; trajectory_id < submap_states_.size(); + for (int trajectory_id = 0; trajectory_id < submap_states_.num_trajectories(); ++trajectory_id) { - for (size_t submap_index = 0; - submap_index < submap_states_.at(trajectory_id).size(); + for (int submap_index = 0; + submap_index < submap_states_.num_indices(trajectory_id); ++submap_index) { - if (submap_states_.at(trajectory_id).at(submap_index).finished) { - CHECK_EQ(submap_states_.at(trajectory_id) - .at(submap_index) - .node_ids.count(node_id), - 0); - ComputeConstraint(node_id, - mapping::SubmapId{static_cast(trajectory_id), - static_cast(submap_index)}); + const mapping::SubmapId submap_id{trajectory_id, submap_index}; + if (submap_states_.at(submap_id).finished) { + CHECK_EQ(submap_states_.at(submap_id).node_ids.count(node_id), 0); + ComputeConstraint(node_id, submap_id); } } } if (finished_submap != nullptr) { const mapping::SubmapId finished_submap_id = GetSubmapId(finished_submap); - SubmapState& finished_submap_state = - submap_states_.at(finished_submap_id.trajectory_id) - .at(finished_submap_id.submap_index); + SubmapState& finished_submap_state = submap_states_.at(finished_submap_id); CHECK(!finished_submap_state.finished); // We have a new completed submap, so we look into adding constraints for // old scans. @@ -410,12 +373,14 @@ void SparsePoseGraph::RunOptimization() { common::MutexLocker locker(&mutex_); const auto& node_data = optimization_problem_.node_data(); - for (size_t trajectory_id = 0; trajectory_id != node_data.size(); - ++trajectory_id) { - size_t node_index = 0; - const size_t num_nodes = trajectory_nodes_.at(trajectory_id).size(); - for (; node_index != node_data[trajectory_id].size(); ++node_index) { - trajectory_nodes_[trajectory_id][node_index].pose = + for (int trajectory_id = 0; + trajectory_id != static_cast(node_data.size()); ++trajectory_id) { + int node_index = 0; + const int num_nodes = trajectory_nodes_.num_indices(trajectory_id); + for (; node_index != static_cast(node_data[trajectory_id].size()); + ++node_index) { + const mapping::NodeId node_id{trajectory_id, node_index}; + trajectory_nodes_.at(node_id).pose = node_data[trajectory_id][node_index].point_cloud_pose; } // Extrapolate all point cloud poses that were added later. @@ -427,9 +392,9 @@ void SparsePoseGraph::RunOptimization() { const transform::Rigid3d extrapolation_transform = new_submap_transforms.back() * old_submap_transforms.back().inverse(); for (; node_index < num_nodes; ++node_index) { - trajectory_nodes_[trajectory_id][node_index].pose = - extrapolation_transform * - trajectory_nodes_[trajectory_id][node_index].pose; + const mapping::NodeId node_id{trajectory_id, node_index}; + trajectory_nodes_.at(node_id).pose = + extrapolation_transform * trajectory_nodes_.at(node_id).pose; } } optimized_submap_transforms_ = optimization_problem_.submap_data(); @@ -445,7 +410,7 @@ void SparsePoseGraph::RunOptimization() { std::vector> SparsePoseGraph::GetTrajectoryNodes() { common::MutexLocker locker(&mutex_); - return trajectory_nodes_; + return trajectory_nodes_.data(); } std::vector SparsePoseGraph::constraints() { @@ -456,15 +421,17 @@ std::vector SparsePoseGraph::constraints() { transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( const int trajectory_id) { common::MutexLocker locker(&mutex_); - if (submap_states_.size() <= static_cast(trajectory_id) || - submap_states_.at(trajectory_id).empty()) { + if (submap_states_.num_trajectories() <= trajectory_id || + submap_states_.num_indices(trajectory_id) == 0) { return transform::Rigid3d::Identity(); } const auto extrapolated_submap_transforms = ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id); return extrapolated_submap_transforms.back() * - submap_states_.at(trajectory_id) - .at(extrapolated_submap_transforms.size() - 1) + submap_states_ + .at(mapping::SubmapId{ + trajectory_id, + static_cast(extrapolated_submap_transforms.size()) - 1}) .submap->local_pose.inverse(); } @@ -483,15 +450,19 @@ std::vector SparsePoseGraph::GetSubmapTransforms( std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( const std::vector>& submap_transforms, - const size_t trajectory_id) const { - if (trajectory_id >= submap_states_.size()) { + const int trajectory_id) const { + if (trajectory_id >= submap_states_.num_trajectories()) { return {transform::Rigid3d::Identity()}; } // Submaps for which we have optimized poses. std::vector result; - for (const auto& state : submap_states_.at(trajectory_id)) { - if (trajectory_id < submap_transforms.size() && + for (int submap_index = 0; + submap_index != submap_states_.num_indices(trajectory_id); + ++submap_index) { + const mapping::SubmapId submap_id{trajectory_id, submap_index}; + const auto& state = submap_states_.at(submap_id); + if (static_cast(trajectory_id) < submap_transforms.size() && result.size() < submap_transforms.at(trajectory_id).size()) { // Submaps for which we have optimized poses. result.push_back( @@ -501,11 +472,12 @@ std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( } else { // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps // is okay, since the member is const. - result.push_back(result.back() * - submap_states_.at(trajectory_id) - .at(result.size() - 1) - .submap->local_pose.inverse() * - state.submap->local_pose); + const mapping::SubmapId previous_submap_id{ + trajectory_id, static_cast(result.size()) - 1}; + result.push_back( + result.back() * + submap_states_.at(previous_submap_id).submap->local_pose.inverse() * + state.submap->local_pose); } } diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 59804cd..a50e7fa 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -149,7 +149,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::vector ExtrapolateSubmapTransforms( const std::vector>& submap_transforms, - size_t trajectory_id) const REQUIRES(mutex_); + int trajectory_id) const REQUIRES(mutex_); const mapping::proto::SparsePoseGraphOptions options_; common::Mutex mutex_; @@ -181,7 +181,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // before they take part in the background computations. std::map submap_ids_ GUARDED_BY(mutex_); - std::vector> submap_states_ GUARDED_BY(mutex_); + mapping::NestedVectorsById submap_states_ + GUARDED_BY(mutex_); // Connectivity structure of our trajectories by IDs. std::vector> connected_components_; @@ -193,8 +194,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Deque to keep references valid for the background computation when adding // new data. std::deque constant_node_data_; - std::vector> trajectory_nodes_ - GUARDED_BY(mutex_); + mapping::NestedVectorsById + trajectory_nodes_ GUARDED_BY(mutex_); int num_trajectory_nodes_ = 0 GUARDED_BY(mutex_); // Current submap transforms used for displaying data.