From f179bd942cf523be90332b28136a7111bd729b76 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Fri, 20 Oct 2017 16:13:18 +0200 Subject: [PATCH] Support multiple trajectories in LoadMap. (#567) --- cartographer/mapping/id.h | 2 + cartographer/mapping/map_builder.cc | 22 +++++++--- cartographer/mapping/sparse_pose_graph.h | 14 +++---- cartographer/mapping_2d/sparse_pose_graph.cc | 40 +++++++++---------- cartographer/mapping_2d/sparse_pose_graph.h | 5 +-- .../sparse_pose_graph/optimization_problem.cc | 14 +++++++ .../sparse_pose_graph/optimization_problem.h | 6 +++ cartographer/mapping_3d/sparse_pose_graph.cc | 37 +++++++++-------- cartographer/mapping_3d/sparse_pose_graph.h | 5 +-- .../sparse_pose_graph/optimization_problem.cc | 18 +++++++++ .../sparse_pose_graph/optimization_problem.h | 5 +++ 11 files changed, 109 insertions(+), 59 deletions(-) diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h index 38fdd80..1f772ef 100644 --- a/cartographer/mapping/id.h +++ b/cartographer/mapping/id.h @@ -249,6 +249,8 @@ class MapById { // Inserts data (which must not exist already) into a trajectory. void Insert(const IdType& id, const DataType& data) { + CHECK_GE(id.trajectory_id, 0); + CHECK_GE(GetIndex(id), 0); auto& trajectory = trajectories_[id.trajectory_id]; trajectory.can_append_ = false; CHECK(trajectory.data_.emplace(GetIndex(id), data).second); diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 50e15ee..adf9815 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -177,8 +177,16 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) { proto::SparsePoseGraph pose_graph; CHECK(reader->ReadProto(&pose_graph)); - const int map_trajectory_id = AddTrajectoryForDeserialization(); - sparse_pose_graph_->FreezeTrajectory(map_trajectory_id); + std::map trajectory_remapping; + for (auto& trajectory_proto : *pose_graph.mutable_trajectory()) { + const int new_trajectory_id = AddTrajectoryForDeserialization(); + CHECK(trajectory_remapping + .emplace(trajectory_proto.trajectory_id(), new_trajectory_id) + .second) + << "Duplicate trajectory ID: " << trajectory_proto.trajectory_id(); + trajectory_proto.set_trajectory_id(new_trajectory_id); + sparse_pose_graph_->FreezeTrajectory(new_trajectory_id); + } MapById submap_poses; for (const proto::Trajectory& trajectory_proto : pose_graph.trajectory()) { @@ -205,18 +213,20 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) { break; } if (proto.has_node()) { + proto.mutable_node()->mutable_node_id()->set_trajectory_id( + trajectory_remapping.at(proto.node().node_id().trajectory_id())); const transform::Rigid3d node_pose = node_poses.at(NodeId{proto.node().node_id().trajectory_id(), proto.node().node_id().node_index()}); - sparse_pose_graph_->AddNodeFromProto(map_trajectory_id, node_pose, - proto.node()); + sparse_pose_graph_->AddNodeFromProto(node_pose, proto.node()); } if (proto.has_submap()) { + proto.mutable_submap()->mutable_submap_id()->set_trajectory_id( + trajectory_remapping.at(proto.submap().submap_id().trajectory_id())); const transform::Rigid3d submap_pose = submap_poses.at(SubmapId{proto.submap().submap_id().trajectory_id(), proto.submap().submap_id().submap_index()}); - sparse_pose_graph_->AddSubmapFromProto(map_trajectory_id, submap_pose, - proto.submap()); + sparse_pose_graph_->AddSubmapFromProto(submap_pose, proto.submap()); } } CHECK(reader->eof()); diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 5919c95..576410b 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -77,16 +77,14 @@ class SparsePoseGraph { // Freezes a trajectory. Poses in this trajectory will not be optimized. virtual void FreezeTrajectory(int trajectory_id) = 0; - // Adds a 'submap' from a proto with the given 'initial_pose' to the frozen - // trajectory with 'trajectory_id'. - virtual void AddSubmapFromProto(int trajectory_id, - const transform::Rigid3d& initial_pose, + // Adds a 'submap' from a proto with the given 'global_pose' to the + // appropriate frozen trajectory. + virtual void AddSubmapFromProto(const transform::Rigid3d& global_pose, const proto::Submap& submap) = 0; - // Adds a 'node' from a proto with the given 'pose' to the frozen trajectory - // with 'trajectory_id'. - virtual void AddNodeFromProto(int trajectory_id, - const transform::Rigid3d& pose, + // Adds a 'node' from a proto with the given 'global_pose' to the + // appropriate frozen trajectory. + virtual void AddNodeFromProto(const transform::Rigid3d& global_pose, const proto::Node& node) = 0; // Adds a 'trimmer'. It will be used after all data added before it has been diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 581414f..b1279b1 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -373,55 +373,55 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) { }); } -void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id, - const transform::Rigid3d& initial_pose, +void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose, const mapping::proto::Submap& submap) { if (!submap.has_submap_2d()) { return; } + const mapping::SubmapId submap_id = {submap.submap_id().trajectory_id(), + submap.submap_id().submap_index()}; std::shared_ptr submap_ptr = std::make_shared(submap.submap_2d()); - const transform::Rigid2d initial_pose_2d = transform::Project2D(initial_pose); + const transform::Rigid2d global_pose_2d = transform::Project2D(global_pose); common::MutexLocker locker(&mutex_); - AddTrajectoryIfNeeded(trajectory_id); - const mapping::SubmapId submap_id = - submap_data_.Append(trajectory_id, SubmapData()); + AddTrajectoryIfNeeded(submap_id.trajectory_id); + submap_data_.Insert(submap_id, SubmapData()); submap_data_.at(submap_id).submap = submap_ptr; // Immediately show the submap at the optimized pose. - CHECK_EQ(submap_id, - optimized_submap_transforms_.Append( - trajectory_id, sparse_pose_graph::SubmapData{initial_pose_2d})); - AddWorkItem([this, submap_id, initial_pose_2d]() REQUIRES(mutex_) { + optimized_submap_transforms_.Insert( + submap_id, sparse_pose_graph::SubmapData{global_pose_2d}); + AddWorkItem([this, submap_id, global_pose_2d]() REQUIRES(mutex_) { CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1); submap_data_.at(submap_id).state = SubmapState::kFinished; - optimization_problem_.AddSubmap(submap_id.trajectory_id, initial_pose_2d); + optimization_problem_.InsertSubmap(submap_id, global_pose_2d); }); } -void SparsePoseGraph::AddNodeFromProto(const int trajectory_id, - const transform::Rigid3d& pose, +void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose, const mapping::proto::Node& node) { + const mapping::NodeId node_id = {node.node_id().trajectory_id(), + node.node_id().node_index()}; std::shared_ptr constant_data = std::make_shared( mapping::FromProto(node.node_data())); common::MutexLocker locker(&mutex_); - AddTrajectoryIfNeeded(trajectory_id); - const mapping::NodeId node_id = trajectory_nodes_.Append( - trajectory_id, mapping::TrajectoryNode{constant_data, pose}); + AddTrajectoryIfNeeded(node_id.trajectory_id); + trajectory_nodes_.Insert(node_id, + mapping::TrajectoryNode{constant_data, global_pose}); - AddWorkItem([this, node_id, pose]() REQUIRES(mutex_) { + AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) { CHECK_EQ(frozen_trajectories_.count(node_id.trajectory_id), 1); const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const auto gravity_alignment_inverse = transform::Rigid3d::Rotation( constant_data->gravity_alignment.inverse()); - optimization_problem_.AddTrajectoryNode( - node_id.trajectory_id, constant_data->time, + optimization_problem_.InsertTrajectoryNode( + node_id, constant_data->time, transform::Project2D(constant_data->local_pose * gravity_alignment_inverse), - transform::Project2D(pose * gravity_alignment_inverse), + transform::Project2D(global_pose * gravity_alignment_inverse), constant_data->gravity_alignment); }); } diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index db3fa75..b0b60fb 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -84,10 +84,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const sensor::FixedFramePoseData& fixed_frame_pose_data); void FreezeTrajectory(int trajectory_id) override; - void AddSubmapFromProto(int trajectory_id, - const transform::Rigid3d& initial_pose, + void AddSubmapFromProto(const transform::Rigid3d& global_pose, const mapping::proto::Submap& submap) override; - void AddNodeFromProto(int trajectory_id, const transform::Rigid3d& pose, + void AddNodeFromProto(const transform::Rigid3d& global_pose, const mapping::proto::Node& node) override; void AddTrimmer(std::unique_ptr trimmer) override; void RunFinalOptimization() override; diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 34a3276..a09d3c0 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -86,6 +86,15 @@ void OptimizationProblem::AddTrajectoryNode( NodeData{time, initial_pose, pose, gravity_alignment}); } +void OptimizationProblem::InsertTrajectoryNode( + const mapping::NodeId& node_id, const common::Time time, + const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose, + const Eigen::Quaterniond& gravity_alignment) { + node_data_.Insert(node_id, + NodeData{time, initial_pose, pose, gravity_alignment}); +} + + void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { node_data_.Trim(node_id); @@ -106,6 +115,11 @@ void OptimizationProblem::AddSubmap(const int trajectory_id, submap_data_.Append(trajectory_id, SubmapData{submap_pose}); } +void OptimizationProblem::InsertSubmap(const mapping::SubmapId& submap_id, + const transform::Rigid2d& submap_pose) { + submap_data_.Insert(submap_id, SubmapData{submap_pose}); +} + void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) { submap_data_.Trim(submap_id); } diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index 2b45850..2a286f4 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -69,8 +69,14 @@ class OptimizationProblem { const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose, const Eigen::Quaterniond& gravity_alignment); + void InsertTrajectoryNode(const mapping::NodeId& node_id, common::Time time, + const transform::Rigid2d& initial_pose, + const transform::Rigid2d& pose, + const Eigen::Quaterniond& gravity_alignment); void TrimTrajectoryNode(const mapping::NodeId& node_id); void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose); + void InsertSubmap(const mapping::SubmapId& submap_id, + const transform::Rigid2d& submap_pose); void TrimSubmap(const mapping::SubmapId& submap_id); void SetMaxNumIterations(int32 max_num_iterations); diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 793d0af..a23371b 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -393,50 +393,49 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) { }); } -void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id, - const transform::Rigid3d& initial_pose, +void SparsePoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_pose, const mapping::proto::Submap& submap) { if (!submap.has_submap_3d()) { return; } + const mapping::SubmapId submap_id = {submap.submap_id().trajectory_id(), + submap.submap_id().submap_index()}; std::shared_ptr submap_ptr = std::make_shared(submap.submap_3d()); common::MutexLocker locker(&mutex_); - AddTrajectoryIfNeeded(trajectory_id); - const mapping::SubmapId submap_id = - submap_data_.Append(trajectory_id, SubmapData()); + AddTrajectoryIfNeeded(submap_id.trajectory_id); + submap_data_.Insert(submap_id, SubmapData()); submap_data_.at(submap_id).submap = submap_ptr; // Immediately show the submap at the optimized 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_) { + optimized_submap_transforms_.Insert( + submap_id, sparse_pose_graph::SubmapData{global_pose}); + AddWorkItem([this, submap_id, global_pose]() REQUIRES(mutex_) { CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1); submap_data_.at(submap_id).state = SubmapState::kFinished; - optimization_problem_.AddSubmap(submap_id.trajectory_id, initial_pose); + optimization_problem_.InsertSubmap(submap_id, global_pose); }); } -void SparsePoseGraph::AddNodeFromProto(const int trajectory_id, - const transform::Rigid3d& pose, +void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose, const mapping::proto::Node& node) { + const mapping::NodeId node_id = {node.node_id().trajectory_id(), + node.node_id().node_index()}; std::shared_ptr constant_data = std::make_shared( mapping::FromProto(node.node_data())); common::MutexLocker locker(&mutex_); - AddTrajectoryIfNeeded(trajectory_id); - const mapping::NodeId node_id = trajectory_nodes_.Append( - trajectory_id, mapping::TrajectoryNode{constant_data, pose}); + AddTrajectoryIfNeeded(node_id.trajectory_id); + trajectory_nodes_.Insert(node_id, + mapping::TrajectoryNode{constant_data, global_pose}); - AddWorkItem([this, node_id, pose]() REQUIRES(mutex_) { + AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) { CHECK_EQ(frozen_trajectories_.count(node_id.trajectory_id), 1); const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; - optimization_problem_.AddTrajectoryNode(node_id.trajectory_id, - constant_data->time, - constant_data->local_pose, pose); + optimization_problem_.InsertTrajectoryNode( + node_id, constant_data->time, constant_data->local_pose, global_pose); }); } diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 204dd79..1b24980 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -84,10 +84,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const sensor::FixedFramePoseData& fixed_frame_pose_data); void FreezeTrajectory(int trajectory_id) override; - void AddSubmapFromProto(int trajectory_id, - const transform::Rigid3d& initial_pose, + void AddSubmapFromProto(const transform::Rigid3d& global_pose, const mapping::proto::Submap& submap) override; - void AddNodeFromProto(int trajectory_id, const transform::Rigid3d& pose, + void AddNodeFromProto(const transform::Rigid3d& global_pose, const mapping::proto::Node& node) override; void AddTrimmer(std::unique_ptr trimmer) override; void RunFinalOptimization() override; diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index 5b9e7a5..58814c5 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -89,6 +89,15 @@ void OptimizationProblem::AddTrajectoryNode( node_data_.Append(trajectory_id, NodeData{time, initial_pose, pose}); } +void OptimizationProblem::InsertTrajectoryNode( + const mapping::NodeId& node_id, const common::Time time, + const transform::Rigid3d& initial_pose, const transform::Rigid3d& pose) { + CHECK_GE(node_id.trajectory_id, 0); + trajectory_data_.resize(std::max( + trajectory_data_.size(), static_cast(node_id.trajectory_id) + 1)); + node_data_.Insert(node_id, NodeData{time, initial_pose, pose}); +} + void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { node_data_.Trim(node_id); @@ -112,6 +121,15 @@ void OptimizationProblem::AddSubmap(const int trajectory_id, submap_data_.Append(trajectory_id, SubmapData{submap_pose}); } +void OptimizationProblem::InsertSubmap(const mapping::SubmapId& submap_id, + const transform::Rigid3d& submap_pose) { + CHECK_GE(submap_id.trajectory_id, 0); + trajectory_data_.resize( + std::max(trajectory_data_.size(), + static_cast(submap_id.trajectory_id) + 1)); + submap_data_.Insert(submap_id, SubmapData{submap_pose}); +} + void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) { submap_data_.Trim(submap_id); } diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index 15ad265..c205ece 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -74,8 +74,13 @@ class OptimizationProblem { void AddTrajectoryNode(int trajectory_id, common::Time time, const transform::Rigid3d& initial_pose, const transform::Rigid3d& pose); + void InsertTrajectoryNode(const mapping::NodeId& node_id, common::Time time, + const transform::Rigid3d& initial_pose, + const transform::Rigid3d& pose); void TrimTrajectoryNode(const mapping::NodeId& node_id); void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose); + void InsertSubmap(const mapping::SubmapId& submap_id, + const transform::Rigid3d& submap_pose); void TrimSubmap(const mapping::SubmapId& submap_id); void SetMaxNumIterations(int32 max_num_iterations);