diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 0079bc3..a49087b 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -113,20 +113,20 @@ string MapBuilder::SubmapToProto(const int trajectory_id, " trajectories."; } - const std::vector submap_transforms = - sparse_pose_graph_->GetSubmapTransforms(trajectory_id); - if (submap_index < 0 || - static_cast(submap_index) >= submap_transforms.size()) { + const int num_submaps = sparse_pose_graph_->num_submaps(trajectory_id); + if (submap_index < 0 || submap_index >= num_submaps) { return "Requested submap " + std::to_string(submap_index) + " from trajectory " + std::to_string(trajectory_id) + - " but there are only " + std::to_string(submap_transforms.size()) + + " but there are only " + std::to_string(num_submaps) + " submaps in this trajectory."; } const Submap* const submap = trajectory_builders_.at(trajectory_id)->submaps()->Get(submap_index); response->set_submap_version(submap->num_range_data()); - submap->ToResponseProto(submap_transforms[submap_index], response); + const auto submap_pose = sparse_pose_graph_->GetSubmapTransform( + SubmapId{trajectory_id, submap_index}); + submap->ToResponseProto(submap_pose, response); return ""; } diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index 583f380..85a07f9 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -86,9 +86,12 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() { } if (!single_trajectory_nodes.empty()) { - for (const auto& transform : GetSubmapTransforms(trajectory_id)) { + const int num_submaps_in_trajectory = num_submaps(trajectory_id); + for (int submap_index = 0; submap_index != num_submaps_in_trajectory; + ++submap_index) { + const SubmapId submap_id{static_cast(trajectory_id), submap_index}; *trajectory_proto->add_submap()->mutable_pose() = - transform::ToProto(transform); + transform::ToProto(GetSubmapTransform(submap_id)); } } } diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 229097c..903169b 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -49,8 +49,8 @@ class SparsePoseGraph { double rotation_weight; }; - mapping::SubmapId submap_id; // 'i' in the paper. - mapping::NodeId node_id; // 'j' in the paper. + SubmapId submap_id; // 'i' in the paper. + NodeId node_id; // 'j' in the paper. // Pose of the scan 'j' relative to submap 'i'. Pose pose; @@ -77,9 +77,11 @@ class SparsePoseGraph { // Gets the current trajectory clusters. virtual std::vector> GetConnectedTrajectories() = 0; - // Returns the current optimized transforms for the given 'trajectory_id'. - virtual std::vector GetSubmapTransforms( - int trajectory_id) = 0; + // Return the number of submaps for the given 'trajectory_id'. + virtual int num_submaps(int trajectory_id) = 0; + + // Returns the current optimized transform for the given 'submap_id'. + virtual transform::Rigid3d GetSubmapTransform(const SubmapId& submap_id) = 0; // 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 diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 6e88baa..0d88ecf 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -427,41 +427,32 @@ std::vector> SparsePoseGraph::GetConnectedTrajectories() { return connected_components_; } -std::vector SparsePoseGraph::GetSubmapTransforms( - const int trajectory_id) { +int SparsePoseGraph::num_submaps(const int trajectory_id) { common::MutexLocker locker(&mutex_); if (trajectory_id >= submap_data_.num_trajectories()) { - return {transform::Rigid3d::Identity()}; + return 0; } + return submap_data_.num_indices(trajectory_id); +} - // Submaps for which we have optimized poses. - std::vector result; - for (int submap_index = 0; - submap_index != submap_data_.num_indices(trajectory_id); - ++submap_index) { - const mapping::SubmapId submap_id{trajectory_id, submap_index}; - const auto& submap_data = submap_data_.at(submap_id); - if (static_cast(trajectory_id) < - optimized_submap_transforms_.size() && - result.size() < optimized_submap_transforms_.at(trajectory_id).size()) { - // Submaps for which we have optimized poses. - result.push_back( - transform::Embed3D(optimized_submap_transforms_.at(trajectory_id) - .at(result.size()) - .pose)); - } else { - // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submap - // is okay, since the member is const. - result.push_back(ComputeLocalToGlobalTransform( - optimized_submap_transforms_, trajectory_id) * - submap_data.submap->local_pose()); - } +transform::Rigid3d SparsePoseGraph::GetSubmapTransform( + const mapping::SubmapId& submap_id) { + common::MutexLocker locker(&mutex_); + // We already have an optimized pose. + if (submap_id.trajectory_id < + static_cast(optimized_submap_transforms_.size()) && + submap_id.submap_index < static_cast(optimized_submap_transforms_ + .at(submap_id.trajectory_id) + .size())) { + return transform::Embed3D( + optimized_submap_transforms_.at(submap_id.trajectory_id) + .at(submap_id.submap_index) + .pose); } - - if (result.empty()) { - result.push_back(transform::Rigid3d::Identity()); - } - return result; + // We have to extrapolate. + return ComputeLocalToGlobalTransform(optimized_submap_transforms_, + submap_id.trajectory_id) * + submap_data_.at(submap_id).submap->local_pose(); } transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 157d3b0..9f4a4c4 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -82,7 +82,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { void AddTrimmer(std::unique_ptr trimmer) override; void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() override; - std::vector GetSubmapTransforms(int trajectory_id) + int num_submaps(int trajectory_id) EXCLUDES(mutex_) override; + transform::Rigid3d GetSubmapTransform(const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) EXCLUDES(mutex_) override; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 29c15a5..35f02c0 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -430,40 +430,31 @@ std::vector> SparsePoseGraph::GetConnectedTrajectories() { return connected_components_; } -std::vector SparsePoseGraph::GetSubmapTransforms( - const int trajectory_id) { +int SparsePoseGraph::num_submaps(const int trajectory_id) { common::MutexLocker locker(&mutex_); if (trajectory_id >= submap_data_.num_trajectories()) { - return {transform::Rigid3d::Identity()}; + return 0; } + return submap_data_.num_indices(trajectory_id); +} - // Submaps for which we have optimized poses. - std::vector result; - for (int submap_index = 0; - submap_index != submap_data_.num_indices(trajectory_id); - ++submap_index) { - const mapping::SubmapId submap_id{trajectory_id, submap_index}; - const auto& submap_data = submap_data_.at(submap_id); - if (static_cast(trajectory_id) < - optimized_submap_transforms_.size() && - result.size() < optimized_submap_transforms_.at(trajectory_id).size()) { - // Submaps for which we have optimized poses. - result.push_back(optimized_submap_transforms_.at(trajectory_id) - .at(result.size()) - .pose); - } else { - // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submap - // is okay, since the member is const. - result.push_back(ComputeLocalToGlobalTransform( - optimized_submap_transforms_, trajectory_id) * - submap_data.submap->local_pose()); - } +transform::Rigid3d SparsePoseGraph::GetSubmapTransform( + const mapping::SubmapId& submap_id) { + common::MutexLocker locker(&mutex_); + // We already have an optimized pose. + if (submap_id.trajectory_id < + static_cast(optimized_submap_transforms_.size()) && + submap_id.submap_index < static_cast(optimized_submap_transforms_ + .at(submap_id.trajectory_id) + .size())) { + return optimized_submap_transforms_.at(submap_id.trajectory_id) + .at(submap_id.submap_index) + .pose; } - - if (result.empty()) { - result.push_back(transform::Rigid3d::Identity()); - } - return result; + // We have to extrapolate. + return ComputeLocalToGlobalTransform(optimized_submap_transforms_, + submap_id.trajectory_id) * + submap_data_.at(submap_id).submap->local_pose(); } transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index d6ba4ab..e1c8c28 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -81,7 +81,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { } void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() override; - std::vector GetSubmapTransforms(int trajectory_id) + int num_submaps(int trajectory_id) EXCLUDES(mutex_) override; + transform::Rigid3d GetSubmapTransform(const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) EXCLUDES(mutex_) override;