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

More const PoseGraph interface (#1156) 

Making getters and non-modifying methods  in PoseGraph* const.

Before most getters where marked non-const because of the need to acquire the mutex for thread safety. This forces all code using the PoseGraph to pass it around as a non-const object even if the consumer is not altering the object.
By making the mutexes mutable we can make getters and methods that do not change the PoseGraph const and thus enable consumer APIs to express how they are going to use the PoseGraph as well.

fixes #1021
master
Sebastian Klose 2018-05-17 09:30:40 +02:00 committed by GitHub
parent a0ec8046bd
commit d419fe8fd7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
13 changed files with 166 additions and 142 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
cartographer/cloud/internal/client/pose_graph_stub.cc
View File

@ -43,12 +43,12 @@ void PoseGraphStub::RunFinalOptimization() {
} }
mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData> mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData>
PoseGraphStub::GetAllSubmapData() { PoseGraphStub::GetAllSubmapData() const {
LOG(FATAL) << "Not implemented"; LOG(FATAL) << "Not implemented";
} }
mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapPose> mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapPose>
PoseGraphStub::GetAllSubmapPoses() { PoseGraphStub::GetAllSubmapPoses() const {
google::protobuf::Empty request; google::protobuf::Empty request;
async_grpc::Client<handlers::GetAllSubmapPosesSignature> client( async_grpc::Client<handlers::GetAllSubmapPosesSignature> client(
client_channel_); client_channel_);
@ -66,7 +66,8 @@ PoseGraphStub::GetAllSubmapPoses() {
return submap_poses; return submap_poses;
} }
transform::Rigid3d PoseGraphStub::GetLocalToGlobalTransform(int trajectory_id) { transform::Rigid3d PoseGraphStub::GetLocalToGlobalTransform(
int trajectory_id) const {
proto::GetLocalToGlobalTransformRequest request; proto::GetLocalToGlobalTransformRequest request;
request.set_trajectory_id(trajectory_id); request.set_trajectory_id(trajectory_id);
async_grpc::Client<handlers::GetLocalToGlobalTransformSignature> client( async_grpc::Client<handlers::GetLocalToGlobalTransformSignature> client(
@ -76,12 +77,12 @@ transform::Rigid3d PoseGraphStub::GetLocalToGlobalTransform(int trajectory_id) {
} }
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode> mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
PoseGraphStub::GetTrajectoryNodes() { PoseGraphStub::GetTrajectoryNodes() const {
LOG(FATAL) << "Not implemented"; LOG(FATAL) << "Not implemented";
} }
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
PoseGraphStub::GetTrajectoryNodePoses() { PoseGraphStub::GetTrajectoryNodePoses() const {
google::protobuf::Empty request; google::protobuf::Empty request;
async_grpc::Client<handlers::GetTrajectoryNodePosesSignature> client( async_grpc::Client<handlers::GetTrajectoryNodePosesSignature> client(
client_channel_); client_channel_);
@ -97,7 +98,8 @@ PoseGraphStub::GetTrajectoryNodePoses() {
return node_poses; return node_poses;
} }
std::map<std::string, transform::Rigid3d> PoseGraphStub::GetLandmarkPoses() { std::map<std::string, transform::Rigid3d> PoseGraphStub::GetLandmarkPoses()
const {
google::protobuf::Empty request; google::protobuf::Empty request;
async_grpc::Client<handlers::GetLandmarkPosesSignature> client( async_grpc::Client<handlers::GetLandmarkPosesSignature> client(
client_channel_); client_channel_);
@ -121,7 +123,7 @@ void PoseGraphStub::SetLandmarkPose(const std::string& landmark_id,
CHECK(client.Write(request)); CHECK(client.Write(request));
} }
bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) { bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) const {
proto::IsTrajectoryFinishedRequest request; proto::IsTrajectoryFinishedRequest request;
request.set_trajectory_id(trajectory_id); request.set_trajectory_id(trajectory_id);
async_grpc::Client<handlers::IsTrajectoryFinishedSignature> client( async_grpc::Client<handlers::IsTrajectoryFinishedSignature> client(
@ -130,7 +132,7 @@ bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) {
return client.response().is_finished(); return client.response().is_finished();
} }
bool PoseGraphStub::IsTrajectoryFrozen(int trajectory_id) { bool PoseGraphStub::IsTrajectoryFrozen(int trajectory_id) const {
proto::IsTrajectoryFrozenRequest request; proto::IsTrajectoryFrozenRequest request;
request.set_trajectory_id(trajectory_id); request.set_trajectory_id(trajectory_id);
async_grpc::Client<handlers::IsTrajectoryFrozenSignature> client( async_grpc::Client<handlers::IsTrajectoryFrozenSignature> client(
@ -140,19 +142,19 @@ bool PoseGraphStub::IsTrajectoryFrozen(int trajectory_id) {
} }
std::map<int, mapping::PoseGraphInterface::TrajectoryData> std::map<int, mapping::PoseGraphInterface::TrajectoryData>
PoseGraphStub::GetTrajectoryData() { PoseGraphStub::GetTrajectoryData() const {
LOG(FATAL) << "Not implemented"; LOG(FATAL) << "Not implemented";
} }
std::vector<mapping::PoseGraphInterface::Constraint> std::vector<mapping::PoseGraphInterface::Constraint>
PoseGraphStub::constraints() { PoseGraphStub::constraints() const {
google::protobuf::Empty request; google::protobuf::Empty request;
async_grpc::Client<handlers::GetConstraintsSignature> client(client_channel_); async_grpc::Client<handlers::GetConstraintsSignature> client(client_channel_);
CHECK(client.Write(request)); CHECK(client.Write(request));
return mapping::FromProto(client.response().constraints()); return mapping::FromProto(client.response().constraints());
} }
mapping::proto::PoseGraph PoseGraphStub::ToProto() { mapping::proto::PoseGraph PoseGraphStub::ToProto() const {
LOG(FATAL) << "Not implemented"; LOG(FATAL) << "Not implemented";
} }

25
cartographer/cloud/internal/client/pose_graph_stub.h
View File

@ -31,22 +31,25 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface {
PoseGraphStub& operator=(const PoseGraphStub&) = delete; PoseGraphStub& operator=(const PoseGraphStub&) = delete;
void RunFinalOptimization() override; void RunFinalOptimization() override;
mapping::MapById<mapping::SubmapId, SubmapData> GetAllSubmapData() override; mapping::MapById<mapping::SubmapId, SubmapData> GetAllSubmapData()
mapping::MapById<mapping::SubmapId, SubmapPose> GetAllSubmapPoses() override; const override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) override; mapping::MapById<mapping::SubmapId, SubmapPose> GetAllSubmapPoses()
const override;
transform::Rigid3d GetLocalToGlobalTransform(
int trajectory_id) const override;
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode> mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
GetTrajectoryNodes() override; GetTrajectoryNodes() const override;
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
GetTrajectoryNodePoses() override; GetTrajectoryNodePoses() const override;
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override; std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override;
void SetLandmarkPose(const std::string& landmark_id, void SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) override; const transform::Rigid3d& global_pose) override;
bool IsTrajectoryFinished(int trajectory_id) override; bool IsTrajectoryFinished(int trajectory_id) const override;
bool IsTrajectoryFrozen(int trajectory_id) override; bool IsTrajectoryFrozen(int trajectory_id) const override;
std::map<int, mapping::PoseGraphInterface::TrajectoryData> GetTrajectoryData() std::map<int, mapping::PoseGraphInterface::TrajectoryData> GetTrajectoryData()
override; const override;
std::vector<Constraint> constraints() override; std::vector<Constraint> constraints() const override;
mapping::proto::PoseGraph ToProto() override; mapping::proto::PoseGraph ToProto() const override;
private: private:
std::shared_ptr<::grpc::Channel> client_channel_; std::shared_ptr<::grpc::Channel> client_channel_;

38
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -433,7 +433,7 @@ void PoseGraph2D::FinishTrajectory(const int trajectory_id) {
}); });
} }
bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) { bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) const {
return finished_trajectories_.count(trajectory_id) > 0; return finished_trajectories_.count(trajectory_id) > 0;
} }
@ -446,7 +446,7 @@ void PoseGraph2D::FreezeTrajectory(const int trajectory_id) {
}); });
} }
bool PoseGraph2D::IsTrajectoryFrozen(const int trajectory_id) { bool PoseGraph2D::IsTrajectoryFrozen(const int trajectory_id) const {
return frozen_trajectories_.count(trajectory_id) > 0; return frozen_trajectories_.count(trajectory_id) > 0;
} }
@ -622,12 +622,13 @@ void PoseGraph2D::RunOptimization() {
global_submap_poses_ = submap_data; global_submap_poses_ = submap_data;
} }
MapById<NodeId, TrajectoryNode> PoseGraph2D::GetTrajectoryNodes() { MapById<NodeId, TrajectoryNode> PoseGraph2D::GetTrajectoryNodes() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return trajectory_nodes_; return trajectory_nodes_;
} }
MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses() { MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses()
const {
MapById<NodeId, TrajectoryNodePose> node_poses; MapById<NodeId, TrajectoryNodePose> node_poses;
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
for (const auto& node_id_data : trajectory_nodes_) { for (const auto& node_id_data : trajectory_nodes_) {
@ -639,7 +640,8 @@ MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses() {
return node_poses; return node_poses;
} }
std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses() { std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses()
const {
std::map<std::string, transform::Rigid3d> landmark_poses; std::map<std::string, transform::Rigid3d> landmark_poses;
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
for (const auto& landmark : landmark_nodes_) { for (const auto& landmark : landmark_nodes_) {
@ -659,33 +661,33 @@ void PoseGraph2D::SetLandmarkPose(const std::string& landmark_id,
}); });
} }
sensor::MapByTime<sensor::ImuData> PoseGraph2D::GetImuData() { sensor::MapByTime<sensor::ImuData> PoseGraph2D::GetImuData() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_->imu_data(); return optimization_problem_->imu_data();
} }
sensor::MapByTime<sensor::OdometryData> PoseGraph2D::GetOdometryData() { sensor::MapByTime<sensor::OdometryData> PoseGraph2D::GetOdometryData() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_->odometry_data(); return optimization_problem_->odometry_data();
} }
std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode> std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
PoseGraph2D::GetLandmarkNodes() { PoseGraph2D::GetLandmarkNodes() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return landmark_nodes_; return landmark_nodes_;
} }
std::map<int, PoseGraphInterface::TrajectoryData> std::map<int, PoseGraphInterface::TrajectoryData>
PoseGraph2D::GetTrajectoryData() { PoseGraph2D::GetTrajectoryData() const {
return {}; // Not implemented yet in 2D. return {}; // Not implemented yet in 2D.
} }
sensor::MapByTime<sensor::FixedFramePoseData> sensor::MapByTime<sensor::FixedFramePoseData>
PoseGraph2D::GetFixedFramePoseData() { PoseGraph2D::GetFixedFramePoseData() const {
return {}; // Not implemented yet in 2D. return {}; // Not implemented yet in 2D.
} }
std::vector<PoseGraphInterface::Constraint> PoseGraph2D::constraints() { std::vector<PoseGraphInterface::Constraint> PoseGraph2D::constraints() const {
std::vector<PoseGraphInterface::Constraint> result; std::vector<PoseGraphInterface::Constraint> result;
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
for (const Constraint& constraint : constraints_) { for (const Constraint& constraint : constraints_) {
@ -732,29 +734,29 @@ transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose(
} }
transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform( transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform(
const int trajectory_id) { const int trajectory_id) const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id); return ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id);
} }
std::vector<std::vector<int>> PoseGraph2D::GetConnectedTrajectories() { std::vector<std::vector<int>> PoseGraph2D::GetConnectedTrajectories() const {
return trajectory_connectivity_state_.Components(); return trajectory_connectivity_state_.Components();
} }
PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapData( PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapData(
const SubmapId& submap_id) { const SubmapId& submap_id) const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return GetSubmapDataUnderLock(submap_id); return GetSubmapDataUnderLock(submap_id);
} }
MapById<SubmapId, PoseGraphInterface::SubmapData> MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph2D::GetAllSubmapData() { PoseGraph2D::GetAllSubmapData() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return GetSubmapDataUnderLock(); return GetSubmapDataUnderLock();
} }
MapById<SubmapId, PoseGraphInterface::SubmapPose> MapById<SubmapId, PoseGraphInterface::SubmapPose>
PoseGraph2D::GetAllSubmapPoses() { PoseGraph2D::GetAllSubmapPoses() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
MapById<SubmapId, SubmapPose> submap_poses; MapById<SubmapId, SubmapPose> submap_poses;
for (const auto& submap_id_data : submap_data_) { for (const auto& submap_id_data : submap_data_) {
@ -792,7 +794,7 @@ transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
} }
PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapDataUnderLock( PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapDataUnderLock(
const SubmapId& submap_id) { const SubmapId& submap_id) const {
const auto it = submap_data_.find(submap_id); const auto it = submap_data_.find(submap_id);
if (it == submap_data_.end()) { if (it == submap_data_.end()) {
return {}; return {};
@ -917,7 +919,7 @@ void PoseGraph2D::TrimmingHandle::MarkSubmapAsTrimmed(
} }
MapById<SubmapId, PoseGraphInterface::SubmapData> MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph2D::GetSubmapDataUnderLock() { PoseGraph2D::GetSubmapDataUnderLock() const {
MapById<SubmapId, PoseGraphInterface::SubmapData> submaps; MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
for (const auto& submap_id_data : submap_data_) { for (const auto& submap_id_data : submap_data_) {
submaps.Insert(submap_id_data.id, submaps.Insert(submap_id_data.id,

42
cartographer/mapping/internal/2d/pose_graph_2d.h
View File

@ -94,9 +94,9 @@ class PoseGraph2D : public PoseGraph {
EXCLUDES(mutex_); EXCLUDES(mutex_);
void FinishTrajectory(int trajectory_id) override; void FinishTrajectory(int trajectory_id) override;
bool IsTrajectoryFinished(int trajectory_id) override REQUIRES(mutex_); bool IsTrajectoryFinished(int trajectory_id) const override REQUIRES(mutex_);
void FreezeTrajectory(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override;
bool IsTrajectoryFrozen(int trajectory_id) override REQUIRES(mutex_); bool IsTrajectoryFrozen(int trajectory_id) const override REQUIRES(mutex_);
void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
const proto::Submap& submap) override; const proto::Submap& submap) override;
void AddNodeFromProto(const transform::Rigid3d& global_pose, void AddNodeFromProto(const transform::Rigid3d& global_pose,
@ -108,32 +108,35 @@ class PoseGraph2D : public PoseGraph {
const std::vector<Constraint>& constraints) override; const std::vector<Constraint>& constraints) override;
void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) override; void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override; void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override; std::vector<std::vector<int>> GetConnectedTrajectories() const override;
PoseGraphInterface::SubmapData GetSubmapData(const SubmapId& submap_id) PoseGraphInterface::SubmapData GetSubmapData(const SubmapId& submap_id) const
EXCLUDES(mutex_) override; EXCLUDES(mutex_) override;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() const
EXCLUDES(mutex_) override; EXCLUDES(mutex_) override;
MapById<SubmapId, SubmapPose> GetAllSubmapPoses() EXCLUDES(mutex_) override; MapById<SubmapId, SubmapPose> GetAllSubmapPoses() const
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override; EXCLUDES(mutex_) override;
MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() override transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) const
EXCLUDES(mutex_) override;
MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override
EXCLUDES(mutex_); EXCLUDES(mutex_);
MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() override MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override
EXCLUDES(mutex_); EXCLUDES(mutex_);
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
EXCLUDES(mutex_); EXCLUDES(mutex_);
void SetLandmarkPose(const std::string& landmark_id, void SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) override const transform::Rigid3d& global_pose) override
EXCLUDES(mutex_); EXCLUDES(mutex_);
sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_); sensor::MapByTime<sensor::ImuData> GetImuData() const override
sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
EXCLUDES(mutex_); EXCLUDES(mutex_);
sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData() override sensor::MapByTime<sensor::OdometryData> GetOdometryData() const override
EXCLUDES(mutex_); EXCLUDES(mutex_);
sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData()
const override EXCLUDES(mutex_);
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode> std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
GetLandmarkNodes() override EXCLUDES(mutex_); GetLandmarkNodes() const override EXCLUDES(mutex_);
std::map<int, TrajectoryData> GetTrajectoryData() override EXCLUDES(mutex_); std::map<int, TrajectoryData> GetTrajectoryData() const override
std::vector<Constraint> constraints() override EXCLUDES(mutex_); EXCLUDES(mutex_);
std::vector<Constraint> constraints() const override EXCLUDES(mutex_);
void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id, void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,
const transform::Rigid3d& pose, const transform::Rigid3d& pose,
const common::Time time) override const common::Time time) override
@ -158,7 +161,7 @@ class PoseGraph2D : public PoseGraph {
}; };
MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock() MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock()
REQUIRES(mutex_); const REQUIRES(mutex_);
// Handles a new work item. // Handles a new work item.
void AddWorkItem(const std::function<void()>& work_item) REQUIRES(mutex_); void AddWorkItem(const std::function<void()>& work_item) REQUIRES(mutex_);
@ -205,7 +208,8 @@ class PoseGraph2D : public PoseGraph {
const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses, const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses,
int trajectory_id) const REQUIRES(mutex_); int trajectory_id) const REQUIRES(mutex_);
SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) REQUIRES(mutex_); SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) const
REQUIRES(mutex_);
common::Time GetLatestNodeTime(const NodeId& node_id, common::Time GetLatestNodeTime(const NodeId& node_id,
const SubmapId& submap_id) const const SubmapId& submap_id) const
@ -217,7 +221,7 @@ class PoseGraph2D : public PoseGraph {
const proto::PoseGraphOptions options_; const proto::PoseGraphOptions options_;
GlobalSlamOptimizationCallback global_slam_optimization_callback_; GlobalSlamOptimizationCallback global_slam_optimization_callback_;
common::Mutex mutex_; mutable common::Mutex mutex_;
// If it exists, further work items must be added to this queue, and will be // If it exists, further work items must be added to this queue, and will be
// considered later. // considered later.

40
cartographer/mapping/internal/3d/pose_graph_3d.cc
View File

@ -448,7 +448,7 @@ void PoseGraph3D::FinishTrajectory(const int trajectory_id) {
}); });
} }
bool PoseGraph3D::IsTrajectoryFinished(const int trajectory_id) { bool PoseGraph3D::IsTrajectoryFinished(const int trajectory_id) const {
return finished_trajectories_.count(trajectory_id) > 0; return finished_trajectories_.count(trajectory_id) > 0;
} }
@ -461,7 +461,7 @@ void PoseGraph3D::FreezeTrajectory(const int trajectory_id) {
}); });
} }
bool PoseGraph3D::IsTrajectoryFrozen(const int trajectory_id) { bool PoseGraph3D::IsTrajectoryFrozen(const int trajectory_id) const {
return frozen_trajectories_.count(trajectory_id) > 0; return frozen_trajectories_.count(trajectory_id) > 0;
} }
@ -587,7 +587,7 @@ void PoseGraph3D::RunFinalOptimization() {
WaitForAllComputations(); WaitForAllComputations();
} }
void PoseGraph3D::LogResidualHistograms() { void PoseGraph3D::LogResidualHistograms() const {
common::Histogram rotational_residual; common::Histogram rotational_residual;
common::Histogram translational_residual; common::Histogram translational_residual;
for (const Constraint& constraint : constraints_) { for (const Constraint& constraint : constraints_) {
@ -663,12 +663,13 @@ void PoseGraph3D::RunOptimization() {
} }
} }
MapById<NodeId, TrajectoryNode> PoseGraph3D::GetTrajectoryNodes() { MapById<NodeId, TrajectoryNode> PoseGraph3D::GetTrajectoryNodes() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return trajectory_nodes_; return trajectory_nodes_;
} }
MapById<NodeId, TrajectoryNodePose> PoseGraph3D::GetTrajectoryNodePoses() { MapById<NodeId, TrajectoryNodePose> PoseGraph3D::GetTrajectoryNodePoses()
const {
MapById<NodeId, TrajectoryNodePose> node_poses; MapById<NodeId, TrajectoryNodePose> node_poses;
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
for (const auto& node_id_data : trajectory_nodes_) { for (const auto& node_id_data : trajectory_nodes_) {
@ -680,7 +681,8 @@ MapById<NodeId, TrajectoryNodePose> PoseGraph3D::GetTrajectoryNodePoses() {
return node_poses; return node_poses;
} }
std::map<std::string, transform::Rigid3d> PoseGraph3D::GetLandmarkPoses() { std::map<std::string, transform::Rigid3d> PoseGraph3D::GetLandmarkPoses()
const {
std::map<std::string, transform::Rigid3d> landmark_poses; std::map<std::string, transform::Rigid3d> landmark_poses;
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
for (const auto& landmark : landmark_nodes_) { for (const auto& landmark : landmark_nodes_) {
@ -700,35 +702,35 @@ void PoseGraph3D::SetLandmarkPose(const std::string& landmark_id,
}); });
} }
sensor::MapByTime<sensor::ImuData> PoseGraph3D::GetImuData() { sensor::MapByTime<sensor::ImuData> PoseGraph3D::GetImuData() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_->imu_data(); return optimization_problem_->imu_data();
} }
sensor::MapByTime<sensor::OdometryData> PoseGraph3D::GetOdometryData() { sensor::MapByTime<sensor::OdometryData> PoseGraph3D::GetOdometryData() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_->odometry_data(); return optimization_problem_->odometry_data();
} }
sensor::MapByTime<sensor::FixedFramePoseData> sensor::MapByTime<sensor::FixedFramePoseData>
PoseGraph3D::GetFixedFramePoseData() { PoseGraph3D::GetFixedFramePoseData() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_->fixed_frame_pose_data(); return optimization_problem_->fixed_frame_pose_data();
} }
std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode> std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
PoseGraph3D::GetLandmarkNodes() { PoseGraph3D::GetLandmarkNodes() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return landmark_nodes_; return landmark_nodes_;
} }
std::map<int, PoseGraphInterface::TrajectoryData> std::map<int, PoseGraphInterface::TrajectoryData>
PoseGraph3D::GetTrajectoryData() { PoseGraph3D::GetTrajectoryData() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_->trajectory_data(); return optimization_problem_->trajectory_data();
} }
std::vector<PoseGraphInterface::Constraint> PoseGraph3D::constraints() { std::vector<PoseGraphInterface::Constraint> PoseGraph3D::constraints() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return constraints_; return constraints_;
} }
@ -763,29 +765,29 @@ transform::Rigid3d PoseGraph3D::GetInterpolatedGlobalTrajectoryPose(
} }
transform::Rigid3d PoseGraph3D::GetLocalToGlobalTransform( transform::Rigid3d PoseGraph3D::GetLocalToGlobalTransform(
const int trajectory_id) { const int trajectory_id) const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id); return ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id);
} }
std::vector<std::vector<int>> PoseGraph3D::GetConnectedTrajectories() { std::vector<std::vector<int>> PoseGraph3D::GetConnectedTrajectories() const {
return trajectory_connectivity_state_.Components(); return trajectory_connectivity_state_.Components();
} }
PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapData( PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapData(
const SubmapId& submap_id) { const SubmapId& submap_id) const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return GetSubmapDataUnderLock(submap_id); return GetSubmapDataUnderLock(submap_id);
} }
MapById<SubmapId, PoseGraphInterface::SubmapData> MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph3D::GetAllSubmapData() { PoseGraph3D::GetAllSubmapData() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return GetSubmapDataUnderLock(); return GetSubmapDataUnderLock();
} }
MapById<SubmapId, PoseGraphInterface::SubmapPose> MapById<SubmapId, PoseGraphInterface::SubmapPose>
PoseGraph3D::GetAllSubmapPoses() { PoseGraph3D::GetAllSubmapPoses() const {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
MapById<SubmapId, SubmapPose> submap_poses; MapById<SubmapId, SubmapPose> submap_poses;
for (const auto& submap_id_data : submap_data_) { for (const auto& submap_id_data : submap_data_) {
@ -822,7 +824,7 @@ transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform(
} }
PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapDataUnderLock( PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapDataUnderLock(
const SubmapId& submap_id) { const SubmapId& submap_id) const {
const auto it = submap_data_.find(submap_id); const auto it = submap_data_.find(submap_id);
if (it == submap_data_.end()) { if (it == submap_data_.end()) {
return {}; return {};
@ -945,7 +947,7 @@ void PoseGraph3D::TrimmingHandle::MarkSubmapAsTrimmed(
} }
MapById<SubmapId, PoseGraphInterface::SubmapData> MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph3D::GetSubmapDataUnderLock() { PoseGraph3D::GetSubmapDataUnderLock() const {
MapById<SubmapId, PoseGraphInterface::SubmapData> submaps; MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
for (const auto& submap_id_data : submap_data_) { for (const auto& submap_id_data : submap_data_) {
submaps.Insert(submap_id_data.id, submaps.Insert(submap_id_data.id,

49
cartographer/mapping/internal/3d/pose_graph_3d.h
View File

@ -93,9 +93,9 @@ class PoseGraph3D : public PoseGraph {
EXCLUDES(mutex_); EXCLUDES(mutex_);
void FinishTrajectory(int trajectory_id) override; void FinishTrajectory(int trajectory_id) override;
bool IsTrajectoryFinished(int trajectory_id) override REQUIRES(mutex_); bool IsTrajectoryFinished(int trajectory_id) const override REQUIRES(mutex_);
void FreezeTrajectory(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override;
bool IsTrajectoryFrozen(int trajectory_id) override REQUIRES(mutex_); bool IsTrajectoryFrozen(int trajectory_id) const override REQUIRES(mutex_);
void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
const proto::Submap& submap) override; const proto::Submap& submap) override;
void AddNodeFromProto(const transform::Rigid3d& global_pose, void AddNodeFromProto(const transform::Rigid3d& global_pose,
@ -107,32 +107,35 @@ class PoseGraph3D : public PoseGraph {
const std::vector<Constraint>& constraints) override; const std::vector<Constraint>& constraints) override;
void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) override; void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override; void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override; std::vector<std::vector<int>> GetConnectedTrajectories() const override;
PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id) PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id) const
EXCLUDES(mutex_) override; EXCLUDES(mutex_) override;
MapById<SubmapId, SubmapData> GetAllSubmapData() EXCLUDES(mutex_) override; MapById<SubmapId, SubmapData> GetAllSubmapData() const
MapById<SubmapId, SubmapPose> GetAllSubmapPoses() EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override; EXCLUDES(mutex_) override;
MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() override MapById<SubmapId, SubmapPose> GetAllSubmapPoses() const
EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) const
EXCLUDES(mutex_) override;
MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override
EXCLUDES(mutex_); EXCLUDES(mutex_);
MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() override MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override
EXCLUDES(mutex_); EXCLUDES(mutex_);
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
EXCLUDES(mutex_); EXCLUDES(mutex_);
void SetLandmarkPose(const std::string& landmark_id, void SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) override const transform::Rigid3d& global_pose) override
EXCLUDES(mutex_); EXCLUDES(mutex_);
sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_); sensor::MapByTime<sensor::ImuData> GetImuData() const override
sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
EXCLUDES(mutex_); EXCLUDES(mutex_);
sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData() override sensor::MapByTime<sensor::OdometryData> GetOdometryData() const override
EXCLUDES(mutex_); EXCLUDES(mutex_);
sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData()
const override EXCLUDES(mutex_);
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode> std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
GetLandmarkNodes() override EXCLUDES(mutex_); GetLandmarkNodes() const override EXCLUDES(mutex_);
std::map<int, TrajectoryData> GetTrajectoryData() override; std::map<int, TrajectoryData> GetTrajectoryData() const override;
std::vector<Constraint> constraints() override EXCLUDES(mutex_); std::vector<Constraint> constraints() const override EXCLUDES(mutex_);
void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id, void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,
const transform::Rigid3d& pose, const transform::Rigid3d& pose,
const common::Time time) override const common::Time time) override
@ -161,7 +164,7 @@ class PoseGraph3D : public PoseGraph {
SubmapState state = SubmapState::kActive; SubmapState state = SubmapState::kActive;
}; };
MapById<SubmapId, SubmapData> GetSubmapDataUnderLock() REQUIRES(mutex_); MapById<SubmapId, SubmapData> GetSubmapDataUnderLock() const REQUIRES(mutex_);
// Handles a new work item. // Handles a new work item.
void AddWorkItem(const std::function<void()>& work_item) REQUIRES(mutex_); void AddWorkItem(const std::function<void()>& work_item) REQUIRES(mutex_);
@ -204,7 +207,7 @@ class PoseGraph3D : public PoseGraph {
const MapById<SubmapId, optimization::SubmapSpec3D>& global_submap_poses, const MapById<SubmapId, optimization::SubmapSpec3D>& global_submap_poses,
int trajectory_id) const REQUIRES(mutex_); int trajectory_id) const REQUIRES(mutex_);
PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) const
REQUIRES(mutex_); REQUIRES(mutex_);
common::Time GetLatestNodeTime(const NodeId& node_id, common::Time GetLatestNodeTime(const NodeId& node_id,
@ -213,15 +216,18 @@ class PoseGraph3D : public PoseGraph {
// Logs histograms for the translational and rotational residual of node // Logs histograms for the translational and rotational residual of node
// poses. // poses.
void LogResidualHistograms() REQUIRES(mutex_); void LogResidualHistograms() const REQUIRES(mutex_);
// Updates the trajectory connectivity structure with a new constraint. // Updates the trajectory connectivity structure with a new constraint.
void UpdateTrajectoryConnectivity(const Constraint& constraint) void UpdateTrajectoryConnectivity(const Constraint& constraint)
REQUIRES(mutex_); REQUIRES(mutex_);
// Schedules optimization (i.e. loop closure) to run.
void DispatchOptimization() REQUIRES(mutex_);
const proto::PoseGraphOptions options_; const proto::PoseGraphOptions options_;
GlobalSlamOptimizationCallback global_slam_optimization_callback_; GlobalSlamOptimizationCallback global_slam_optimization_callback_;
common::Mutex mutex_; mutable common::Mutex mutex_;
// If it exists, further work items must be added to this queue, and will be // If it exists, further work items must be added to this queue, and will be
// considered later. // considered later.
@ -241,9 +247,6 @@ class PoseGraph3D : public PoseGraph {
// Whether the optimization has to be run before more data is added. // Whether the optimization has to be run before more data is added.
bool run_loop_closure_ GUARDED_BY(mutex_) = false; bool run_loop_closure_ GUARDED_BY(mutex_) = false;
// Schedules optimization (i.e. loop closure) to run.
void DispatchOptimization() REQUIRES(mutex_);
// Current optimization problem. // Current optimization problem.
std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem_; std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem_;
constraints::ConstraintBuilder3D constraint_builder_ GUARDED_BY(mutex_); constraints::ConstraintBuilder3D constraint_builder_ GUARDED_BY(mutex_);

1
cartographer/mapping/internal/connected_components.cc
View File

@ -54,6 +54,7 @@ int ConnectedComponents::FindSet(const int trajectory_id) {
auto it = forest_.find(trajectory_id); auto it = forest_.find(trajectory_id);
CHECK(it != forest_.end()); CHECK(it != forest_.end());
if (it->first != it->second) { if (it->first != it->second) {
// Path compression for efficiency.
it->second = FindSet(it->second); it->second = FindSet(it->second);
} }
return it->second; return it->second;

25
cartographer/mapping/internal/testing/mock_pose_graph.h
View File

@ -32,25 +32,28 @@ class MockPoseGraph : public mapping::PoseGraphInterface {
~MockPoseGraph() override = default; ~MockPoseGraph() override = default;
MOCK_METHOD0(RunFinalOptimization, void()); MOCK_METHOD0(RunFinalOptimization, void());
MOCK_METHOD0(GetAllSubmapData, MOCK_CONST_METHOD0(GetAllSubmapData,
mapping::MapById<mapping::SubmapId, SubmapData>()); mapping::MapById<mapping::SubmapId, SubmapData>());
MOCK_METHOD0(GetAllSubmapPoses, MOCK_CONST_METHOD0(GetAllSubmapPoses,
mapping::MapById<mapping::SubmapId, SubmapPose>()); mapping::MapById<mapping::SubmapId, SubmapPose>());
MOCK_METHOD1(GetLocalToGlobalTransform, transform::Rigid3d(int)); MOCK_CONST_METHOD1(GetLocalToGlobalTransform, transform::Rigid3d(int));
MOCK_METHOD0(GetTrajectoryNodes, MOCK_CONST_METHOD0(
GetTrajectoryNodes,
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>()); mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>());
MOCK_METHOD0( MOCK_CONST_METHOD0(
GetTrajectoryNodePoses, GetTrajectoryNodePoses,
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>()); mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>());
MOCK_METHOD0(GetLandmarkPoses, std::map<std::string, transform::Rigid3d>()); MOCK_CONST_METHOD0(GetLandmarkPoses,
std::map<std::string, transform::Rigid3d>());
MOCK_METHOD2(SetLandmarkPose, MOCK_METHOD2(SetLandmarkPose,
void(const std::string&, const transform::Rigid3d&)); void(const std::string&, const transform::Rigid3d&));
MOCK_METHOD1(IsTrajectoryFinished, bool(int)); MOCK_CONST_METHOD1(IsTrajectoryFinished, bool(int));
MOCK_METHOD1(IsTrajectoryFrozen, bool(int)); MOCK_CONST_METHOD1(IsTrajectoryFrozen, bool(int));
MOCK_METHOD0(GetTrajectoryData, MOCK_CONST_METHOD0(
GetTrajectoryData,
std::map<int, mapping::PoseGraphInterface::TrajectoryData>()); std::map<int, mapping::PoseGraphInterface::TrajectoryData>());
MOCK_METHOD0(constraints, std::vector<Constraint>()); MOCK_CONST_METHOD0(constraints, std::vector<Constraint>());
MOCK_METHOD0(ToProto, mapping::proto::PoseGraph()); MOCK_CONST_METHOD0(ToProto, mapping::proto::PoseGraph());
}; };
} // namespace testing } // namespace testing

6
cartographer/mapping/internal/trajectory_connectivity_state.cc
View File

@ -55,18 +55,18 @@ void TrajectoryConnectivityState::Connect(const int trajectory_id_a,
} }
bool TrajectoryConnectivityState::TransitivelyConnected( bool TrajectoryConnectivityState::TransitivelyConnected(
const int trajectory_id_a, const int trajectory_id_b) { const int trajectory_id_a, const int trajectory_id_b) const {
return connected_components_.TransitivelyConnected(trajectory_id_a, return connected_components_.TransitivelyConnected(trajectory_id_a,
trajectory_id_b); trajectory_id_b);
} }
std::vector<std::vector<int>> TrajectoryConnectivityState::Components() { std::vector<std::vector<int>> TrajectoryConnectivityState::Components() const {
return connected_components_.Components(); return connected_components_.Components();
} }
common::Time TrajectoryConnectivityState::LastConnectionTime( common::Time TrajectoryConnectivityState::LastConnectionTime(
const int trajectory_id_a, const int trajectory_id_b) { const int trajectory_id_a, const int trajectory_id_b) {
auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b); const auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b);
return last_connection_time_map_[sorted_pair]; return last_connection_time_map_[sorted_pair];
} }

7
cartographer/mapping/internal/trajectory_connectivity_state.h
View File

@ -49,10 +49,10 @@ class TrajectoryConnectivityState {
// either trajectory is not being tracked, returns false, except when it is // either trajectory is not being tracked, returns false, except when it is
// the same trajectory, where it returns true. This function is invariant to // the same trajectory, where it returns true. This function is invariant to
// the order of its arguments. // the order of its arguments.
bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b); bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b) const;
// The trajectory IDs, grouped by connectivity. // The trajectory IDs, grouped by connectivity.
std::vector<std::vector<int>> Components(); std::vector<std::vector<int>> Components() const;
// Return the last connection count between the two trajectories. If either of // Return the last connection count between the two trajectories. If either of
// the trajectories is untracked or they have never been connected returns the // the trajectories is untracked or they have never been connected returns the
@ -60,7 +60,8 @@ class TrajectoryConnectivityState {
common::Time LastConnectionTime(int trajectory_id_a, int trajectory_id_b); common::Time LastConnectionTime(int trajectory_id_a, int trajectory_id_b);
private: private:
ConnectedComponents connected_components_; // ConnectedComponents are thread safe.
mutable ConnectedComponents connected_components_;
// Tracks the last time a direct connection between two trajectories has // Tracks the last time a direct connection between two trajectories has
// been added. The exception is when a connection between two trajectories // been added. The exception is when a connection between two trajectories

2
cartographer/mapping/pose_graph.cc
View File

@ -115,7 +115,7 @@ proto::PoseGraph::Constraint ToProto(const PoseGraph::Constraint& constraint) {
return constraint_proto; return constraint_proto;
} }
proto::PoseGraph PoseGraph::ToProto() { proto::PoseGraph PoseGraph::ToProto() const {
proto::PoseGraph proto; proto::PoseGraph proto;
std::map<int, proto::Trajectory* const> trajectory_protos; std::map<int, proto::Trajectory* const> trajectory_protos;

16
cartographer/mapping/pose_graph.h
View File

@ -114,28 +114,28 @@ class PoseGraph : public PoseGraphInterface {
virtual void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) = 0; virtual void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) = 0;
// Gets the current trajectory clusters. // Gets the current trajectory clusters.
virtual std::vector<std::vector<int>> GetConnectedTrajectories() = 0; virtual std::vector<std::vector<int>> GetConnectedTrajectories() const = 0;
// Returns the current optimized transform and submap itself for the given // Returns the current optimized transform and submap itself for the given
// 'submap_id'. Returns 'nullptr' for the 'submap' member if the submap does // 'submap_id'. Returns 'nullptr' for the 'submap' member if the submap does
// not exist (anymore). // not exist (anymore).
virtual SubmapData GetSubmapData(const SubmapId& submap_id) = 0; virtual SubmapData GetSubmapData(const SubmapId& submap_id) const = 0;
proto::PoseGraph ToProto() override; proto::PoseGraph ToProto() const override;
// Returns the IMU data. // Returns the IMU data.
virtual sensor::MapByTime<sensor::ImuData> GetImuData() = 0; virtual sensor::MapByTime<sensor::ImuData> GetImuData() const = 0;
// Returns the odometry data. // Returns the odometry data.
virtual sensor::MapByTime<sensor::OdometryData> GetOdometryData() = 0; virtual sensor::MapByTime<sensor::OdometryData> GetOdometryData() const = 0;
// Returns the fixed frame pose data. // Returns the fixed frame pose data.
virtual sensor::MapByTime<sensor::FixedFramePoseData> virtual sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData()
GetFixedFramePoseData() = 0; const = 0;
// Returns the landmark data. // Returns the landmark data.
virtual std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode> virtual std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
GetLandmarkNodes() = 0; GetLandmarkNodes() const = 0;
// Sets a relative initial pose 'relative_pose' for 'from_trajectory_id' with // Sets a relative initial pose 'relative_pose' for 'from_trajectory_id' with
// respect to 'to_trajectory_id' at time 'time'. // respect to 'to_trajectory_id' at time 'time'.

25
cartographer/mapping/pose_graph_interface.h
View File

@ -89,43 +89,46 @@ class PoseGraphInterface {
virtual void RunFinalOptimization() = 0; virtual void RunFinalOptimization() = 0;
// Returns data for all submaps. // Returns data for all submaps.
virtual MapById<SubmapId, SubmapData> GetAllSubmapData() = 0; virtual MapById<SubmapId, SubmapData> GetAllSubmapData() const = 0;
// Returns the global poses for all submaps. // Returns the global poses for all submaps.
virtual MapById<SubmapId, SubmapPose> GetAllSubmapPoses() = 0; virtual MapById<SubmapId, SubmapPose> GetAllSubmapPoses() const = 0;
// Returns the transform converting data in the local map frame (i.e. the // Returns the transform converting data in the local map frame (i.e. the
// continuous, non-loop-closed frame) into the global map frame (i.e. the // continuous, non-loop-closed frame) into the global map frame (i.e. the
// discontinuous, loop-closed frame). // discontinuous, loop-closed frame).
virtual transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) = 0; virtual transform::Rigid3d GetLocalToGlobalTransform(
int trajectory_id) const = 0;
// Returns the current optimized trajectories. // Returns the current optimized trajectories.
virtual MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() = 0; virtual MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const = 0;
// Returns the current optimized trajectory poses. // Returns the current optimized trajectory poses.
virtual MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() = 0; virtual MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses()
const = 0;
// Returns the current optimized landmark poses. // Returns the current optimized landmark poses.
virtual std::map<std::string, transform::Rigid3d> GetLandmarkPoses() = 0; virtual std::map<std::string, transform::Rigid3d> GetLandmarkPoses()
const = 0;
// Sets global pose of landmark 'landmark_id' to given 'global_pose'. // Sets global pose of landmark 'landmark_id' to given 'global_pose'.
virtual void SetLandmarkPose(const std::string& landmark_id, virtual void SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) = 0; const transform::Rigid3d& global_pose) = 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) const = 0;
// Checks if the given trajectory is frozen. // Checks if the given trajectory is frozen.
virtual bool IsTrajectoryFrozen(int trajectory_id) = 0; virtual bool IsTrajectoryFrozen(int trajectory_id) const = 0;
// Returns the trajectory data. // Returns the trajectory data.
virtual std::map<int, TrajectoryData> GetTrajectoryData() = 0; virtual std::map<int, TrajectoryData> GetTrajectoryData() const = 0;
// Returns the collection of constraints. // Returns the collection of constraints.
virtual std::vector<Constraint> constraints() = 0; virtual std::vector<Constraint> constraints() const = 0;
// Serializes the constraints and trajectories. // Serializes the constraints and trajectories.
virtual proto::PoseGraph ToProto() = 0; virtual proto::PoseGraph ToProto() const = 0;
}; };
} // namespace mapping } // namespace mapping