diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc
index 6a09ede..c3ad9f1 100644
--- a/cartographer/mapping_2d/sparse_pose_graph.cc
+++ b/cartographer/mapping_2d/sparse_pose_graph.cc
@@ -250,9 +250,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
                                  ->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);
+  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);
   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/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
index 49c47ac..27984d1 100644
--- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
+++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
@@ -80,14 +80,16 @@ void OptimizationProblem::AddOdometerData(
 
 void OptimizationProblem::AddTrajectoryNode(
     const int trajectory_id, const common::Time time,
-    const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose) {
+    const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
+    const Eigen::Quaterniond& gravity_alignment) {
   CHECK_GE(trajectory_id, 0);
   node_data_.resize(
       std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
   trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size()));
   auto& trajectory_data = trajectory_data_[trajectory_id];
-  node_data_[trajectory_id].emplace(trajectory_data.next_node_index,
-                                    NodeData{time, initial_pose, pose});
+  node_data_[trajectory_id].emplace(
+      trajectory_data.next_node_index,
+      NodeData{time, initial_pose, pose, gravity_alignment});
   ++trajectory_data.next_node_index;
 }
 
@@ -227,8 +229,13 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
               node_data_[trajectory_id][node_index].time);
       const transform::Rigid3d relative_pose =
           odometry_available
-              ? odometry_data_[trajectory_id].Lookup(node_data.time).inverse() *
-                    odometry_data_[trajectory_id].Lookup(next_node_data.time)
+              ? transform::Rigid3d::Rotation(node_data.gravity_alignment) *
+                    odometry_data_[trajectory_id]
+                        .Lookup(node_data.time)
+                        .inverse() *
+                    odometry_data_[trajectory_id].Lookup(next_node_data.time) *
+                    transform::Rigid3d::Rotation(
+                        next_node_data.gravity_alignment.inverse())
               : transform::Embed3D(node_data.initial_pose.inverse() *
                                    next_node_data.initial_pose);
       problem.AddResidualBlock(
diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
index c5f0b60..2af47b1 100644
--- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
+++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
@@ -41,6 +41,7 @@ struct NodeData {
   common::Time time;
   transform::Rigid2d initial_pose;
   transform::Rigid2d pose;
+  Eigen::Quaterniond gravity_alignment;
 };
 
 struct SubmapData {
@@ -65,7 +66,8 @@ class OptimizationProblem {
                        const sensor::OdometryData& odometry_data);
   void AddTrajectoryNode(int trajectory_id, common::Time time,
                          const transform::Rigid2d& initial_pose,
-                         const transform::Rigid2d& pose);
+                         const transform::Rigid2d& pose,
+                         const Eigen::Quaterniond& gravity_alignment);
   void TrimTrajectoryNode(const mapping::NodeId& node_id);
   void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose);
   void TrimSubmap(const mapping::SubmapId& submap_id);