diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
index e5e5418..0d1f6ac 100644
--- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
+++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
@@ -30,7 +30,6 @@
 #include "cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h"
 #include "cartographer/sensor/odometry_data.h"
 #include "cartographer/transform/transform.h"
-#include "cartographer/transform/transform_interpolation_buffer.h"
 #include "ceres/ceres.h"
 #include "glog/logging.h"
 
@@ -69,10 +68,7 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
 
 void OptimizationProblem::AddOdometerData(
     const int trajectory_id, const sensor::OdometryData& odometry_data) {
-  CHECK_GE(trajectory_id, 0);
-  odometry_data_.resize(
-      std::max(odometry_data_.size(), static_cast<size_t>(trajectory_id) + 1));
-  odometry_data_[trajectory_id].Push(odometry_data.time, odometry_data.pose);
+  odometry_data_.Append(trajectory_id, odometry_data);
 }
 
 void OptimizationProblem::AddTrajectoryNode(
@@ -93,6 +89,7 @@ void OptimizationProblem::InsertTrajectoryNode(
 
 void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
   imu_data_.Trim(node_data_, node_id);
+  odometry_data_.Trim(node_data_, node_id);
   node_data_.Trim(node_id);
 }
 
@@ -184,23 +181,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
         continue;
       }
 
-      const bool odometry_available =
-          trajectory_id < static_cast<int>(odometry_data_.size()) &&
-          odometry_data_[trajectory_id].Has(second_node_data.time) &&
-          odometry_data_[trajectory_id].Has(first_node_data.time);
       const transform::Rigid3d relative_pose =
-          odometry_available
-              ? transform::Rigid3d::Rotation(
-                    first_node_data.gravity_alignment) *
-                    odometry_data_[trajectory_id]
-                        .Lookup(first_node_data.time)
-                        .inverse() *
-                    odometry_data_[trajectory_id].Lookup(
-                        second_node_data.time) *
-                    transform::Rigid3d::Rotation(
-                        second_node_data.gravity_alignment.inverse())
-              : transform::Embed3D(first_node_data.initial_pose.inverse() *
-                                   second_node_data.initial_pose);
+          ComputeRelativePose(trajectory_id, first_node_data, second_node_data);
       problem.AddResidualBlock(
           new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
               new SpaCostFunction(Constraint::Pose{
@@ -246,6 +228,44 @@ const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
   return imu_data_;
 }
 
+std::unique_ptr<transform::Rigid3d> OptimizationProblem::InterpolateOdometry(
+    const int trajectory_id, const common::Time time) const {
+  const auto it = odometry_data_.lower_bound(trajectory_id, time);
+  if (it == odometry_data_.EndOfTrajectory(trajectory_id)) {
+    return nullptr;
+  }
+  if (it == odometry_data_.BeginOfTrajectory(trajectory_id)) {
+    if (it->time == time) {
+      return common::make_unique<transform::Rigid3d>(it->pose);
+    }
+    return nullptr;
+  }
+  const auto prev_it = std::prev(it);
+  return common::make_unique<transform::Rigid3d>(
+      Interpolate(transform::TimestampedTransform{prev_it->time, prev_it->pose},
+                  transform::TimestampedTransform{it->time, it->pose}, time)
+          .transform);
+}
+
+transform::Rigid3d OptimizationProblem::ComputeRelativePose(
+    const int trajectory_id, const NodeData& first_node_data,
+    const NodeData& second_node_data) const {
+  if (odometry_data_.HasTrajectory(trajectory_id)) {
+    const std::unique_ptr<transform::Rigid3d> first_node_odometry =
+        InterpolateOdometry(trajectory_id, first_node_data.time);
+    const std::unique_ptr<transform::Rigid3d> second_node_odometry =
+        InterpolateOdometry(trajectory_id, second_node_data.time);
+    if (first_node_odometry != nullptr && second_node_odometry != nullptr) {
+      return transform::Rigid3d::Rotation(first_node_data.gravity_alignment) *
+             first_node_odometry->inverse() * (*second_node_odometry) *
+             transform::Rigid3d::Rotation(
+                 second_node_data.gravity_alignment.inverse());
+    }
+  }
+  return transform::Embed3D(first_node_data.initial_pose.inverse() *
+                            second_node_data.initial_pose);
+}
+
 }  // namespace sparse_pose_graph
 }  // namespace mapping_2d
 }  // namespace cartographer
diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
index b91798f..2859a17 100644
--- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
+++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
@@ -33,7 +33,7 @@
 #include "cartographer/sensor/imu_data.h"
 #include "cartographer/sensor/map_by_time.h"
 #include "cartographer/sensor/odometry_data.h"
-#include "cartographer/transform/transform_interpolation_buffer.h"
+#include "cartographer/transform/timestamped_transform.h"
 
 namespace cartographer {
 namespace mapping_2d {
@@ -92,11 +92,18 @@ class OptimizationProblem {
   const sensor::MapByTime<sensor::ImuData>& imu_data() const;
 
  private:
+  std::unique_ptr<transform::Rigid3d> InterpolateOdometry(
+      int trajectory_id, common::Time time) const;
+  // Uses odometry if available, otherwise the local SLAM results.
+  transform::Rigid3d ComputeRelativePose(
+      int trajectory_id, const NodeData& first_node_data,
+      const NodeData& second_node_data) const;
+
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
-  sensor::MapByTime<sensor::ImuData> imu_data_;
   mapping::MapById<mapping::NodeId, NodeData> node_data_;
-  std::vector<transform::TransformInterpolationBuffer> odometry_data_;
   mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
+  sensor::MapByTime<sensor::ImuData> imu_data_;
+  sensor::MapByTime<sensor::OdometryData> odometry_data_;
 };
 
 }  // namespace sparse_pose_graph
diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
index d4d6604..78e7e35 100644
--- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
+++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
@@ -36,6 +36,7 @@
 #include "cartographer/mapping_3d/rotation_cost_function.h"
 #include "cartographer/mapping_3d/rotation_parameterization.h"
 #include "cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h"
+#include "cartographer/transform/timestamped_transform.h"
 #include "cartographer/transform/transform.h"
 #include "ceres/ceres.h"
 #include "ceres/jet.h"
@@ -61,10 +62,7 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
 
 void OptimizationProblem::AddOdometerData(
     const int trajectory_id, const sensor::OdometryData& odometry_data) {
-  CHECK_GE(trajectory_id, 0);
-  odometry_data_.resize(
-      std::max(odometry_data_.size(), static_cast<size_t>(trajectory_id) + 1));
-  odometry_data_[trajectory_id].Push(odometry_data.time, odometry_data.pose);
+  odometry_data_.Append(trajectory_id, odometry_data);
 }
 
 void OptimizationProblem::AddFixedFramePoseData(
@@ -99,6 +97,7 @@ void OptimizationProblem::InsertTrajectoryNode(
 
 void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
   imu_data_.Trim(node_data_, node_id);
+  odometry_data_.Trim(node_data_, node_id);
   node_data_.Trim(node_id);
 }
 
@@ -323,18 +322,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
           continue;
         }
 
-        const bool odometry_available =
-            trajectory_id < static_cast<int>(odometry_data_.size()) &&
-            odometry_data_[trajectory_id].Has(second_node_data.time) &&
-            odometry_data_[trajectory_id].Has(first_node_data.time);
-        const transform::Rigid3d relative_pose =
-            odometry_available ? odometry_data_[trajectory_id]
-                                         .Lookup(first_node_data.time)
-                                         .inverse() *
-                                     odometry_data_[trajectory_id].Lookup(
-                                         second_node_data.time)
-                               : first_node_data.local_pose.inverse() *
-                                     second_node_data.local_pose;
+        const transform::Rigid3d relative_pose = ComputeRelativePose(
+            trajectory_id, first_node_data, second_node_data);
         problem.AddResidualBlock(
             new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
                 new SpaCostFunction(Constraint::Pose{
@@ -447,6 +436,40 @@ const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
   return imu_data_;
 }
 
+std::unique_ptr<transform::Rigid3d> OptimizationProblem::InterpolateOdometry(
+    const int trajectory_id, const common::Time time) const {
+  const auto it = odometry_data_.lower_bound(trajectory_id, time);
+  if (it == odometry_data_.EndOfTrajectory(trajectory_id)) {
+    return nullptr;
+  }
+  if (it == odometry_data_.BeginOfTrajectory(trajectory_id)) {
+    if (it->time == time) {
+      return common::make_unique<transform::Rigid3d>(it->pose);
+    }
+    return nullptr;
+  }
+  const auto prev_it = std::prev(it);
+  return common::make_unique<transform::Rigid3d>(
+      Interpolate(transform::TimestampedTransform{prev_it->time, prev_it->pose},
+                  transform::TimestampedTransform{it->time, it->pose}, time)
+          .transform);
+}
+
+transform::Rigid3d OptimizationProblem::ComputeRelativePose(
+    const int trajectory_id, const NodeData& first_node_data,
+    const NodeData& second_node_data) const {
+  if (odometry_data_.HasTrajectory(trajectory_id)) {
+    const std::unique_ptr<transform::Rigid3d> first_node_odometry =
+        InterpolateOdometry(trajectory_id, first_node_data.time);
+    const std::unique_ptr<transform::Rigid3d> second_node_odometry =
+        InterpolateOdometry(trajectory_id, second_node_data.time);
+    if (first_node_odometry != nullptr && second_node_odometry != nullptr) {
+      return first_node_odometry->inverse() * (*second_node_odometry);
+    }
+  }
+  return first_node_data.local_pose.inverse() * second_node_data.local_pose;
+}
+
 }  // namespace sparse_pose_graph
 }  // namespace mapping_3d
 }  // namespace cartographer
diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
index ddbd087..b79329e 100644
--- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
+++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
@@ -96,6 +96,13 @@ class OptimizationProblem {
   const sensor::MapByTime<sensor::ImuData>& imu_data() const;
 
  private:
+  std::unique_ptr<transform::Rigid3d> InterpolateOdometry(
+      int trajectory_id, common::Time time) const;
+  // Uses odometry if available, otherwise the local SLAM results.
+  transform::Rigid3d ComputeRelativePose(
+      int trajectory_id, const NodeData& first_node_data,
+      const NodeData& second_node_data) const;
+
   struct TrajectoryData {
     double gravity_constant = 9.8;
     std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
@@ -103,10 +110,10 @@ class OptimizationProblem {
 
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
   FixZ fix_z_;
-  sensor::MapByTime<sensor::ImuData> imu_data_;
   mapping::MapById<mapping::NodeId, NodeData> node_data_;
-  std::vector<transform::TransformInterpolationBuffer> odometry_data_;
   mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
+  sensor::MapByTime<sensor::ImuData> imu_data_;
+  sensor::MapByTime<sensor::OdometryData> odometry_data_;
   std::vector<TrajectoryData> trajectory_data_;
   std::vector<transform::TransformInterpolationBuffer> fixed_frame_pose_data_;
 };
diff --git a/cartographer/sensor/map_by_time.h b/cartographer/sensor/map_by_time.h
index de7fc3c..e220c76 100644
--- a/cartographer/sensor/map_by_time.h
+++ b/cartographer/sensor/map_by_time.h
@@ -191,6 +191,15 @@ class MapByTime {
                                          EndOfTrajectory(trajectory_id));
   }
 
+  // Returns an iterator to the the first element in the container belonging to
+  // trajectory 'trajectory_id' whose time is not considered to go before
+  // 'time', or EndOfTrajectory(trajectory_id) if all keys are considered to go
+  // before 'time'. 'trajectory_id' must refer to an existing trajectory.
+  ConstIterator lower_bound(const int trajectory_id,
+                            const common::Time time) const {
+    return ConstIterator(data_.at(trajectory_id).lower_bound(time));
+  }
+
  private:
   std::map<int, std::map<common::Time, DataType>> data_;
 };