From 196b4b891cec5331a56393996505db9a7b714745 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Mon, 8 Jan 2018 09:13:51 +0100 Subject: [PATCH] Add PoseGraphInterface::GetTrajectoryNodePoses() (#795) --- cartographer/mapping/pose_graph_interface.h | 3 +++ cartographer/mapping/trajectory_node.h | 8 ++++++++ cartographer/mapping_2d/pose_graph.cc | 13 +++++++++++++ cartographer/mapping_2d/pose_graph.h | 2 ++ cartographer/mapping_3d/pose_graph.cc | 13 +++++++++++++ cartographer/mapping_3d/pose_graph.h | 2 ++ cartographer_grpc/mapping/pose_graph_stub.cc | 6 ++++++ cartographer_grpc/mapping/pose_graph_stub.h | 3 +++ 8 files changed, 50 insertions(+) diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h index 2db3b21..03083ef 100644 --- a/cartographer/mapping/pose_graph_interface.h +++ b/cartographer/mapping/pose_graph_interface.h @@ -82,6 +82,9 @@ class PoseGraphInterface { // Returns the current optimized trajectories. virtual MapById GetTrajectoryNodes() = 0; + // Returns the current optimized trajectory poses. + virtual MapById GetTrajectoryNodePoses() = 0; + // Checks if the given trajectory is finished. virtual bool IsTrajectoryFinished(int trajectory_id) = 0; diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index dea4d32..e0ee709 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -29,6 +29,14 @@ namespace cartographer { namespace mapping { +struct TrajectoryNodePose { + // Indicates whether the node has constant data set. + bool has_constant_data; + + // The node pose in the global SLAM frame. + transform::Rigid3d global_pose; +}; + struct TrajectoryNode { struct Data { common::Time time; diff --git a/cartographer/mapping_2d/pose_graph.cc b/cartographer/mapping_2d/pose_graph.cc index 787c5ab..7579b46 100644 --- a/cartographer/mapping_2d/pose_graph.cc +++ b/cartographer/mapping_2d/pose_graph.cc @@ -575,6 +575,19 @@ PoseGraph::GetTrajectoryNodes() { return trajectory_nodes_; } +mapping::MapById +PoseGraph::GetTrajectoryNodePoses() { + mapping::MapById node_poses; + common::MutexLocker locker(&mutex_); + for (const auto& node_id_data : trajectory_nodes_) { + node_poses.Insert( + node_id_data.id, + mapping::TrajectoryNodePose{node_id_data.data.constant_data != nullptr, + node_id_data.data.global_pose}); + } + return node_poses; +} + sensor::MapByTime PoseGraph::GetImuData() { common::MutexLocker locker(&mutex_); return optimization_problem_.imu_data(); diff --git a/cartographer/mapping_2d/pose_graph.h b/cartographer/mapping_2d/pose_graph.h index 0514794..1b39d51 100644 --- a/cartographer/mapping_2d/pose_graph.h +++ b/cartographer/mapping_2d/pose_graph.h @@ -110,6 +110,8 @@ class PoseGraph : public mapping::PoseGraph { EXCLUDES(mutex_) override; mapping::MapById GetTrajectoryNodes() override EXCLUDES(mutex_); + mapping::MapById + GetTrajectoryNodePoses() override EXCLUDES(mutex_); sensor::MapByTime GetImuData() override EXCLUDES(mutex_); sensor::MapByTime GetOdometryData() override EXCLUDES(mutex_); diff --git a/cartographer/mapping_3d/pose_graph.cc b/cartographer/mapping_3d/pose_graph.cc index 7e430d1..396e11c 100644 --- a/cartographer/mapping_3d/pose_graph.cc +++ b/cartographer/mapping_3d/pose_graph.cc @@ -605,6 +605,19 @@ PoseGraph::GetTrajectoryNodes() { return trajectory_nodes_; } +mapping::MapById +PoseGraph::GetTrajectoryNodePoses() { + mapping::MapById node_poses; + common::MutexLocker locker(&mutex_); + for (const auto& node_id_data : trajectory_nodes_) { + node_poses.Insert( + node_id_data.id, + mapping::TrajectoryNodePose{node_id_data.data.constant_data != nullptr, + node_id_data.data.global_pose}); + } + return node_poses; +} + sensor::MapByTime PoseGraph::GetImuData() { common::MutexLocker locker(&mutex_); return optimization_problem_.imu_data(); diff --git a/cartographer/mapping_3d/pose_graph.h b/cartographer/mapping_3d/pose_graph.h index a0863c4..ebc79f4 100644 --- a/cartographer/mapping_3d/pose_graph.h +++ b/cartographer/mapping_3d/pose_graph.h @@ -110,6 +110,8 @@ class PoseGraph : public mapping::PoseGraph { EXCLUDES(mutex_) override; mapping::MapById GetTrajectoryNodes() override EXCLUDES(mutex_); + mapping::MapById + GetTrajectoryNodePoses() override EXCLUDES(mutex_); sensor::MapByTime GetImuData() override EXCLUDES(mutex_); sensor::MapByTime GetOdometryData() override EXCLUDES(mutex_); diff --git a/cartographer_grpc/mapping/pose_graph_stub.cc b/cartographer_grpc/mapping/pose_graph_stub.cc index bbb277c..1bcd5ff 100644 --- a/cartographer_grpc/mapping/pose_graph_stub.cc +++ b/cartographer_grpc/mapping/pose_graph_stub.cc @@ -52,6 +52,12 @@ PoseGraphStub::GetTrajectoryNodes() { LOG(FATAL) << "Not implemented"; } +cartographer::mapping::MapById +PoseGraphStub::GetTrajectoryNodePoses() { + LOG(FATAL) << "Not implemented"; +} + bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) { LOG(FATAL) << "Not implemented"; } diff --git a/cartographer_grpc/mapping/pose_graph_stub.h b/cartographer_grpc/mapping/pose_graph_stub.h index f063c48..e85e1f0 100644 --- a/cartographer_grpc/mapping/pose_graph_stub.h +++ b/cartographer_grpc/mapping/pose_graph_stub.h @@ -42,6 +42,9 @@ class PoseGraphStub : public cartographer::mapping::PoseGraphInterface { cartographer::mapping::MapById GetTrajectoryNodes() override; + cartographer::mapping::MapById + GetTrajectoryNodePoses() override; bool IsTrajectoryFinished(int trajectory_id) override; std::vector constraints() override;