diff --git a/cartographer/kalman_filter/pose_tracker.h b/cartographer/kalman_filter/pose_tracker.h
index 73b30e0..d0bd5f3 100644
--- a/cartographer/kalman_filter/pose_tracker.h
+++ b/cartographer/kalman_filter/pose_tracker.h
@@ -122,6 +122,10 @@ class PoseTracker {
 
   const mapping::OdometryStateTracker::OdometryStates& odometry_states() const;
 
+  Eigen::Quaterniond gravity_orientation() const {
+    return imu_tracker_.orientation();
+  }
+
  private:
   // Returns the distribution required to initialize the KalmanFilter.
   static Distribution KalmanFilterInit();
diff --git a/cartographer/mapping/imu_tracker.h b/cartographer/mapping/imu_tracker.h
index 2e0aa6a..a1a755c 100644
--- a/cartographer/mapping/imu_tracker.h
+++ b/cartographer/mapping/imu_tracker.h
@@ -41,7 +41,7 @@ class ImuTracker {
       const Eigen::Vector3d& imu_angular_velocity);
 
   // Query the current orientation estimate.
-  Eigen::Quaterniond orientation() { return orientation_; }
+  Eigen::Quaterniond orientation() const { return orientation_; }
 
  private:
   const double imu_gravity_time_constant_;
diff --git a/cartographer/mapping/submaps.h b/cartographer/mapping/submaps.h
index 332d8b8..1e85a4f 100644
--- a/cartographer/mapping/submaps.h
+++ b/cartographer/mapping/submaps.h
@@ -52,19 +52,15 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) {
   return value;
 }
 
-// An individual submap, which has an initial position 'origin', keeps track of
-// how many range data were inserted into it, and sets the
+// An individual submap, which has a 'local_pose' in the local SLAM frame, keeps
+// track of how many range data were inserted into it, and sets the
 // 'finished_probability_grid' to be used for loop closing once the map no
 // longer changes.
 struct Submap {
-  Submap(const Eigen::Vector3f& origin) : origin(origin) {}
+  Submap(const transform::Rigid3d& local_pose) : local_pose(local_pose) {}
 
-  transform::Rigid3d local_pose() const {
-    return transform::Rigid3d::Translation(origin.cast<double>());
-  }
-
-  // Origin of this submap.
-  const Eigen::Vector3f origin;
+  // Local SLAM pose of this submap.
+  const transform::Rigid3d local_pose;
 
   // Number of RangeData inserted.
   int num_range_data = 0;
diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc
index 155be5e..d699d39 100644
--- a/cartographer/mapping_2d/sparse_pose_graph.cc
+++ b/cartographer/mapping_2d/sparse_pose_graph.cc
@@ -441,8 +441,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
   return extrapolated_submap_transforms.back() *
          submap_states_.at(trajectory_id)
              .at(extrapolated_submap_transforms.size() - 1)
-             .submap->local_pose()
-             .inverse();
+             .submap->local_pose.inverse();
 }
 
 std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
@@ -476,14 +475,13 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
     } else if (result.empty()) {
       result.push_back(transform::Rigid3d::Identity());
     } else {
-      // Extrapolate to the remaining submaps. Accessing local_pose() in Submaps
+      // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps
       // is okay, since the member is const.
       result.push_back(result.back() *
                        submap_states_.at(trajectory_id)
                            .at(result.size() - 1)
-                           .submap->local_pose()
-                           .inverse() *
-                       state.submap->local_pose());
+                           .submap->local_pose.inverse() *
+                       state.submap->local_pose);
     }
   }
 
diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
index eb3a820..d602b91 100644
--- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
+++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
@@ -40,7 +40,7 @@ namespace mapping_2d {
 namespace sparse_pose_graph {
 
 transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap) {
-  return transform::Project2D(submap.local_pose());
+  return transform::Project2D(submap.local_pose);
 }
 
 ConstraintBuilder::ConstraintBuilder(
diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc
index 1ea41a2..9dfa001 100644
--- a/cartographer/mapping_2d/submaps.cc
+++ b/cartographer/mapping_2d/submaps.cc
@@ -102,7 +102,8 @@ proto::SubmapsOptions CreateSubmapsOptions(
 }
 
 Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin)
-    : mapping::Submap(Eigen::Vector3f(origin.x(), origin.y(), 0.)),
+    : mapping::Submap(transform::Rigid3d::Translation(
+          Eigen::Vector3d(origin.x(), origin.y(), 0.))),
       probability_grid(limits) {}
 
 Submaps::Submaps(const proto::SubmapsOptions& options)
@@ -137,7 +138,7 @@ int Submaps::size() const { return submaps_.size(); }
 void Submaps::SubmapToProto(
     const int index, const transform::Rigid3d&,
     mapping::proto::SubmapQuery::Response* const response) const {
-  AddProbabilityGridToResponse(Get(index)->local_pose(),
+  AddProbabilityGridToResponse(Get(index)->local_pose,
                                Get(index)->probability_grid, response);
 }
 
diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc
index 69cb072..eea1691 100644
--- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc
+++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc
@@ -143,7 +143,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
   const Submap* const matching_submap =
       submaps_->Get(submaps_->matching_index());
   transform::Rigid3d initial_ceres_pose =
-      matching_submap->local_pose().inverse() * pose_prediction;
+      matching_submap->local_pose.inverse() * pose_prediction;
   sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
       options_.high_resolution_adaptive_voxel_filter_options());
   const sensor::PointCloud filtered_point_cloud_in_tracking =
@@ -171,7 +171,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
                                &matching_submap->low_resolution_hybrid_grid}},
                              &pose_observation_in_submap, &summary);
   const transform::Rigid3d pose_observation =
-      matching_submap->local_pose() * pose_observation_in_submap;
+      matching_submap->local_pose * pose_observation_in_submap;
   pose_tracker_->AddPoseObservation(
       time, pose_observation,
       options_.kalman_local_trajectory_builder_options()
@@ -227,8 +227,10 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
   for (int insertion_index : submaps_->insertion_indices()) {
     insertion_submaps.push_back(submaps_->Get(insertion_index));
   }
-  submaps_->InsertRangeData(sensor::TransformRangeData(
-      range_data_in_tracking, pose_observation.cast<float>()));
+  submaps_->InsertRangeData(
+      sensor::TransformRangeData(range_data_in_tracking,
+                                 pose_observation.cast<float>()),
+      pose_tracker_->gravity_orientation());
   return std::unique_ptr<InsertionResult>(new InsertionResult{
       time, range_data_in_tracking, pose_observation, covariance_estimate,
       matching_submap, insertion_submaps});
diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
index ab1c927..61c215c 100644
--- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
+++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
@@ -242,7 +242,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
   // We transform the states in 'batches_' in place to be in the submap frame as
   // expected by the OccupiedSpaceCostFunctor. This is reverted after solving
   // the optimization problem.
-  TransformStates(matching_submap->local_pose().inverse());
+  TransformStates(matching_submap->local_pose.inverse());
   for (size_t i = 0; i < batches_.size(); ++i) {
     Batch& batch = batches_[i];
     problem.AddResidualBlock(
@@ -351,7 +351,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
   ceres::Solve(ceres_solver_options_, &problem, &summary);
   // The optimized states in 'batches_' are in the submap frame and we transform
   // them in place to be in the local SLAM frame again.
-  TransformStates(matching_submap->local_pose());
+  TransformStates(matching_submap->local_pose);
   if (num_accumulated_ < options_.scans_per_accumulation()) {
     return nullptr;
   }
@@ -416,8 +416,13 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
   for (int insertion_index : submaps_->insertion_indices()) {
     insertion_submaps.push_back(submaps_->Get(insertion_index));
   }
-  submaps_->InsertRangeData(sensor::TransformRangeData(
-      range_data_in_tracking, pose_observation.cast<float>()));
+  // TODO(whess): Use an ImuTracker to track the gravity direction.
+  const Eigen::Quaterniond kFakeGravityOrientation =
+      Eigen::Quaterniond::Identity();
+  submaps_->InsertRangeData(
+      sensor::TransformRangeData(range_data_in_tracking,
+                                 pose_observation.cast<float>()),
+      kFakeGravityOrientation);
 
   const kalman_filter::PoseCovariance kCovariance =
       1e-7 * kalman_filter::PoseCovariance::Identity();
diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc
index 686c2c2..2f87f39 100644
--- a/cartographer/mapping_3d/sparse_pose_graph.cc
+++ b/cartographer/mapping_3d/sparse_pose_graph.cc
@@ -81,8 +81,8 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
         submap_data.at(trajectory_id).at(first_submap_id.submap_index).pose;
     optimization_problem_.AddSubmap(
         trajectory_id, first_submap_pose *
-                           insertion_submaps[0]->local_pose().inverse() *
-                           insertion_submaps[1]->local_pose());
+                           insertion_submaps[0]->local_pose.inverse() *
+                           insertion_submaps[1]->local_pose);
   }
 }
 
@@ -164,6 +164,12 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
           .at(submap_id.submap_index)
           .pose.inverse();
 
+  const transform::Rigid3d initial_relative_pose =
+      inverse_submap_pose * optimization_problem_.node_data()
+                                .at(node_id.trajectory_id)
+                                .at(node_id.node_index)
+                                .point_cloud_pose;
+
   std::vector<mapping::TrajectoryNode> submap_nodes;
   for (const mapping::NodeId& submap_node_id :
        submap_states_.at(submap_id.trajectory_id)
@@ -181,6 +187,18 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
   // Only globally match against submaps not in this trajectory.
   if (node_id.trajectory_id != submap_id.trajectory_id &&
       global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
+    // In this situation, 'initial_relative_pose' is:
+    //
+    // submap <- global map 2 <- global map 1 <- tracking
+    //               (agreeing on gravity)
+    //
+    // Since they possibly came from two disconnected trajectories, the only
+    // guaranteed connection between the tracking and the submap frames is
+    // an agreement on the direction of gravity. Therefore, excluding yaw,
+    // 'initial_relative_pose.rotation()' is a good estimate of the relative
+    // orientation of the point cloud in the submap frame. Finding the correct
+    // yaw component will be handled by the matching procedure in the
+    // FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
     constraint_builder_.MaybeAddGlobalConstraint(
         submap_id,
         submap_states_.at(submap_id.trajectory_id)
@@ -190,7 +208,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
         &trajectory_nodes_.at(node_id.trajectory_id)
              .at(node_id.node_index)
              .constant_data->range_data_3d.returns,
-        submap_nodes, &trajectory_connectivity_);
+        submap_nodes, initial_relative_pose.rotation(),
+        &trajectory_connectivity_);
   } else {
     const bool scan_and_submap_trajectories_connected =
         reverse_connected_components_.count(node_id.trajectory_id) > 0 &&
@@ -199,11 +218,6 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
             reverse_connected_components_.at(submap_id.trajectory_id);
     if (node_id.trajectory_id == submap_id.trajectory_id ||
         scan_and_submap_trajectories_connected) {
-      const transform::Rigid3d initial_relative_pose =
-          inverse_submap_pose * optimization_problem_.node_data()
-                                    .at(node_id.trajectory_id)
-                                    .at(node_id.node_index)
-                                    .point_cloud_pose;
       constraint_builder_.MaybeAddConstraint(
           submap_id,
           submap_states_.at(submap_id.trajectory_id)
@@ -242,7 +256,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
           .at(matching_id.trajectory_id)
           .at(matching_id.submap_index)
           .pose *
-      matching_submap->local_pose().inverse() * pose;
+      matching_submap->local_pose.inverse() * pose;
   CHECK_EQ(scan_index, scan_index_to_node_id_.size());
   const mapping::NodeId node_id{
       matching_id.trajectory_id,
@@ -266,7 +280,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
         .node_ids.emplace(node_id);
     // Unchanged covariance as (submap <- map) is a translation.
     const transform::Rigid3d constraint_transform =
-        submap->local_pose().inverse() * pose;
+        submap->local_pose.inverse() * pose;
     constraints_.push_back(
         Constraint{submap_id,
                    node_id,
@@ -447,8 +461,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
   return extrapolated_submap_transforms.back() *
          submap_states_.at(trajectory_id)
              .at(extrapolated_submap_transforms.size() - 1)
-             .submap->local_pose()
-             .inverse();
+             .submap->local_pose.inverse();
 }
 
 std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
@@ -482,14 +495,13 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
     } else if (result.empty()) {
       result.push_back(transform::Rigid3d::Identity());
     } else {
-      // Extrapolate to the remaining submaps. Accessing local_pose() in Submaps
+      // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps
       // is okay, since the member is const.
       result.push_back(result.back() *
                        submap_states_.at(trajectory_id)
                            .at(result.size() - 1)
-                           .submap->local_pose()
-                           .inverse() *
-                       state.submap->local_pose());
+                           .submap->local_pose.inverse() *
+                       state.submap->local_pose);
     }
   }
 
diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
index ef2b4c4..079b6f6 100644
--- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
+++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
@@ -88,6 +88,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
     const mapping::NodeId& node_id,
     const sensor::CompressedPointCloud* const compressed_point_cloud,
     const std::vector<mapping::TrajectoryNode>& submap_nodes,
+    const Eigen::Quaterniond& gravity_alignment,
     mapping::TrajectoryConnectivity* const trajectory_connectivity) {
   common::MutexLocker locker(&mutex_);
   constraints_.emplace_back();
@@ -97,10 +98,10 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
   ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
       submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
       [=]() EXCLUDES(mutex_) {
-        ComputeConstraint(submap_id, submap, node_id,
-                          true, /* match_full_submap */
-                          trajectory_connectivity, compressed_point_cloud,
-                          transform::Rigid3d::Identity(), constraint);
+        ComputeConstraint(
+            submap_id, submap, node_id, true, /* match_full_submap */
+            trajectory_connectivity, compressed_point_cloud,
+            transform::Rigid3d::Rotation(gravity_alignment), constraint);
         FinishComputation(current_computation);
       });
 }
diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h
index 9778b48..91916f3 100644
--- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h
+++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h
@@ -84,7 +84,9 @@ class ConstraintBuilder {
   // 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
   // This performs full-submap matching.
   //
-  // The 'trajectory_connectivity' is updated if the full-submap match succeeds.
+  // The 'gravity_alignment' is the rotation to apply to the point cloud data
+  // to make it approximately gravity aligned. The 'trajectory_connectivity' is
+  // updated if the full-submap match succeeds.
   //
   // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
   // all computations are finished.
@@ -93,6 +95,7 @@ class ConstraintBuilder {
       const mapping::NodeId& node_id,
       const sensor::CompressedPointCloud* compressed_point_cloud,
       const std::vector<mapping::TrajectoryNode>& submap_nodes,
+      const Eigen::Quaterniond& gravity_alignment,
       mapping::TrajectoryConnectivity* trajectory_connectivity);
 
   // Must be called after all computations related to one node have been added.
diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submaps.cc
index f919315..d781acd 100644
--- a/cartographer/mapping_3d/submaps.cc
+++ b/cartographer/mapping_3d/submaps.cc
@@ -217,17 +217,20 @@ proto::SubmapsOptions CreateSubmapsOptions(
 }
 
 Submap::Submap(const float high_resolution, const float low_resolution,
-               const Eigen::Vector3f& origin)
-    : mapping::Submap(origin),
+               const transform::Rigid3d& local_pose)
+    : mapping::Submap(local_pose),
       high_resolution_hybrid_grid(high_resolution),
       low_resolution_hybrid_grid(low_resolution) {}
 
 Submaps::Submaps(const proto::SubmapsOptions& options)
     : options_(options),
       range_data_inserter_(options.range_data_inserter_options()) {
-  // We always want to have at least one likelihood field which we can return,
-  // and will create it at the origin in absence of a better choice.
-  AddSubmap(Eigen::Vector3f::Zero());
+  // We always want to have at least one submap which we can return and will
+  // create it at the origin in absence of a better choice.
+  //
+  // TODO(whess): Start with no submaps, so that all of them can be
+  // approximately gravity aligned.
+  AddSubmap(transform::Rigid3d::Identity());
 }
 
 const Submap* Submaps::Get(int index) const {
@@ -271,11 +274,12 @@ void Submaps::SubmapToProto(
                              global_submap_pose.translation().z())));
 }
 
-void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
+void Submaps::InsertRangeData(const sensor::RangeData& range_data,
+                              const Eigen::Quaterniond& gravity_alignment) {
   for (const int index : insertion_indices()) {
     Submap* submap = submaps_[index].get();
     const sensor::RangeData transformed_range_data = sensor::TransformRangeData(
-        range_data, submap->local_pose().inverse().cast<float>());
+        range_data, submap->local_pose.inverse().cast<float>());
     range_data_inserter_.Insert(
         FilterRangeDataByMaxRange(transformed_range_data,
                                   options_.high_resolution_max_range()),
@@ -286,18 +290,19 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
   }
   const Submap* const last_submap = Get(size() - 1);
   if (last_submap->num_range_data == options_.num_range_data()) {
-    AddSubmap(range_data.origin);
+    AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(),
+                                 gravity_alignment));
   }
 }
 
-void Submaps::AddSubmap(const Eigen::Vector3f& origin) {
+void Submaps::AddSubmap(const transform::Rigid3d& local_pose) {
   if (size() > 1) {
     Submap* submap = submaps_[size() - 2].get();
     CHECK(!submap->finished);
     submap->finished = true;
   }
   submaps_.emplace_back(new Submap(options_.high_resolution(),
-                                   options_.low_resolution(), origin));
+                                   options_.low_resolution(), local_pose));
   LOG(INFO) << "Added submap " << size();
 }
 
diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h
index 08caa5f..8c9a67a 100644
--- a/cartographer/mapping_3d/submaps.h
+++ b/cartographer/mapping_3d/submaps.h
@@ -32,6 +32,7 @@
 #include "cartographer/mapping_3d/proto/submaps_options.pb.h"
 #include "cartographer/mapping_3d/range_data_inserter.h"
 #include "cartographer/sensor/range_data.h"
+#include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
 
 namespace cartographer {
@@ -48,7 +49,7 @@ proto::SubmapsOptions CreateSubmapsOptions(
 
 struct Submap : public mapping::Submap {
   Submap(float high_resolution, float low_resolution,
-         const Eigen::Vector3f& origin);
+         const transform::Rigid3d& local_pose);
 
   HybridGrid high_resolution_hybrid_grid;
   HybridGrid low_resolution_hybrid_grid;
@@ -69,8 +70,11 @@ class Submaps : public mapping::Submaps {
       int index, const transform::Rigid3d& global_submap_pose,
       mapping::proto::SubmapQuery::Response* response) const override;
 
-  // Inserts 'range_data' into the Submap collection.
-  void InsertRangeData(const sensor::RangeData& range_data);
+  // Inserts 'range_data' into the Submap collection. 'gravity_alignment' is
+  // used for the orientation of new submaps so that the z axis approximately
+  // aligns with gravity.
+  void InsertRangeData(const sensor::RangeData& range_data,
+                       const Eigen::Quaterniond& gravity_alignment);
 
  private:
   struct PixelData {
@@ -81,7 +85,7 @@ class Submaps : public mapping::Submaps {
     float max_probability = 0.5f;
   };
 
-  void AddSubmap(const Eigen::Vector3f& origin);
+  void AddSubmap(const transform::Rigid3d& local_pose);
 
   std::vector<PixelData> AccumulatePixelData(
       const int width, const int height, const Eigen::Array2i& min_index,