diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 867fe9c..4778302 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -65,20 +65,25 @@ std::vector SparsePoseGraph::GrowSubmapTransformsAsNeeded( trajectory_id, sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0])); } - const mapping::SubmapId submap_id{ - trajectory_id, static_cast(submap_data[trajectory_id].size()) - 1}; + CHECK_EQ(optimization_problem_.num_trimmed_submaps(trajectory_id), 0); + CHECK_EQ(submap_data[trajectory_id].size(), 1); + const mapping::SubmapId submap_id{trajectory_id, 0}; CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front()); return {submap_id}; } CHECK_EQ(2, insertion_submaps.size()); + const int num_trimmed_submaps = + optimization_problem_.num_trimmed_submaps(trajectory_id); const mapping::SubmapId last_submap_id{ - trajectory_id, - static_cast(submap_data.at(trajectory_id).size() - 1)}; + trajectory_id, static_cast(submap_data.at(trajectory_id).size() + + num_trimmed_submaps - 1)}; if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) { // In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()' // and 'insertions_submaps.back()' is new. const auto& first_submap_pose = - submap_data.at(trajectory_id).at(last_submap_id.submap_index).pose; + submap_data.at(trajectory_id) + .at(last_submap_id.submap_index - num_trimmed_submaps) + .pose; optimization_problem_.AddSubmap( trajectory_id, first_submap_pose * @@ -186,7 +191,9 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, const transform::Rigid2d initial_relative_pose = optimization_problem_.submap_data() .at(submap_id.trajectory_id) - .at(submap_id.submap_index) + .at(submap_id.submap_index - + optimization_problem_.num_trimmed_submaps( + submap_id.trajectory_id)) .pose.inverse() * optimization_problem_.node_data() .at(node_id.trajectory_id) @@ -231,10 +238,11 @@ void SparsePoseGraph::ComputeConstraintsForScan( GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps); CHECK_EQ(submap_ids.size(), insertion_submaps.size()); const mapping::SubmapId matching_id = submap_ids.front(); + const int num_trimmed_submaps = optimization_problem_.num_trimmed_submaps(trajectory_id); const transform::Rigid2d optimized_pose = optimization_problem_.submap_data() .at(matching_id.trajectory_id) - .at(matching_id.submap_index) + .at(matching_id.submap_index - num_trimmed_submaps) .pose * sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front()) .inverse() * @@ -389,6 +397,14 @@ void SparsePoseGraph::RunOptimization() { optimization_problem_.Solve(constraints_); common::MutexLocker locker(&mutex_); + std::vector num_trimmed_submaps; + const auto& submap_data = optimization_problem_.submap_data(); + for (int trajectory_id = 0; + trajectory_id != static_cast(submap_data.size()); ++trajectory_id) { + num_trimmed_submaps.push_back( + optimization_problem_.num_trimmed_submaps(trajectory_id)); + } + const auto& node_data = optimization_problem_.node_data(); for (int trajectory_id = 0; trajectory_id != static_cast(node_data.size()); ++trajectory_id) { @@ -403,9 +419,10 @@ void SparsePoseGraph::RunOptimization() { } // Extrapolate all point cloud poses that were added later. const auto local_to_new_global = ComputeLocalToGlobalTransform( - optimization_problem_.submap_data(), trajectory_id); + submap_data, num_trimmed_submaps, trajectory_id); const auto local_to_old_global = ComputeLocalToGlobalTransform( - optimized_submap_transforms_, trajectory_id); + optimized_submap_transforms_, num_trimmed_submaps_at_last_optimization_, + trajectory_id); const transform::Rigid3d old_global_to_new_global = local_to_new_global * local_to_old_global.inverse(); for (; node_index < num_nodes; ++node_index) { @@ -414,7 +431,8 @@ void SparsePoseGraph::RunOptimization() { old_global_to_new_global * trajectory_nodes_.at(node_id).pose; } } - optimized_submap_transforms_ = optimization_problem_.submap_data(); + optimized_submap_transforms_ = submap_data; + num_trimmed_submaps_at_last_optimization_ = num_trimmed_submaps; connected_components_ = trajectory_connectivity_.ConnectedComponents(); reverse_connected_components_.clear(); for (size_t i = 0; i != connected_components_.size(); ++i) { @@ -444,6 +462,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( const int trajectory_id) { common::MutexLocker locker(&mutex_); return ComputeLocalToGlobalTransform(optimized_submap_transforms_, + num_trimmed_submaps_at_last_optimization_, trajectory_id); } @@ -486,8 +505,9 @@ SparsePoseGraph::GetAllSubmapData() { } transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( - const std::vector>& + const std::vector>& submap_transforms, + const std::vector& num_trimmed_submaps, const int trajectory_id) const { if (trajectory_id >= static_cast(submap_transforms.size()) || submap_transforms.at(trajectory_id).empty()) { @@ -495,7 +515,8 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( } const mapping::SubmapId last_optimized_submap_id{ trajectory_id, - static_cast(submap_transforms.at(trajectory_id).size() - 1)}; + static_cast(submap_transforms.at(trajectory_id).size() + + num_trimmed_submaps.at(trajectory_id) - 1)}; // Accessing 'local_pose' in Submap is okay, since the member is const. return transform::Embed3D(submap_transforms.at(trajectory_id).back().pose) * submap_data_.at(last_optimized_submap_id) @@ -509,20 +530,25 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( return {}; } auto submap = submap_data_.at(submap_id).submap; - // 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 {submap, transform::Embed3D( - optimized_submap_transforms_.at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .pose)}; + static_cast(optimized_submap_transforms_.size())) { + const size_t submap_data_index = + submap_id.submap_index - + num_trimmed_submaps_at_last_optimization_.at(submap_id.trajectory_id); + if (submap_data_index < + optimized_submap_transforms_.at(submap_id.trajectory_id).size()) { + // We already have an optimized pose. + return {submap, transform::Embed3D(optimized_submap_transforms_ + .at(submap_id.trajectory_id) + .at(submap_data_index) + .pose)}; + } } // We have to extrapolate. - return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_, - submap_id.trajectory_id) * + return {submap, ComputeLocalToGlobalTransform( + optimized_submap_transforms_, + num_trimmed_submaps_at_last_optimization_, + submap_id.trajectory_id) * submap->local_pose()}; } @@ -531,7 +557,8 @@ SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent) int SparsePoseGraph::TrimmingHandle::num_submaps( const int trajectory_id) const { - return parent_->optimization_problem_.submap_data().at(trajectory_id).size(); + return parent_->optimization_problem_.submap_data().at(trajectory_id).size() + + parent_->optimization_problem_.num_trimmed_submaps(trajectory_id); } void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( @@ -585,6 +612,7 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( CHECK(submap_data.submap != nullptr); submap_data.submap.reset(); parent_->constraint_builder_.DeleteScanMatcher(submap_id); + parent_->optimization_problem_.TrimSubmap(submap_id); // Mark the 'nodes_to_remove' as trimmed and remove their data. for (const mapping::NodeId& node_id : nodes_to_remove) { @@ -592,11 +620,6 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( parent_->trajectory_nodes_.at(node_id).constant_data.reset(); parent_->optimization_problem_.TrimTrajectoryNode(node_id); } - - // TODO(whess): The optimization problem should no longer include the submap. - - // TODO(whess): If the first submap is gone, we want to tie the first not - // yet trimmed submap to be set fixed to its current pose. } } // namespace mapping_2d diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 00cd00a..2c61b30 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -150,9 +150,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Computes the local to global frame transform based on the given optimized // 'submap_transforms'. transform::Rigid3d ComputeLocalToGlobalTransform( - const std::vector>& + const std::vector>& submap_transforms, - int trajectory_id) const REQUIRES(mutex_); + const std::vector& num_trimmed_submaps, int trajectory_id) const + REQUIRES(mutex_); mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock( const mapping::SubmapId& submap_id) REQUIRES(mutex_); @@ -199,7 +200,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Current submap transforms used for displaying data. - std::vector> + std::vector num_trimmed_submaps_at_last_optimization_ GUARDED_BY(mutex_); + std::vector> optimized_submap_transforms_ GUARDED_BY(mutex_); // List of all trimmers to consult when optimizations finish. diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 51417de..1d8eafe 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -104,6 +104,18 @@ void OptimizationProblem::AddSubmap(const int trajectory_id, submap_data_.resize( std::max(submap_data_.size(), static_cast(trajectory_id) + 1)); submap_data_[trajectory_id].push_back(SubmapData{submap_pose}); + trajectory_data_.resize( + std::max(trajectory_data_.size(), submap_data_.size())); +} + +void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) { + auto& trajectory_data = trajectory_data_.at(submap_id.trajectory_id); + // We only allow trimming from the start. + CHECK_EQ(trajectory_data.num_trimmed_submaps, submap_id.submap_index); + auto& submap_data = submap_data_.at(submap_id.trajectory_id); + CHECK(!submap_data.empty()); + submap_data.pop_front(); + ++trajectory_data.num_trimmed_submaps; } void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { @@ -122,23 +134,22 @@ void OptimizationProblem::Solve(const std::vector& constraints) { // Set the starting point. // TODO(hrapp): Move ceres data into SubmapData. - std::vector>> C_submaps(submap_data_.size()); + std::vector>> C_submaps(submap_data_.size()); std::vector>> C_nodes(node_data_.size()); + bool first_submap = true; for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); ++trajectory_id) { - for (size_t submap_index = 0; - submap_index != submap_data_[trajectory_id].size(); ++submap_index) { - if (trajectory_id == 0 && submap_index == 0) { - // Fix the pose of the first submap of the first trajectory. - C_submaps[trajectory_id].push_back( - FromPose(transform::Rigid2d::Identity())); - problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3); + // Reserve guarantees that data does not move, so the pointers for Ceres + // stay valid. + C_submaps[trajectory_id].reserve(submap_data_[trajectory_id].size()); + for (const SubmapData& submap_data : submap_data_[trajectory_id]) { + C_submaps[trajectory_id].push_back(FromPose(submap_data.pose)); + problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3); + if (first_submap) { + first_submap = false; + // Fix the pose of the first submap. problem.SetParameterBlockConstant( C_submaps[trajectory_id].back().data()); - } else { - C_submaps[trajectory_id].push_back( - FromPose(submap_data_[trajectory_id][submap_index].pose)); - problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3); } } } @@ -163,7 +174,9 @@ void OptimizationProblem::Solve(const std::vector& constraints) { ? new ceres::HuberLoss(options_.huber_scale()) : nullptr, C_submaps.at(constraint.submap_id.trajectory_id) - .at(constraint.submap_id.submap_index) + .at(constraint.submap_id.submap_index - + trajectory_data_.at(constraint.submap_id.trajectory_id) + .num_trimmed_submaps) .data(), C_nodes.at(constraint.node_id.trajectory_id) .at(constraint.node_id.node_index - @@ -207,10 +220,10 @@ void OptimizationProblem::Solve(const std::vector& constraints) { // Store the result. for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); ++trajectory_id) { - for (size_t submap_index = 0; - submap_index != submap_data_[trajectory_id].size(); ++submap_index) { - submap_data_[trajectory_id][submap_index].pose = - ToPose(C_submaps[trajectory_id][submap_index]); + for (size_t submap_data_index = 0; + submap_data_index != submap_data_[trajectory_id].size(); ++submap_data_index) { + submap_data_[trajectory_id][submap_data_index].pose = + ToPose(C_submaps[trajectory_id][submap_data_index]); } } for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); @@ -229,7 +242,7 @@ const std::vector>& OptimizationProblem::node_data() return node_data_; } -const std::vector>& OptimizationProblem::submap_data() +const std::vector>& OptimizationProblem::submap_data() const { return submap_data_; } @@ -238,6 +251,10 @@ int OptimizationProblem::num_trimmed_nodes(int trajectory_id) const { return trajectory_data_.at(trajectory_id).num_trimmed_nodes; } +int OptimizationProblem::num_trimmed_submaps(int trajectory_id) const { + return trajectory_data_.at(trajectory_id).num_trimmed_submaps; +} + } // namespace sparse_pose_graph } // namespace mapping_2d } // namespace cartographer diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index 06f4c94..5683c5f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -65,6 +65,7 @@ class OptimizationProblem { const transform::Rigid2d& point_cloud_pose); void TrimTrajectoryNode(const mapping::NodeId& node_id); void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose); + void TrimSubmap(const mapping::SubmapId& submap_id); void SetMaxNumIterations(int32 max_num_iterations); @@ -72,19 +73,21 @@ class OptimizationProblem { void Solve(const std::vector& constraints); const std::vector>& node_data() const; - const std::vector>& submap_data() const; + const std::vector>& submap_data() const; int num_trimmed_nodes(int trajectory_id) const; + int num_trimmed_submaps(int trajectory_id) const; private: struct TrajectoryData { // TODO(hrapp): Remove, once we can relabel constraints. int num_trimmed_nodes = 0; + int num_trimmed_submaps = 0; }; mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; std::vector> imu_data_; std::vector> node_data_; - std::vector> submap_data_; + std::vector> submap_data_; std::vector trajectory_data_; };