From 088681f9eccfb31bd3728295697710104bac9ffb Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 7 Nov 2017 11:26:41 +0100 Subject: [PATCH] Add 3D localization support. (#632) This adds information about which nodes are contained in which deserialized submaps. This is necessary for the rotational scan matcher. Also skips frozen trajectories when tying together nodes of a trajectory with IMU data which is unavailable in this case. --- cartographer/mapping/map_builder.cc | 16 ++++++++++++++++ cartographer/mapping/sparse_pose_graph.h | 5 +++++ cartographer/mapping_2d/sparse_pose_graph.cc | 8 ++++++++ cartographer/mapping_2d/sparse_pose_graph.h | 2 ++ cartographer/mapping_3d/sparse_pose_graph.cc | 8 ++++++++ cartographer/mapping_3d/sparse_pose_graph.h | 2 ++ .../sparse_pose_graph/optimization_problem.cc | 5 +++++ 7 files changed, 46 insertions(+) diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index adf9815..183e202 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -229,6 +229,22 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) { sparse_pose_graph_->AddSubmapFromProto(submap_pose, proto.submap()); } } + + // Add information about which nodes belong to which submap. + for (const proto::SparsePoseGraph::Constraint& constraint_proto : + pose_graph.constraint()) { + if (constraint_proto.tag() != + mapping::proto::SparsePoseGraph::Constraint::INTRA_SUBMAP) { + continue; + } + const NodeId node_id{ + trajectory_remapping.at(constraint_proto.node_id().trajectory_id()), + constraint_proto.node_id().node_index()}; + const SubmapId submap_id{ + trajectory_remapping.at(constraint_proto.submap_id().trajectory_id()), + constraint_proto.submap_id().submap_index()}; + sparse_pose_graph_->AddNodeToSubmap(node_id, submap_id); + } CHECK(reader->eof()); } diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index e5f0ca6..99acf63 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -87,6 +87,11 @@ class SparsePoseGraph { virtual void AddNodeFromProto(const transform::Rigid3d& global_pose, const proto::Node& node) = 0; + // Adds information that 'node_id' was inserted into 'submap_id'. The submap + // has to be deserialized first. + virtual void AddNodeToSubmap(const NodeId& node_id, + const SubmapId& submap_id) = 0; + // Adds serialized constraints. The corresponding trajectory nodes and submaps // have to be deserialized before calling this function. virtual void AddSerializedConstraints( diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 8ba549e..645b6e2 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -427,6 +427,14 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose, }); } +void SparsePoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id, + const mapping::SubmapId& submap_id) { + common::MutexLocker locker(&mutex_); + AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) { + submap_data_.at(submap_id).node_ids.insert(node_id); + }); +} + void SparsePoseGraph::AddSerializedConstraints( const std::vector& constraints) { common::MutexLocker locker(&mutex_); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 8b2f8ec..eeadc20 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -88,6 +88,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const mapping::proto::Submap& submap) override; void AddNodeFromProto(const transform::Rigid3d& global_pose, const mapping::proto::Node& node) override; + void AddNodeToSubmap(const mapping::NodeId& node_id, + const mapping::SubmapId& submap_id) override; void AddSerializedConstraints( const std::vector& constraints) override; void AddTrimmer(std::unique_ptr trimmer) override; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index ce0bb18..d7b42f0 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -441,6 +441,14 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose, }); } +void SparsePoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id, + const mapping::SubmapId& submap_id) { + common::MutexLocker locker(&mutex_); + AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) { + submap_data_.at(submap_id).node_ids.insert(node_id); + }); +} + void SparsePoseGraph::AddSerializedConstraints( const std::vector& constraints) { common::MutexLocker locker(&mutex_); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 19457cf..27117e4 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -88,6 +88,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const mapping::proto::Submap& submap) override; void AddNodeFromProto(const transform::Rigid3d& global_pose, const mapping::proto::Node& node) override; + void AddNodeToSubmap(const mapping::NodeId& node_id, + const mapping::SubmapId& submap_id) override; void AddSerializedConstraints( const std::vector& constraints) override; void AddTrimmer(std::unique_ptr trimmer) override; diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index 082b09f..9597e11 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -216,6 +216,11 @@ void OptimizationProblem::Solve(const std::vector& constraints, for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { const int trajectory_id = node_it->id.trajectory_id; const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); + if (frozen_trajectories.count(trajectory_id) != 0) { + // We skip frozen trajectories. + node_it = trajectory_end; + continue; + } TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id); problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4,