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

cartographer/cloud/internal/client/pose_graph_stub.cc

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

cartographer/cloud/internal/client/pose_graph_stub.h

@@ -31,22 +31,25 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface {
   PoseGraphStub& operator=(const PoseGraphStub&) = delete;
 
   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>
-  GetTrajectoryNodes() override;
+  GetTrajectoryNodes() const override;
   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,
                        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()
-      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:
   std::shared_ptr<::grpc::Channel> client_channel_;

cartographer/mapping/internal/2d/pose_graph_2d.cc

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

cartographer/mapping/internal/2d/pose_graph_2d.h

@@ -94,9 +94,9 @@ class PoseGraph2D : public PoseGraph {
       EXCLUDES(mutex_);
 
   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;
-  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,
                           const proto::Submap& submap) override;
   void AddNodeFromProto(const transform::Rigid3d& global_pose,
@@ -108,32 +108,35 @@ class PoseGraph2D : public PoseGraph {
       const std::vector<Constraint>& constraints) override;
   void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) 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;
-  MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
+  MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() const
       EXCLUDES(mutex_) override;
-  MapById<SubmapId, SubmapPose> GetAllSubmapPoses() EXCLUDES(mutex_) override;
+  MapById<SubmapId, SubmapPose> GetAllSubmapPoses() const
-  transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
       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_);
-  MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() override
+  MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override
       EXCLUDES(mutex_);
-  std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override
+  std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
       EXCLUDES(mutex_);
   void SetLandmarkPose(const std::string& landmark_id,
                        const transform::Rigid3d& global_pose) override
       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_);
-  sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData() override
+  sensor::MapByTime<sensor::OdometryData> GetOdometryData() const override
       EXCLUDES(mutex_);
+  sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData()
+      const override EXCLUDES(mutex_);
   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,
                                 const transform::Rigid3d& pose,
                                 const common::Time time) override
@@ -158,7 +161,7 @@ class PoseGraph2D : public PoseGraph {
   };
 
   MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock()
-      REQUIRES(mutex_);
+      const REQUIRES(mutex_);
 
   // Handles a new work item.
   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,
       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,
                                  const SubmapId& submap_id) const
@@ -217,7 +221,7 @@ class PoseGraph2D : public PoseGraph {
 
   const proto::PoseGraphOptions options_;
   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
   // considered later.

cartographer/mapping/internal/3d/pose_graph_3d.cc

@@ -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;
 }
 
@@ -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;
 }
 
@@ -587,7 +587,7 @@ void PoseGraph3D::RunFinalOptimization() {
   WaitForAllComputations();
 }
 
-void PoseGraph3D::LogResidualHistograms() {
+void PoseGraph3D::LogResidualHistograms() const {
   common::Histogram rotational_residual;
   common::Histogram translational_residual;
   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_);
   return trajectory_nodes_;
 }
 
-MapById<NodeId, TrajectoryNodePose> PoseGraph3D::GetTrajectoryNodePoses() {
+MapById<NodeId, TrajectoryNodePose> PoseGraph3D::GetTrajectoryNodePoses()
+    const {
   MapById<NodeId, TrajectoryNodePose> node_poses;
   common::MutexLocker locker(&mutex_);
   for (const auto& node_id_data : trajectory_nodes_) {
@@ -680,7 +681,8 @@ MapById<NodeId, TrajectoryNodePose> PoseGraph3D::GetTrajectoryNodePoses() {
   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;
   common::MutexLocker locker(&mutex_);
   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_);
   return optimization_problem_->imu_data();
 }
 
-sensor::MapByTime<sensor::OdometryData> PoseGraph3D::GetOdometryData() {
+sensor::MapByTime<sensor::OdometryData> PoseGraph3D::GetOdometryData() const {
   common::MutexLocker locker(&mutex_);
   return optimization_problem_->odometry_data();
 }
 
 sensor::MapByTime<sensor::FixedFramePoseData>
-PoseGraph3D::GetFixedFramePoseData() {
+PoseGraph3D::GetFixedFramePoseData() const {
   common::MutexLocker locker(&mutex_);
   return optimization_problem_->fixed_frame_pose_data();
 }
 
 std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
-PoseGraph3D::GetLandmarkNodes() {
+PoseGraph3D::GetLandmarkNodes() const {
   common::MutexLocker locker(&mutex_);
   return landmark_nodes_;
 }
 
 std::map<int, PoseGraphInterface::TrajectoryData>
-PoseGraph3D::GetTrajectoryData() {
+PoseGraph3D::GetTrajectoryData() const {
   common::MutexLocker locker(&mutex_);
   return optimization_problem_->trajectory_data();
 }
 
-std::vector<PoseGraphInterface::Constraint> PoseGraph3D::constraints() {
+std::vector<PoseGraphInterface::Constraint> PoseGraph3D::constraints() const {
   common::MutexLocker locker(&mutex_);
   return constraints_;
 }
@@ -763,29 +765,29 @@ transform::Rigid3d PoseGraph3D::GetInterpolatedGlobalTrajectoryPose(
 }
 
 transform::Rigid3d PoseGraph3D::GetLocalToGlobalTransform(
-    const int trajectory_id) {
+    const int trajectory_id) const {
   common::MutexLocker locker(&mutex_);
   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();
 }
 
 PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapData(
-    const SubmapId& submap_id) {
+    const SubmapId& submap_id) const {
   common::MutexLocker locker(&mutex_);
   return GetSubmapDataUnderLock(submap_id);
 }
 
 MapById<SubmapId, PoseGraphInterface::SubmapData>
-PoseGraph3D::GetAllSubmapData() {
+PoseGraph3D::GetAllSubmapData() const {
   common::MutexLocker locker(&mutex_);
   return GetSubmapDataUnderLock();
 }
 
 MapById<SubmapId, PoseGraphInterface::SubmapPose>
-PoseGraph3D::GetAllSubmapPoses() {
+PoseGraph3D::GetAllSubmapPoses() const {
   common::MutexLocker locker(&mutex_);
   MapById<SubmapId, SubmapPose> submap_poses;
   for (const auto& submap_id_data : submap_data_) {
@@ -822,7 +824,7 @@ transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform(
 }
 
 PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapDataUnderLock(
-    const SubmapId& submap_id) {
+    const SubmapId& submap_id) const {
   const auto it = submap_data_.find(submap_id);
   if (it == submap_data_.end()) {
     return {};
@@ -945,7 +947,7 @@ void PoseGraph3D::TrimmingHandle::MarkSubmapAsTrimmed(
 }
 
 MapById<SubmapId, PoseGraphInterface::SubmapData>
-PoseGraph3D::GetSubmapDataUnderLock() {
+PoseGraph3D::GetSubmapDataUnderLock() const {
   MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
   for (const auto& submap_id_data : submap_data_) {
     submaps.Insert(submap_id_data.id,

cartographer/mapping/internal/3d/pose_graph_3d.h

@@ -93,9 +93,9 @@ class PoseGraph3D : public PoseGraph {
       EXCLUDES(mutex_);
 
   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;
-  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,
                           const proto::Submap& submap) override;
   void AddNodeFromProto(const transform::Rigid3d& global_pose,
@@ -107,32 +107,35 @@ class PoseGraph3D : public PoseGraph {
       const std::vector<Constraint>& constraints) override;
   void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) 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;
-  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;
-  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_);
-  MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() override
+  MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override
       EXCLUDES(mutex_);
-  std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override
+  std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
       EXCLUDES(mutex_);
   void SetLandmarkPose(const std::string& landmark_id,
                        const transform::Rigid3d& global_pose) override
       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_);
-  sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData() override
+  sensor::MapByTime<sensor::OdometryData> GetOdometryData() const override
       EXCLUDES(mutex_);
+  sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData()
+      const override EXCLUDES(mutex_);
   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,
                                 const transform::Rigid3d& pose,
                                 const common::Time time) override
@@ -161,7 +164,7 @@ class PoseGraph3D : public PoseGraph {
     SubmapState state = SubmapState::kActive;
   };
 
-  MapById<SubmapId, SubmapData> GetSubmapDataUnderLock() REQUIRES(mutex_);
+  MapById<SubmapId, SubmapData> GetSubmapDataUnderLock() const REQUIRES(mutex_);
 
   // Handles a new work item.
   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,
       int trajectory_id) const REQUIRES(mutex_);
 
-  PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id)
+  PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) const
       REQUIRES(mutex_);
 
   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
   // poses.
-  void LogResidualHistograms() REQUIRES(mutex_);
+  void LogResidualHistograms() const REQUIRES(mutex_);
 
   // Updates the trajectory connectivity structure with a new constraint.
   void UpdateTrajectoryConnectivity(const Constraint& constraint)
       REQUIRES(mutex_);
 
+  // Schedules optimization (i.e. loop closure) to run.
+  void DispatchOptimization() REQUIRES(mutex_);
+
   const proto::PoseGraphOptions options_;
   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
   // considered later.
@@ -241,9 +247,6 @@ class PoseGraph3D : public PoseGraph {
   // Whether the optimization has to be run before more data is added.
   bool run_loop_closure_ GUARDED_BY(mutex_) = false;
 
-  // Schedules optimization (i.e. loop closure) to run.
-  void DispatchOptimization() REQUIRES(mutex_);
-
   // Current optimization problem.
   std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem_;
   constraints::ConstraintBuilder3D constraint_builder_ GUARDED_BY(mutex_);

cartographer/mapping/internal/connected_components.cc

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

cartographer/mapping/internal/testing/mock_pose_graph.h

@@ -32,25 +32,28 @@ class MockPoseGraph : public mapping::PoseGraphInterface {
   ~MockPoseGraph() override = default;
 
   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(
-               mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>());
+      GetTrajectoryNodes,
-  MOCK_METHOD0(
+      mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>());
+  MOCK_CONST_METHOD0(
       GetTrajectoryNodePoses,
       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,
                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(
-               std::map<int, mapping::PoseGraphInterface::TrajectoryData>());
+      GetTrajectoryData,
-  MOCK_METHOD0(constraints, std::vector<Constraint>());
+      std::map<int, mapping::PoseGraphInterface::TrajectoryData>());
-  MOCK_METHOD0(ToProto, mapping::proto::PoseGraph());
+  MOCK_CONST_METHOD0(constraints, std::vector<Constraint>());
+  MOCK_CONST_METHOD0(ToProto, mapping::proto::PoseGraph());
 };
 
 }  // namespace testing

cartographer/mapping/internal/trajectory_connectivity_state.cc

@@ -55,18 +55,18 @@ void TrajectoryConnectivityState::Connect(const int trajectory_id_a,
 }
 
 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,
                                                      trajectory_id_b);
 }
 
-std::vector<std::vector<int>> TrajectoryConnectivityState::Components() {
+std::vector<std::vector<int>> TrajectoryConnectivityState::Components() const {
   return connected_components_.Components();
 }
 
 common::Time TrajectoryConnectivityState::LastConnectionTime(
     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];
 }
 

cartographer/mapping/internal/trajectory_connectivity_state.h

@@ -49,10 +49,10 @@ class TrajectoryConnectivityState {
   // 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 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.
-  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
   // 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);
 
  private:
-  ConnectedComponents connected_components_;
+  // ConnectedComponents are thread safe.
+  mutable ConnectedComponents connected_components_;
 
   // Tracks the last time a direct connection between two trajectories has
   // been added. The exception is when a connection between two trajectories

cartographer/mapping/pose_graph.cc

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

cartographer/mapping/pose_graph.h

@@ -114,28 +114,28 @@ class PoseGraph : public PoseGraphInterface {
   virtual void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) = 0;
 
   // 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
   // 'submap_id'. Returns 'nullptr' for the 'submap' member if the submap does
   // 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.
-  virtual sensor::MapByTime<sensor::ImuData> GetImuData() = 0;
+  virtual sensor::MapByTime<sensor::ImuData> GetImuData() const = 0;
 
   // 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.
-  virtual sensor::MapByTime<sensor::FixedFramePoseData>
+  virtual sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData()
-  GetFixedFramePoseData() = 0;
+      const = 0;
 
   // Returns the landmark data.
   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
   // respect to 'to_trajectory_id' at time 'time'.

cartographer/mapping/pose_graph_interface.h

@@ -89,43 +89,46 @@ class PoseGraphInterface {
   virtual void RunFinalOptimization() = 0;
 
   // 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.
-  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
   // continuous, non-loop-closed frame) into the global map frame (i.e. the
   // 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.
-  virtual MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() = 0;
+  virtual MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const = 0;
 
   // 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.
-  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'.
   virtual void SetLandmarkPose(const std::string& landmark_id,
                                const transform::Rigid3d& global_pose) = 0;
 
   // 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.
-  virtual bool IsTrajectoryFrozen(int trajectory_id) = 0;
+  virtual bool IsTrajectoryFrozen(int trajectory_id) const = 0;
 
   // Returns the trajectory data.
-  virtual std::map<int, TrajectoryData> GetTrajectoryData() = 0;
+  virtual std::map<int, TrajectoryData> GetTrajectoryData() const = 0;
 
   // Returns the collection of constraints.
-  virtual std::vector<Constraint> constraints() = 0;
+  virtual std::vector<Constraint> constraints() const = 0;
 
   // Serializes the constraints and trajectories.
-  virtual proto::PoseGraph ToProto() = 0;
+  virtual proto::PoseGraph ToProto() const = 0;
 };
 
 }  // namespace mapping