diff --git a/cartographer/cloud/internal/client/pose_graph_stub.cc b/cartographer/cloud/internal/client/pose_graph_stub.cc
index 70bb921..6765752 100644
--- a/cartographer/cloud/internal/client/pose_graph_stub.cc
+++ b/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";
 }
 
diff --git a/cartographer/cloud/internal/client/pose_graph_stub.h b/cartographer/cloud/internal/client/pose_graph_stub.h
index 4b41fe6..52d4040 100644
--- a/cartographer/cloud/internal/client/pose_graph_stub.h
+++ b/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, SubmapPose> GetAllSubmapPoses() override;
-  transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) override;
+  mapping::MapById<mapping::SubmapId, SubmapData> GetAllSubmapData()
+      const 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;
-  std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override;
+  GetTrajectoryNodePoses() const 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 IsTrajectoryFrozen(int trajectory_id) override;
+  bool IsTrajectoryFinished(int trajectory_id) const override;
+  bool IsTrajectoryFrozen(int trajectory_id) const override;
   std::map<int, mapping::PoseGraphInterface::TrajectoryData> GetTrajectoryData()
-      override;
-  std::vector<Constraint> constraints() override;
-  mapping::proto::PoseGraph ToProto() override;
+      const override;
+  std::vector<Constraint> constraints() const override;
+  mapping::proto::PoseGraph ToProto() const override;
 
  private:
   std::shared_ptr<::grpc::Channel> client_channel_;
diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc
index f98a257..d9a88b7 100644
--- a/cartographer/mapping/internal/2d/pose_graph_2d.cc
+++ b/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,
diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h
index 6844aa9..c271609 100644
--- a/cartographer/mapping/internal/2d/pose_graph_2d.h
+++ b/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;
-  PoseGraphInterface::SubmapData GetSubmapData(const SubmapId& submap_id)
+  std::vector<std::vector<int>> GetConnectedTrajectories() const override;
+  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;
-  transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
+  MapById<SubmapId, SubmapPose> GetAllSubmapPoses() const
       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::OdometryData> GetOdometryData() override
+  sensor::MapByTime<sensor::ImuData> GetImuData() const 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_);
-  std::map<int, TrajectoryData> GetTrajectoryData() override EXCLUDES(mutex_);
-  std::vector<Constraint> constraints() override EXCLUDES(mutex_);
+  GetLandmarkNodes() const override EXCLUDES(mutex_);
+  std::map<int, TrajectoryData> GetTrajectoryData() const 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
@@ -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.
diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc
index e2459ac..0a39366 100644
--- a/cartographer/mapping/internal/3d/pose_graph_3d.cc
+++ b/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,
diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h
index e025d27..b8b9c0d 100644
--- a/cartographer/mapping/internal/3d/pose_graph_3d.h
+++ b/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;
-  PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id)
+  std::vector<std::vector<int>> GetConnectedTrajectories() const override;
+  PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id) const
       EXCLUDES(mutex_) override;
-  MapById<SubmapId, SubmapData> GetAllSubmapData() EXCLUDES(mutex_) override;
-  MapById<SubmapId, SubmapPose> GetAllSubmapPoses() EXCLUDES(mutex_) override;
-  transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
+  MapById<SubmapId, SubmapData> GetAllSubmapData() const
       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::OdometryData> GetOdometryData() override
+  sensor::MapByTime<sensor::ImuData> GetImuData() const 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_);
-  std::map<int, TrajectoryData> GetTrajectoryData() override;
+  GetLandmarkNodes() const override EXCLUDES(mutex_);
+  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_);
diff --git a/cartographer/mapping/internal/connected_components.cc b/cartographer/mapping/internal/connected_components.cc
index b3a4ecd..dca563a 100644
--- a/cartographer/mapping/internal/connected_components.cc
+++ b/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;
diff --git a/cartographer/mapping/internal/testing/mock_pose_graph.h b/cartographer/mapping/internal/testing/mock_pose_graph.h
index 4a54a12..6150409 100644
--- a/cartographer/mapping/internal/testing/mock_pose_graph.h
+++ b/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,
-               mapping::MapById<mapping::SubmapId, SubmapData>());
-  MOCK_METHOD0(GetAllSubmapPoses,
-               mapping::MapById<mapping::SubmapId, SubmapPose>());
-  MOCK_METHOD1(GetLocalToGlobalTransform, transform::Rigid3d(int));
-  MOCK_METHOD0(GetTrajectoryNodes,
-               mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>());
-  MOCK_METHOD0(
+  MOCK_CONST_METHOD0(GetAllSubmapData,
+                     mapping::MapById<mapping::SubmapId, SubmapData>());
+  MOCK_CONST_METHOD0(GetAllSubmapPoses,
+                     mapping::MapById<mapping::SubmapId, SubmapPose>());
+  MOCK_CONST_METHOD1(GetLocalToGlobalTransform, transform::Rigid3d(int));
+  MOCK_CONST_METHOD0(
+      GetTrajectoryNodes,
+      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_METHOD1(IsTrajectoryFrozen, bool(int));
-  MOCK_METHOD0(GetTrajectoryData,
-               std::map<int, mapping::PoseGraphInterface::TrajectoryData>());
-  MOCK_METHOD0(constraints, std::vector<Constraint>());
-  MOCK_METHOD0(ToProto, mapping::proto::PoseGraph());
+  MOCK_CONST_METHOD1(IsTrajectoryFinished, bool(int));
+  MOCK_CONST_METHOD1(IsTrajectoryFrozen, bool(int));
+  MOCK_CONST_METHOD0(
+      GetTrajectoryData,
+      std::map<int, mapping::PoseGraphInterface::TrajectoryData>());
+  MOCK_CONST_METHOD0(constraints, std::vector<Constraint>());
+  MOCK_CONST_METHOD0(ToProto, mapping::proto::PoseGraph());
 };
 
 }  // namespace testing
diff --git a/cartographer/mapping/internal/trajectory_connectivity_state.cc b/cartographer/mapping/internal/trajectory_connectivity_state.cc
index 2a9d57b..73ddf46 100644
--- a/cartographer/mapping/internal/trajectory_connectivity_state.cc
+++ b/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];
 }
 
diff --git a/cartographer/mapping/internal/trajectory_connectivity_state.h b/cartographer/mapping/internal/trajectory_connectivity_state.h
index 1f7c386..f7b2069 100644
--- a/cartographer/mapping/internal/trajectory_connectivity_state.h
+++ b/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
diff --git a/cartographer/mapping/pose_graph.cc b/cartographer/mapping/pose_graph.cc
index fe2fad2..c45c2b4 100644
--- a/cartographer/mapping/pose_graph.cc
+++ b/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;
diff --git a/cartographer/mapping/pose_graph.h b/cartographer/mapping/pose_graph.h
index 5cfa18c..833582b 100644
--- a/cartographer/mapping/pose_graph.h
+++ b/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>
-  GetFixedFramePoseData() = 0;
+  virtual sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData()
+      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'.
diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h
index e17ef5f..5642185 100644
--- a/cartographer/mapping/pose_graph_interface.h
+++ b/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