diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index c8ce8c4..f26bc32 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -128,17 +128,17 @@ string MapBuilder::SubmapToProto(const int trajectory_id, const Submaps* const submaps = trajectory_builders_.at(trajectory_id)->submaps(); - if (submap_index < 0 || submap_index >= submaps->size()) { + const std::vector submap_transforms = + sparse_pose_graph_->GetSubmapTransforms(submaps); + if (submap_index < 0 || + static_cast(submap_index) >= submap_transforms.size()) { return "Requested submap " + std::to_string(submap_index) + " from trajectory " + std::to_string(trajectory_id) + - " but there are only " + std::to_string(submaps->size()) + + " but there are only " + std::to_string(submap_transforms.size()) + " submaps in this trajectory."; } response->set_submap_version(submaps->Get(submap_index)->num_range_data); - const std::vector submap_transforms = - sparse_pose_graph_->GetSubmapTransforms(*submaps); - CHECK_EQ(submap_transforms.size(), submaps->size()); submaps->SubmapToProto(submap_index, sparse_pose_graph_->GetTrajectoryNodes(), submap_transforms[submap_index], response); return ""; diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index 65525d2..bb1bcf0 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -150,8 +150,8 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() { transform::ToProto(node.pose * node.constant_data->tracking_to_pose); } - const Submaps* const submaps = group[0].constant_data->trajectory; - for (const auto& transform : GetSubmapTransforms(*submaps)) { + const Submaps* const trajectory = group[0].constant_data->trajectory; + for (const auto& transform : GetSubmapTransforms(trajectory)) { *trajectory_proto->add_submap()->mutable_pose() = transform::ToProto(transform); } diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index c44bf6b..e390eda 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -109,15 +109,15 @@ class SparsePoseGraph { virtual std::vector> GetConnectedTrajectories() = 0; - // Returns the current optimized transforms for the given 'submaps'. + // Returns the current optimized transforms for the given 'trajectory'. virtual std::vector GetSubmapTransforms( - const Submaps& submaps) = 0; + const Submaps* trajectory) = 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 // discontinuous, loop-closed frame). virtual transform::Rigid3d GetLocalToGlobalTransform( - const Submaps& submaps) = 0; + const Submaps* submaps) = 0; // Returns the current optimized trajectory. virtual std::vector GetTrajectoryNodes() = 0; diff --git a/cartographer/mapping/submaps.h b/cartographer/mapping/submaps.h index 7ad2879..689a86f 100644 --- a/cartographer/mapping/submaps.h +++ b/cartographer/mapping/submaps.h @@ -75,7 +75,7 @@ struct Submap { } // Origin of this submap. - Eigen::Vector3f origin; + const Eigen::Vector3f origin; // Number of RangeData inserted. int num_range_data = 0; diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 0123454..755c27f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -84,11 +84,11 @@ 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* submaps, + const mapping::Submaps* const trajectory, const mapping::Submap* const matching_submap, const std::vector& insertion_submaps) { - const transform::Rigid3d optimized_pose(GetLocalToGlobalTransform(*submaps) * - transform::Embed3D(pose)); + const transform::Rigid3d optimized_pose( + GetLocalToGlobalTransform(trajectory) * transform::Embed3D(pose)); common::MutexLocker locker(&mutex_); const int j = trajectory_nodes_.size(); @@ -96,19 +96,19 @@ void SparsePoseGraph::AddScan( constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ time, range_data_in_pose, - Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), submaps, + Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), trajectory, tracking_to_pose}); trajectory_nodes_.push_back(mapping::TrajectoryNode{ &constant_node_data_.back(), optimized_pose, }); - trajectory_connectivity_.Add(submaps); + trajectory_connectivity_.Add(trajectory); if (submap_indices_.count(insertion_submaps.back()) == 0) { submap_indices_.emplace(insertion_submaps.back(), static_cast(submap_indices_.size())); submap_states_.emplace_back(); submap_states_.back().submap = insertion_submaps.back(); - submap_states_.back().trajectory = submaps; + submap_states_.back().trajectory = trajectory; CHECK_EQ(submap_states_.size(), submap_indices_.size()); } const mapping::Submap* const finished_submap = @@ -117,8 +117,8 @@ void SparsePoseGraph::AddScan( : nullptr; // Make sure we have a sampler for this trajectory. - if (!global_localization_samplers_[submaps]) { - global_localization_samplers_[submaps] = + if (!global_localization_samplers_[trajectory]) { + global_localization_samplers_[trajectory] = common::make_unique( options_.global_sampling_ratio()); } @@ -347,13 +347,14 @@ void SparsePoseGraph::RunOptimization() { const mapping::Submaps* trajectory = trajectory_nodes_[i].constant_data->trajectory; if (extrapolation_transforms.count(trajectory) == 0) { - extrapolation_transforms[trajectory] = transform::Rigid3d( - ExtrapolateSubmapTransforms(submap_transforms_, *trajectory) - .back() * - ExtrapolateSubmapTransforms(optimized_submap_transforms_, - *trajectory) - .back() - .inverse()); + const auto new_submap_transforms = + ExtrapolateSubmapTransforms(submap_transforms_, trajectory); + const auto old_submap_transforms = ExtrapolateSubmapTransforms( + optimized_submap_transforms_, trajectory); + CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); + extrapolation_transforms[trajectory] = + transform::Rigid3d(new_submap_transforms.back() * + old_submap_transforms.back().inverse()); } trajectory_nodes_[i].pose = extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose; @@ -384,9 +385,12 @@ std::vector SparsePoseGraph::constraints() { } transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( - const mapping::Submaps& submaps) { - return GetSubmapTransforms(submaps).back() * - submaps.Get(submaps.size() - 1)->local_pose().inverse(); + const mapping::Submaps* const submaps) { + const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps); + return extrapolated_submap_transforms.back() * + submaps->Get(extrapolated_submap_transforms.size() - 1) + ->local_pose() + .inverse(); } std::vector> @@ -396,39 +400,51 @@ SparsePoseGraph::GetConnectedTrajectories() { } std::vector SparsePoseGraph::GetSubmapTransforms( - const mapping::Submaps& submaps) { + const mapping::Submaps* const trajectory) { common::MutexLocker locker(&mutex_); - return ExtrapolateSubmapTransforms(optimized_submap_transforms_, submaps); + return ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory); } std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( const std::vector& submap_transforms, - const mapping::Submaps& submaps) const { + const mapping::Submaps* const trajectory) const { std::vector result; - int i = 0; + size_t flat_index = 0; + size_t flat_index_of_result_back = -1; + // Submaps for which we have optimized poses. - for (; i < submaps.size(); ++i) { - const mapping::Submap* submap = submaps.Get(i); - const auto submap_and_index = submap_indices_.find(submap); - // Determine if we have a loop-closed transform for this submap. - if (submap_and_index == submap_indices_.end() || - static_cast(submap_and_index->second) >= - submap_transforms.size()) { + for (; flat_index != submap_states_.size(); ++flat_index) { + const auto& state = submap_states_[flat_index]; + if (state.trajectory != trajectory) { + continue; + } + if (flat_index >= submap_transforms.size()) { break; } - result.push_back( - transform::Embed3D(submap_transforms[submap_and_index->second])); + result.push_back(transform::Embed3D(submap_transforms[flat_index])); + flat_index_of_result_back = flat_index; } // Extrapolate to the remaining submaps. - for (; i < submaps.size(); ++i) { - if (i == 0) { - result.push_back(transform::Rigid3d::Identity()); + for (; flat_index != submap_states_.size(); ++flat_index) { + const auto& state = submap_states_[flat_index]; + if (state.trajectory != trajectory) { continue; } - result.push_back(result.back() * - submaps.Get(result.size() - 1)->local_pose().inverse() * - submaps.Get(result.size())->local_pose()); + if (result.empty()) { + result.push_back(transform::Rigid3d::Identity()); + } else { + // Accessing local_pose() in Submaps is okay, since the member is const. + result.push_back(result.back() * + submap_states_[flat_index_of_result_back] + .submap->local_pose() + .inverse() * + state.submap->local_pose()); + } + flat_index_of_result_back = flat_index; + } + if (result.empty()) { + result.push_back(transform::Rigid3d::Identity()); } return result; } diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index b8dd225..1d3eeb8 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -72,7 +72,7 @@ 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* submaps, + const mapping::Submaps* trajectory, const mapping::Submap* matching_submap, const std::vector& insertion_submaps) EXCLUDES(mutex_); @@ -86,8 +86,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::vector> GetConnectedTrajectories() override; std::vector GetSubmapTransforms( - const mapping::Submaps& submaps) EXCLUDES(mutex_) override; - transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps& submaps) + const mapping::Submaps* trajectory) EXCLUDES(mutex_) override; + transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps) EXCLUDES(mutex_) override; std::vector GetTrajectoryNodes() override EXCLUDES(mutex_); @@ -139,7 +139,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Adds extrapolated transforms, so that there are transforms for all submaps. std::vector ExtrapolateSubmapTransforms( const std::vector& submap_transforms, - const mapping::Submaps& submaps) const REQUIRES(mutex_); + const mapping::Submaps* trajectory) const REQUIRES(mutex_); const mapping::proto::SparsePoseGraphOptions options_; common::Mutex mutex_; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 75133f4..f6e53b3 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -83,11 +83,11 @@ 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* submaps, - const Submap* const matching_submap, + const kalman_filter::PoseCovariance& covariance, + const Submaps* const trajectory, const Submap* const matching_submap, const std::vector& insertion_submaps) { - const transform::Rigid3d optimized_pose(GetLocalToGlobalTransform(*submaps) * - pose); + const transform::Rigid3d optimized_pose( + GetLocalToGlobalTransform(trajectory) * pose); common::MutexLocker locker(&mutex_); const int j = trajectory_nodes_.size(); @@ -95,26 +95,26 @@ void SparsePoseGraph::AddScan( constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}, - sensor::Compress(range_data_in_tracking), submaps, + sensor::Compress(range_data_in_tracking), trajectory, transform::Rigid3d::Identity()}); trajectory_nodes_.push_back( mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose}); - trajectory_connectivity_.Add(submaps); + trajectory_connectivity_.Add(trajectory); if (submap_indices_.count(insertion_submaps.back()) == 0) { submap_indices_.emplace(insertion_submaps.back(), static_cast(submap_indices_.size())); submap_states_.emplace_back(); submap_states_.back().submap = insertion_submaps.back(); - submap_states_.back().trajectory = submaps; + submap_states_.back().trajectory = trajectory; CHECK_EQ(submap_states_.size(), submap_indices_.size()); } const Submap* const finished_submap = insertion_submaps.front()->finished ? insertion_submaps.front() : nullptr; // Make sure we have a sampler for this trajectory. - if (!global_localization_samplers_[submaps]) { - global_localization_samplers_[submaps] = + if (!global_localization_samplers_[trajectory]) { + global_localization_samplers_[trajectory] = common::make_unique( options_.global_sampling_ratio()); } @@ -341,13 +341,14 @@ void SparsePoseGraph::RunOptimization() { const mapping::Submaps* trajectory = trajectory_nodes_[i].constant_data->trajectory; if (extrapolation_transforms.count(trajectory) == 0) { - extrapolation_transforms[trajectory] = transform::Rigid3d( - ExtrapolateSubmapTransforms(submap_transforms_, *trajectory) - .back() * - ExtrapolateSubmapTransforms(optimized_submap_transforms_, - *trajectory) - .back() - .inverse()); + const auto new_submap_transforms = + ExtrapolateSubmapTransforms(submap_transforms_, trajectory); + const auto old_submap_transforms = ExtrapolateSubmapTransforms( + optimized_submap_transforms_, trajectory); + CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); + extrapolation_transforms[trajectory] = + transform::Rigid3d(new_submap_transforms.back() * + old_submap_transforms.back().inverse()); } trajectory_nodes_[i].pose = extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose; @@ -388,9 +389,12 @@ std::vector SparsePoseGraph::constraints() { } transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( - const mapping::Submaps& submaps) { - return GetSubmapTransforms(submaps).back() * - submaps.Get(submaps.size() - 1)->local_pose().inverse(); + const mapping::Submaps* const submaps) { + const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps); + return extrapolated_submap_transforms.back() * + submaps->Get(extrapolated_submap_transforms.size() - 1) + ->local_pose() + .inverse(); } std::vector> @@ -400,38 +404,51 @@ SparsePoseGraph::GetConnectedTrajectories() { } std::vector SparsePoseGraph::GetSubmapTransforms( - const mapping::Submaps& submaps) { + const mapping::Submaps* const trajectory) { common::MutexLocker locker(&mutex_); - return ExtrapolateSubmapTransforms(optimized_submap_transforms_, submaps); + return ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory); } std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( const std::vector& submap_transforms, - const mapping::Submaps& submaps) const { + const mapping::Submaps* const trajectory) const { std::vector result; - int i = 0; + size_t flat_index = 0; + size_t flat_index_of_result_back = -1; + // Submaps for which we have optimized poses. - for (; i < submaps.size(); ++i) { - const mapping::Submap* submap = submaps.Get(i); - const auto submap_and_index = submap_indices_.find(submap); - // Determine if we have a loop-closed transform for this submap. - if (submap_and_index == submap_indices_.end() || - static_cast(submap_and_index->second) >= - submap_transforms.size()) { + for (; flat_index != submap_states_.size(); ++flat_index) { + const auto& state = submap_states_[flat_index]; + if (state.trajectory != trajectory) { + continue; + } + if (flat_index >= submap_transforms.size()) { break; } - result.push_back(submap_transforms[submap_and_index->second]); + result.push_back(submap_transforms[flat_index]); + flat_index_of_result_back = flat_index; } // Extrapolate to the remaining submaps. - for (; i < submaps.size(); ++i) { - if (i == 0) { - result.push_back(transform::Rigid3d::Identity()); + for (; flat_index != submap_states_.size(); ++flat_index) { + const auto& state = submap_states_[flat_index]; + if (state.trajectory != trajectory) { continue; } - result.push_back(result.back() * - submaps.Get(result.size() - 1)->local_pose().inverse() * - submaps.Get(result.size())->local_pose()); + if (result.empty()) { + result.push_back(transform::Rigid3d::Identity()); + } else { + // Accessing local_pose() in Submaps is okay, since the member is const. + result.push_back(result.back() * + submap_states_[flat_index_of_result_back] + .submap->local_pose() + .inverse() * + state.submap->local_pose()); + } + flat_index_of_result_back = flat_index; + } + if (result.empty()) { + result.push_back(transform::Rigid3d::Identity()); } return result; } diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 6b83401..98be737 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -71,7 +71,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* submaps, const Submap* matching_submap, + const Submaps* trajectory, const Submap* matching_submap, const std::vector& insertion_submaps) EXCLUDES(mutex_); @@ -88,8 +88,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::vector> GetConnectedTrajectories() override; std::vector GetSubmapTransforms( - const mapping::Submaps& submaps) EXCLUDES(mutex_) override; - transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps& submaps) + const mapping::Submaps* trajectory) EXCLUDES(mutex_) override; + transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps) EXCLUDES(mutex_) override; std::vector GetTrajectoryNodes() override EXCLUDES(mutex_); @@ -159,7 +159,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Adds extrapolated transforms, so that there are transforms for all submaps. std::vector ExtrapolateSubmapTransforms( const std::vector& submap_transforms, - const mapping::Submaps& submaps) const REQUIRES(mutex_); + const mapping::Submaps* trajectory) const REQUIRES(mutex_); const mapping::proto::SparsePoseGraphOptions options_; common::Mutex mutex_; diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h index 17ced54..55fba95 100644 --- a/cartographer/mapping_3d/submaps.h +++ b/cartographer/mapping_3d/submaps.h @@ -109,6 +109,8 @@ class Submaps : public mapping::Submaps { const proto::SubmapsOptions options_; + // 'submaps_' contains pointers, so that resizing the vector does not + // invalidate handed out Submap* pointers. std::vector> submaps_; RangeDataInserter range_data_inserter_; };