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

View File

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

View File

@ -29,6 +29,14 @@
namespace cartographer { namespace cartographer {
namespace mapping { 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 TrajectoryNode {
struct Data { struct Data {
common::Time time; common::Time time;

View File

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

View File

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

View File

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

View File

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

View File

@ -52,6 +52,12 @@ PoseGraphStub::GetTrajectoryNodes() {
LOG(FATAL) << "Not implemented"; 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) { bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) {
LOG(FATAL) << "Not implemented"; LOG(FATAL) << "Not implemented";
} }

View File

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