12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add PoseGraphInterface::GetTrajectoryNodePoses() (#795)

master
Christoph Schütte 2018-01-08 09:13:51 +01:00 committed by Wally B. Feed
parent 1a837ef3ab
commit 196b4b891c
8 changed files with 50 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer/mapping/pose_graph_interface.h
View File

@ -82,6 +82,9 @@ class PoseGraphInterface {
// Returns the current optimized trajectories.
virtual MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() = 0;
// Returns the current optimized trajectory poses.
virtual MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() = 0;
// Checks if the given trajectory is finished.
virtual bool IsTrajectoryFinished(int trajectory_id) = 0;

8
cartographer/mapping/trajectory_node.h
View File

@ -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;

13
cartographer/mapping_2d/pose_graph.cc
View File

@ -575,6 +575,19 @@ PoseGraph::GetTrajectoryNodes() {
return trajectory_nodes_;
}
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
PoseGraph::GetTrajectoryNodePoses() {
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> 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<sensor::ImuData> PoseGraph::GetImuData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_.imu_data();

2
cartographer/mapping_2d/pose_graph.h
View File

@ -110,6 +110,8 @@ class PoseGraph : public mapping::PoseGraph {
EXCLUDES(mutex_) override;
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
GetTrajectoryNodes() override EXCLUDES(mutex_);
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
GetTrajectoryNodePoses() override EXCLUDES(mutex_);
sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
EXCLUDES(mutex_);

13
cartographer/mapping_3d/pose_graph.cc
View File

@ -605,6 +605,19 @@ PoseGraph::GetTrajectoryNodes() {
return trajectory_nodes_;
}
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
PoseGraph::GetTrajectoryNodePoses() {
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> 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<sensor::ImuData> PoseGraph::GetImuData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_.imu_data();

2
cartographer/mapping_3d/pose_graph.h
View File

@ -110,6 +110,8 @@ class PoseGraph : public mapping::PoseGraph {
EXCLUDES(mutex_) override;
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
GetTrajectoryNodes() override EXCLUDES(mutex_);
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
GetTrajectoryNodePoses() override EXCLUDES(mutex_);
sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
EXCLUDES(mutex_);

6
cartographer_grpc/mapping/pose_graph_stub.cc
View File

@ -52,6 +52,12 @@ PoseGraphStub::GetTrajectoryNodes() {
LOG(FATAL) << "Not implemented";
}
cartographer::mapping::MapById<cartographer::mapping::NodeId,
cartographer::mapping::TrajectoryNodePose>
PoseGraphStub::GetTrajectoryNodePoses() {
LOG(FATAL) << "Not implemented";
}
bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) {
LOG(FATAL) << "Not implemented";
}

3
cartographer_grpc/mapping/pose_graph_stub.h
View File

@ -42,6 +42,9 @@ class PoseGraphStub : public cartographer::mapping::PoseGraphInterface {
cartographer::mapping::MapById<cartographer::mapping::NodeId,
cartographer::mapping::TrajectoryNode>
GetTrajectoryNodes() override;
cartographer::mapping::MapById<cartographer::mapping::NodeId,
cartographer::mapping::TrajectoryNodePose>
GetTrajectoryNodePoses() override;
bool IsTrajectoryFinished(int trajectory_id) override;
std::vector<Constraint> constraints() override;