diff --git a/cartographer/mapping/global_trajectory_builder.h b/cartographer/mapping/global_trajectory_builder.h
index c45f7df..9a7a60c 100644
--- a/cartographer/mapping/global_trajectory_builder.h
+++ b/cartographer/mapping/global_trajectory_builder.h
@@ -51,9 +51,8 @@ class GlobalTrajectoryBuilder
     if (insertion_result == nullptr) {
       return;
     }
-    sparse_pose_graph_->AddScan(
-        insertion_result->constant_data, insertion_result->pose_observation,
-        trajectory_id_, insertion_result->insertion_submaps);
+    sparse_pose_graph_->AddScan(insertion_result->constant_data, trajectory_id_,
+                                insertion_result->insertion_submaps);
   }
 
   void AddSensorData(const sensor::ImuData& imu_data) override {
diff --git a/cartographer/mapping/proto/trajectory_node_data.proto b/cartographer/mapping/proto/trajectory_node_data.proto
index 8cbc528..533cc87 100644
--- a/cartographer/mapping/proto/trajectory_node_data.proto
+++ b/cartographer/mapping/proto/trajectory_node_data.proto
@@ -28,4 +28,5 @@ message TrajectoryNodeData {
   optional sensor.proto.CompressedPointCloud high_resolution_point_cloud = 4;
   optional sensor.proto.CompressedPointCloud low_resolution_point_cloud = 5;
   repeated float rotational_scan_matcher_histogram = 6;
+  optional transform.proto.Rigid3d initial_pose = 7;
 }
diff --git a/cartographer/mapping/trajectory_node.cc b/cartographer/mapping/trajectory_node.cc
index ad2325a..850a1ab 100644
--- a/cartographer/mapping/trajectory_node.cc
+++ b/cartographer/mapping/trajectory_node.cc
@@ -44,6 +44,8 @@ proto::TrajectoryNodeData ToProto(const TrajectoryNode::Data& constant_data) {
     proto.add_rotational_scan_matcher_histogram(
         constant_data.rotational_scan_matcher_histogram(i));
   }
+  *proto.mutable_initial_pose() =
+      transform::ToProto(constant_data.initial_pose);
   return proto;
 }
 
@@ -63,7 +65,8 @@ TrajectoryNode::Data FromProto(const proto::TrajectoryNodeData& proto) {
           .Decompress(),
       sensor::CompressedPointCloud(proto.low_resolution_point_cloud())
           .Decompress(),
-      rotational_scan_matcher_histogram};
+      rotational_scan_matcher_histogram,
+      transform::ToRigid3(proto.initial_pose())};
 }
 
 }  // namespace mapping
diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h
index 95c659b..0687bbe 100644
--- a/cartographer/mapping/trajectory_node.h
+++ b/cartographer/mapping/trajectory_node.h
@@ -45,6 +45,9 @@ struct TrajectoryNode {
     sensor::PointCloud high_resolution_point_cloud;
     sensor::PointCloud low_resolution_point_cloud;
     Eigen::VectorXf rotational_scan_matcher_histogram;
+
+    // The initial unoptimized node pose.
+    transform::Rigid3d initial_pose;
   };
 
   common::Time time() const { return constant_data->time; }
diff --git a/cartographer/mapping/trajectory_node_test.cc b/cartographer/mapping/trajectory_node_test.cc
index 141e288..658daa4 100644
--- a/cartographer/mapping/trajectory_node_test.cc
+++ b/cartographer/mapping/trajectory_node_test.cc
@@ -21,6 +21,7 @@
 #include "Eigen/Core"
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/proto/trajectory_node_data.pb.h"
+#include "cartographer/transform/rigid_transform_test_helpers.h"
 #include "gtest/gtest.h"
 
 namespace cartographer {
@@ -36,7 +37,8 @@ TEST(TrajectoryNodeTest, ToAndFromProto) {
       sensor::CompressedPointCloud({{2.f, 3.f, 4.f}}).Decompress(),
       sensor::CompressedPointCloud({{-1.f, 2.f, 0.f}}).Decompress(),
       Eigen::VectorXf::Unit(20, 4),
-  };
+      transform::Rigid3d({1., 2., 3.},
+                         Eigen::Quaterniond(4., 5., -6., -7.).normalized())};
   const proto::TrajectoryNodeData proto = ToProto(expected);
   const TrajectoryNode::Data actual = FromProto(proto);
   EXPECT_EQ(expected.time, actual.time);
@@ -49,6 +51,8 @@ TEST(TrajectoryNodeTest, ToAndFromProto) {
             actual.low_resolution_point_cloud);
   EXPECT_EQ(expected.rotational_scan_matcher_histogram,
             actual.rotational_scan_matcher_histogram);
+  EXPECT_THAT(actual.initial_pose,
+              transform::IsNearly(expected.initial_pose, 1e-9));
 }
 
 }  // namespace
diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc
index 955b863..a70d37e 100644
--- a/cartographer/mapping_2d/local_trajectory_builder.cc
+++ b/cartographer/mapping_2d/local_trajectory_builder.cc
@@ -184,8 +184,9 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
               filtered_gravity_aligned_point_cloud,
               {},  // 'high_resolution_point_cloud' is only used in 3D.
               {},  // 'low_resolution_point_cloud' is only used in 3D.
-          }),
-      pose_estimate, std::move(insertion_submaps)});
+              {},  // 'rotational_scan_matcher_histogram' is only used in 3D.
+              pose_estimate}),
+      std::move(insertion_submaps)});
 }
 
 const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {
diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h
index 447c6a2..0a6fef6 100644
--- a/cartographer/mapping_2d/local_trajectory_builder.h
+++ b/cartographer/mapping_2d/local_trajectory_builder.h
@@ -42,7 +42,6 @@ class LocalTrajectoryBuilder {
  public:
   struct InsertionResult {
     std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
-    transform::Rigid3d pose_observation;
     std::vector<std::shared_ptr<const Submap>> insertion_submaps;
   };
 
diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc
index 7dad55b..3f8831f 100644
--- a/cartographer/mapping_2d/sparse_pose_graph.cc
+++ b/cartographer/mapping_2d/sparse_pose_graph.cc
@@ -96,10 +96,10 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
 
 void SparsePoseGraph::AddScan(
     std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
-    const transform::Rigid3d& pose, const int trajectory_id,
+    const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
-      GetLocalToGlobalTransform(trajectory_id) * pose);
+      GetLocalToGlobalTransform(trajectory_id) * constant_data->initial_pose);
 
   common::MutexLocker locker(&mutex_);
   trajectory_nodes_.Append(
@@ -132,11 +132,8 @@ void SparsePoseGraph::AddScan(
   // execute the lambda.
   const bool newly_finished_submap = insertion_submaps.front()->finished();
   AddWorkItem([=]() REQUIRES(mutex_) {
-    ComputeConstraintsForScan(
-        trajectory_id, insertion_submaps, newly_finished_submap,
-        transform::Project2D(pose *
-                             transform::Rigid3d::Rotation(
-                                 constant_data->gravity_alignment.inverse())));
+    ComputeConstraintsForScan(trajectory_id, insertion_submaps,
+                              newly_finished_submap);
   });
 }
 
@@ -227,11 +224,27 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
 void SparsePoseGraph::ComputeConstraintsForScan(
     const int trajectory_id,
     std::vector<std::shared_ptr<const Submap>> insertion_submaps,
-    const bool newly_finished_submap, const transform::Rigid2d& pose) {
+    const bool newly_finished_submap) {
   const std::vector<mapping::SubmapId> submap_ids =
       GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps);
   CHECK_EQ(submap_ids.size(), insertion_submaps.size());
   const mapping::SubmapId matching_id = submap_ids.front();
+  const mapping::NodeId node_id{
+      matching_id.trajectory_id,
+      static_cast<size_t>(matching_id.trajectory_id) <
+          optimization_problem_.node_data().size() &&
+          !optimization_problem_.node_data()[matching_id.trajectory_id]
+              .empty()
+      ? static_cast<int>(optimization_problem_.node_data()
+          .at(matching_id.trajectory_id)
+          .rbegin()
+          ->first +
+          1)
+      : 0};
+  const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
+  const transform::Rigid2d pose = transform::Project2D(
+      constant_data->initial_pose *
+      transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
   const transform::Rigid2d optimized_pose =
       optimization_problem_.submap_data()
           .at(matching_id.trajectory_id)
@@ -240,22 +253,9 @@ void SparsePoseGraph::ComputeConstraintsForScan(
       sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front())
           .inverse() *
       pose;
-  const mapping::NodeId node_id{
-      matching_id.trajectory_id,
-      static_cast<size_t>(matching_id.trajectory_id) <
-                  optimization_problem_.node_data().size() &&
-              !optimization_problem_.node_data()[matching_id.trajectory_id]
-                   .empty()
-          ? static_cast<int>(optimization_problem_.node_data()
-                                 .at(matching_id.trajectory_id)
-                                 .rbegin()
-                                 ->first +
-                             1)
-          : 0};
-  const auto& node_data = trajectory_nodes_.at(node_id).constant_data;
-  optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
-                                          node_data->time, pose, optimized_pose,
-                                          node_data->gravity_alignment);
+  optimization_problem_.AddTrajectoryNode(
+      matching_id.trajectory_id, constant_data->time, pose, optimized_pose,
+      constant_data->gravity_alignment);
   for (size_t i = 0; i < insertion_submaps.size(); ++i) {
     const mapping::SubmapId submap_id = submap_ids[i];
     // Even if this was the last scan added to 'submap_id', the submap will only
diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h
index 965cb60..650a486 100644
--- a/cartographer/mapping_2d/sparse_pose_graph.h
+++ b/cartographer/mapping_2d/sparse_pose_graph.h
@@ -71,7 +71,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // 'true', this submap was inserted into for the last time.
   void AddScan(
       std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
-      const transform::Rigid3d& pose, int trajectory_id,
+      int trajectory_id,
       const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
       EXCLUDES(mutex_);
 
@@ -130,8 +130,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   void ComputeConstraintsForScan(
       int trajectory_id,
       std::vector<std::shared_ptr<const Submap>> insertion_submaps,
-      bool newly_finished_submap, const transform::Rigid2d& pose)
-      REQUIRES(mutex_);
+      bool newly_finished_submap) REQUIRES(mutex_);
 
   // Computes constraints for a scan and submap pair.
   void ComputeConstraint(const mapping::NodeId& node_id,
diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc
index 07d9074..cbb39cc 100644
--- a/cartographer/mapping_2d/sparse_pose_graph_test.cc
+++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc
@@ -164,8 +164,9 @@ class SparsePoseGraphTest : public ::testing::Test {
                                           range_data.returns,
                                           {},
                                           {},
-                                          {}}),
-        transform::Embed3D(pose_estimate), kTrajectoryId, insertion_submaps);
+                                          {},
+                                          transform::Embed3D(pose_estimate)}),
+        kTrajectoryId, insertion_submaps);
   }
 
   void MoveRelative(const transform::Rigid2d& movement) {
diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc
index cede6be..a3c3215 100644
--- a/cartographer/mapping_3d/local_trajectory_builder.cc
+++ b/cartographer/mapping_3d/local_trajectory_builder.cc
@@ -221,8 +221,9 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
               {},  // 'filtered_point_cloud' is only used in 2D.
               high_resolution_point_cloud,
               low_resolution_point_cloud,
-              rotational_scan_matcher_histogram}),
-      pose_observation, std::move(insertion_submaps)});
+              rotational_scan_matcher_histogram,
+              pose_observation}),
+      std::move(insertion_submaps)});
 }
 
 }  // namespace mapping_3d
diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h
index 103c119..81398a6 100644
--- a/cartographer/mapping_3d/local_trajectory_builder.h
+++ b/cartographer/mapping_3d/local_trajectory_builder.h
@@ -42,7 +42,6 @@ class LocalTrajectoryBuilder {
  public:
   struct InsertionResult {
     std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
-    transform::Rigid3d pose_observation;
     std::vector<std::shared_ptr<const Submap>> insertion_submaps;
   };
 
diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc
index 4964085..9ebe63f 100644
--- a/cartographer/mapping_3d/sparse_pose_graph.cc
+++ b/cartographer/mapping_3d/sparse_pose_graph.cc
@@ -94,10 +94,10 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
 
 void SparsePoseGraph::AddScan(
     std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
-    const transform::Rigid3d& pose, const int trajectory_id,
+    const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
-      GetLocalToGlobalTransform(trajectory_id) * pose);
+      GetLocalToGlobalTransform(trajectory_id) * constant_data->initial_pose);
 
   common::MutexLocker locker(&mutex_);
   trajectory_nodes_.Append(
@@ -131,7 +131,7 @@ void SparsePoseGraph::AddScan(
   const bool newly_finished_submap = insertion_submaps.front()->finished();
   AddWorkItem([=]() REQUIRES(mutex_) {
     ComputeConstraintsForScan(trajectory_id, insertion_submaps,
-                              newly_finished_submap, pose);
+                              newly_finished_submap);
   });
 }
 
@@ -250,32 +250,33 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
 void SparsePoseGraph::ComputeConstraintsForScan(
     const int trajectory_id,
     std::vector<std::shared_ptr<const Submap>> insertion_submaps,
-    const bool newly_finished_submap, const transform::Rigid3d& pose) {
+    const bool newly_finished_submap) {
   const std::vector<mapping::SubmapId> submap_ids =
       GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps);
   CHECK_EQ(submap_ids.size(), insertion_submaps.size());
   const mapping::SubmapId matching_id = submap_ids.front();
+  const mapping::NodeId node_id{
+      matching_id.trajectory_id,
+      static_cast<size_t>(matching_id.trajectory_id) <
+          optimization_problem_.node_data().size() &&
+          !optimization_problem_.node_data()[matching_id.trajectory_id]
+              .empty()
+      ? static_cast<int>(optimization_problem_.node_data()
+          .at(matching_id.trajectory_id)
+          .rbegin()
+          ->first +
+          1)
+      : 0};
+  const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
+  const transform::Rigid3d& pose = constant_data->initial_pose;
   const transform::Rigid3d optimized_pose =
       optimization_problem_.submap_data()
           .at(matching_id.trajectory_id)
           .at(matching_id.submap_index)
           .pose *
       insertion_submaps.front()->local_pose().inverse() * pose;
-  const mapping::NodeId node_id{
-      matching_id.trajectory_id,
-      static_cast<size_t>(matching_id.trajectory_id) <
-                  optimization_problem_.node_data().size() &&
-              !optimization_problem_.node_data()[matching_id.trajectory_id]
-                   .empty()
-          ? static_cast<int>(optimization_problem_.node_data()
-                                 .at(matching_id.trajectory_id)
-                                 .rbegin()
-                                 ->first +
-                             1)
-          : 0};
-  const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
   optimization_problem_.AddTrajectoryNode(
-      matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
+      matching_id.trajectory_id, constant_data->time, pose, optimized_pose);
   for (size_t i = 0; i < insertion_submaps.size(); ++i) {
     const mapping::SubmapId submap_id = submap_ids[i];
     // Even if this was the last scan added to 'submap_id', the submap will only
diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h
index 6bdcfad..755fc36 100644
--- a/cartographer/mapping_3d/sparse_pose_graph.h
+++ b/cartographer/mapping_3d/sparse_pose_graph.h
@@ -72,7 +72,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // 'true', this submap was inserted into for the last time.
   void AddScan(
       std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
-      const transform::Rigid3d& pose, int trajectory_id,
+      int trajectory_id,
       const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
       EXCLUDES(mutex_);
 
@@ -131,8 +131,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   void ComputeConstraintsForScan(
       int trajectory_id,
       std::vector<std::shared_ptr<const Submap>> insertion_submaps,
-      bool newly_finished_submap, const transform::Rigid3d& pose)
-      REQUIRES(mutex_);
+      bool newly_finished_submap) REQUIRES(mutex_);
 
   // Computes constraints for a scan and submap pair.
   void ComputeConstraint(const mapping::NodeId& node_id,