diff --git a/cartographer/mapping_3d/ceres_pose.cc b/cartographer/mapping_3d/ceres_pose.cc index b1d940f..659e4f3 100644 --- a/cartographer/mapping_3d/ceres_pose.cc +++ b/cartographer/mapping_3d/ceres_pose.cc @@ -24,21 +24,22 @@ CeresPose::CeresPose( std::unique_ptr translation_parametrization, std::unique_ptr rotation_parametrization, ceres::Problem* problem) - : translation_({{rigid.translation().x(), rigid.translation().y(), - rigid.translation().z()}}), - rotation_({{rigid.rotation().w(), rigid.rotation().x(), - rigid.rotation().y(), rigid.rotation().z()}}) { - problem->AddParameterBlock(translation_.data(), 3, + : data_(std::make_shared( + CeresPose::Data{{{rigid.translation().x(), rigid.translation().y(), + rigid.translation().z()}}, + {{rigid.rotation().w(), rigid.rotation().x(), + rigid.rotation().y(), rigid.rotation().z()}}})) { + problem->AddParameterBlock(data_->translation.data(), 3, translation_parametrization.release()); - problem->AddParameterBlock(rotation_.data(), 4, + problem->AddParameterBlock(data_->rotation.data(), 4, rotation_parametrization.release()); } const transform::Rigid3d CeresPose::ToRigid() const { + const auto& rotation = data_->rotation; return transform::Rigid3d( - Eigen::Map(translation_.data()), - Eigen::Quaterniond(rotation_[0], rotation_[1], rotation_[2], - rotation_[3])); + Eigen::Map(data_->translation.data()), + Eigen::Quaterniond(rotation[0], rotation[1], rotation[2], rotation[3])); } } // namespace mapping_3d diff --git a/cartographer/mapping_3d/ceres_pose.h b/cartographer/mapping_3d/ceres_pose.h index 7dd29bb..b5f5230 100644 --- a/cartographer/mapping_3d/ceres_pose.h +++ b/cartographer/mapping_3d/ceres_pose.h @@ -35,18 +35,18 @@ class CeresPose { std::unique_ptr rotation_parametrization, ceres::Problem* problem); - CeresPose(const CeresPose&) = delete; - CeresPose& operator=(const CeresPose&) = delete; - const transform::Rigid3d ToRigid() const; - double* translation() { return translation_.data(); } - double* rotation() { return rotation_.data(); } + double* translation() { return data_->translation.data(); } + double* rotation() { return data_->rotation.data(); } private: - std::array translation_; - // Rotation quaternion as (w, x, y, z). - std::array rotation_; + struct Data { + std::array translation; + // Rotation quaternion as (w, x, y, z). + std::array rotation; + }; + std::shared_ptr data_; }; } // namespace mapping_3d diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 169b4c4..29f628d 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -59,25 +59,23 @@ std::vector SparsePoseGraph::GrowSubmapTransformsAsNeeded( const auto& submap_data = optimization_problem_.submap_data(); if (insertion_submaps.size() == 1) { // If we don't already have an entry for the first submap, add one. - if (static_cast(trajectory_id) >= submap_data.size() || - submap_data[trajectory_id].empty()) { + if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) { optimization_problem_.AddSubmap(trajectory_id, insertion_submaps[0]->local_pose()); } - CHECK_EQ(submap_data[trajectory_id].size(), 1); + CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id)); 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()); - CHECK(!submap_data.at(trajectory_id).empty()); - const mapping::SubmapId last_submap_id{ - trajectory_id, submap_data.at(trajectory_id).rbegin()->first}; + const auto end_it = submap_data.EndOfTrajectory(trajectory_id); + CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it); + const mapping::SubmapId last_submap_id = std::prev(end_it)->id; 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; + const auto& first_submap_pose = submap_data.at(last_submap_id).pose; optimization_problem_.AddSubmap( trajectory_id, first_submap_pose * insertion_submaps[0]->local_pose().inverse() * @@ -177,10 +175,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished); const transform::Rigid3d inverse_submap_pose = - optimization_problem_.submap_data() - .at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .pose.inverse(); + optimization_problem_.submap_data().at(submap_id).pose.inverse(); const transform::Rigid3d initial_relative_pose = inverse_submap_pose * optimization_problem_.node_data() @@ -273,10 +268,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const transform::Rigid3d& pose = constant_data->initial_pose; const transform::Rigid3d optimized_pose = - optimization_problem_.submap_data() - .at(matching_id.trajectory_id) - .at(matching_id.submap_index) - .pose * + optimization_problem_.submap_data().at(matching_id).pose * insertion_submaps.front()->local_pose().inverse() * pose; optimization_problem_.AddTrajectoryNode( matching_id.trajectory_id, constant_data->time, pose, optimized_pose); @@ -446,14 +438,9 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id, submap_data_.Append(trajectory_id, SubmapData()); submap_data_.at(submap_id).submap = submap_ptr; // Immediately show the submap at the optimized pose. - CHECK_GE(static_cast(submap_data_.num_trajectories()), - optimized_submap_transforms_.size()); - optimized_submap_transforms_.resize(submap_data_.num_trajectories()); - CHECK_EQ(optimized_submap_transforms_.at(trajectory_id).size(), - submap_id.submap_index); - optimized_submap_transforms_.at(trajectory_id) - .emplace(submap_id.submap_index, - sparse_pose_graph::SubmapData{initial_pose}); + CHECK_EQ(submap_id, + optimized_submap_transforms_.Append( + trajectory_id, sparse_pose_graph::SubmapData{initial_pose})); AddWorkItem([this, submap_id, initial_pose]() REQUIRES(mutex_) { CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1); submap_data_.at(submap_id).state = SubmapState::kFinished; @@ -512,9 +499,7 @@ void SparsePoseGraph::LogResidualHistograms() { const cartographer::transform::Rigid3d node_to_submap_constraint = constraint.pose.zbar_ij; const cartographer::transform::Rigid3d optimized_submap_to_map = - optimized_submap_transforms_.at(constraint.submap_id.trajectory_id) - .at(constraint.submap_id.submap_index) - .pose; + optimized_submap_transforms_.at(constraint.submap_id).pose; const cartographer::transform::Rigid3d optimized_node_to_submap = optimized_submap_to_map.inverse() * optimized_node_to_map; const cartographer::transform::Rigid3d residual = @@ -632,18 +617,17 @@ SparsePoseGraph::GetAllSubmapData() { } transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( - const std::vector>& + const mapping::MapById& submap_transforms, const int trajectory_id) const { - if (trajectory_id >= static_cast(submap_transforms.size()) || - submap_transforms.at(trajectory_id).empty()) { + auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id); + auto end_it = submap_transforms.EndOfTrajectory(trajectory_id); + if (begin_it == end_it) { return transform::Rigid3d::Identity(); } - - const int submap_index = submap_transforms.at(trajectory_id).rbegin()->first; - const mapping::SubmapId last_optimized_submap_id{trajectory_id, submap_index}; + const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id; // Accessing 'local_pose' in Submap is okay, since the member is const. - return submap_transforms.at(trajectory_id).at(submap_index).pose * + return submap_transforms.at(last_optimized_submap_id).pose * submap_data_.at(last_optimized_submap_id) .submap->local_pose() .inverse(); @@ -655,15 +639,9 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( return {}; } auto submap = submap_data_.at(submap_id).submap; - 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())) { + if (optimized_submap_transforms_.Contains(submap_id)) { // We already have an optimized pose. - return {submap, optimized_submap_transforms_.at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .pose}; + return {submap, optimized_submap_transforms_.at(submap_id).pose}; } // We have to extrapolate. return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_, @@ -676,7 +654,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(); + const auto& submap_data = parent_->optimization_problem_.submap_data(); + return submap_data.SizeOfTrajectoryOrZero(trajectory_id); } void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index b0a99ea..6c529da 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -161,7 +161,7 @@ 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 mapping::MapById& submap_transforms, int trajectory_id) const REQUIRES(mutex_); @@ -218,7 +218,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Current submap transforms used for displaying data. - std::vector> + mapping::MapById optimized_submap_transforms_ GUARDED_BY(mutex_); // List of all trimmers to consult when optimizations finish. diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index 9e6248f..993f620 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -110,19 +110,13 @@ void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { void OptimizationProblem::AddSubmap(const int trajectory_id, const transform::Rigid3d& submap_pose) { CHECK_GE(trajectory_id, 0); - submap_data_.resize( - std::max(submap_data_.size(), static_cast(trajectory_id) + 1)); - trajectory_data_.resize( - std::max(trajectory_data_.size(), submap_data_.size())); - auto& trajectory_data = trajectory_data_[trajectory_id]; - submap_data_[trajectory_id].emplace(trajectory_data.next_submap_index, - SubmapData{submap_pose}); - ++trajectory_data.next_submap_index; + trajectory_data_.resize(std::max(trajectory_data_.size(), + static_cast(trajectory_id) + 1)); + submap_data_.Append(trajectory_id, SubmapData{submap_pose}); } void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) { - auto& submap_data = submap_data_.at(submap_id.trajectory_id); - CHECK(submap_data.erase(submap_id.submap_index)); + submap_data_.Trim(submap_id); } void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { @@ -150,43 +144,37 @@ void OptimizationProblem::Solve(const std::vector& constraints, // Set the starting point. CHECK(!submap_data_.empty()); - CHECK(!submap_data_[0].empty()); - // TODO(hrapp): Move ceres data into SubmapData. - std::vector> C_submaps(submap_data_.size()); + CHECK(submap_data_.Contains(mapping::SubmapId{0, 0})); + mapping::MapById C_submaps; std::vector> C_nodes(node_data_.size()); bool first_submap = true; - for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); - ++trajectory_id) { - const bool frozen = frozen_trajectories.count(trajectory_id) != 0; - for (const auto& index_submap_data : submap_data_[trajectory_id]) { - const int submap_index = index_submap_data.first; - if (first_submap) { - first_submap = false; - // Fix the first submap of the first trajectory except for allowing - // gravity alignment. - C_submaps[trajectory_id].emplace( - std::piecewise_construct, std::forward_as_tuple(submap_index), - std::forward_as_tuple( - index_submap_data.second.pose, translation_parameterization(), - common::make_unique>(), - &problem)); - problem.SetParameterBlockConstant( - C_submaps[trajectory_id].at(submap_index).translation()); - } else { - C_submaps[trajectory_id].emplace( - std::piecewise_construct, std::forward_as_tuple(submap_index), - std::forward_as_tuple( - index_submap_data.second.pose, translation_parameterization(), - common::make_unique(), - &problem)); - } - if (frozen) { - problem.SetParameterBlockConstant( - C_submaps[trajectory_id].at(submap_index).rotation()); - problem.SetParameterBlockConstant( - C_submaps[trajectory_id].at(submap_index).translation()); - } + for (const auto& submap_id_data : submap_data_) { + const bool frozen = + frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0; + if (first_submap) { + first_submap = false; + // Fix the first submap of the first trajectory except for allowing + // gravity alignment. + C_submaps.Insert( + submap_id_data.id, + CeresPose(submap_id_data.data.pose, translation_parameterization(), + common::make_unique>(), + &problem)); + problem.SetParameterBlockConstant( + C_submaps.at(submap_id_data.id).translation()); + } else { + C_submaps.Insert( + submap_id_data.id, + CeresPose(submap_id_data.data.pose, translation_parameterization(), + common::make_unique(), + &problem)); + } + if (frozen) { + problem.SetParameterBlockConstant( + C_submaps.at(submap_id_data.id).rotation()); + problem.SetParameterBlockConstant( + C_submaps.at(submap_id_data.id).translation()); } } for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); @@ -217,12 +205,8 @@ void OptimizationProblem::Solve(const std::vector& constraints, constraint.tag == Constraint::INTER_SUBMAP ? new ceres::HuberLoss(options_.huber_scale()) : nullptr, - C_submaps.at(constraint.submap_id.trajectory_id) - .at(constraint.submap_id.submap_index) - .rotation(), - C_submaps.at(constraint.submap_id.trajectory_id) - .at(constraint.submap_id.submap_index) - .translation(), + C_submaps.at(constraint.submap_id).rotation(), + C_submaps.at(constraint.submap_id).translation(), C_nodes.at(constraint.node_id.trajectory_id) .at(constraint.node_id.node_index) .rotation(), @@ -446,12 +430,8 @@ void OptimizationProblem::Solve(const std::vector& constraints, } // Store the result. - for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); - ++trajectory_id) { - for (auto& index_submap_data : submap_data_[trajectory_id]) { - index_submap_data.second.pose = - C_submaps[trajectory_id].at(index_submap_data.first).ToRigid(); - } + for (const auto& C_submap_id_data : C_submaps) { + submap_data_.at(C_submap_id_data.id).pose = C_submap_id_data.data.ToRigid(); } for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { @@ -467,8 +447,8 @@ const std::vector>& OptimizationProblem::node_data() return node_data_; } -const std::vector>& OptimizationProblem::submap_data() - const { +const mapping::MapById& +OptimizationProblem::submap_data() const { return submap_data_; } diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index 3657955..bddf617 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -27,6 +27,7 @@ #include "Eigen/Geometry" #include "cartographer/common/port.h" #include "cartographer/common/time.h" +#include "cartographer/mapping/id.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/sensor/fixed_frame_pose_data.h" @@ -84,13 +85,12 @@ class OptimizationProblem { const std::set& frozen_trajectories); const std::vector>& node_data() const; - const std::vector>& submap_data() const; + const mapping::MapById& submap_data() const; private: struct TrajectoryData { double gravity_constant = 9.8; std::array imu_calibration{{1., 0., 0., 0.}}; - int next_submap_index = 0; int next_node_index = 0; }; @@ -99,7 +99,7 @@ class OptimizationProblem { std::vector> imu_data_; std::vector> node_data_; std::vector odometry_data_; - std::vector> submap_data_; + mapping::MapById submap_data_; std::vector trajectory_data_; std::vector fixed_frame_pose_data_; };