diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc
index 9b7d085..821eea6 100644
--- a/cartographer/mapping_3d/sparse_pose_graph.cc
+++ b/cartographer/mapping_3d/sparse_pose_graph.cc
@@ -158,7 +158,11 @@ void SparsePoseGraph::AddImuData(const int trajectory_id,
 void SparsePoseGraph::AddFixedFramePoseData(
     const int trajectory_id,
     const sensor::FixedFramePoseData& fixed_frame_pose_data) {
-  LOG(FATAL) << "Not yet implemented for 3D.";
+  common::MutexLocker locker(&mutex_);
+  AddWorkItem([=]() REQUIRES(mutex_) {
+    optimization_problem_.AddFixedFramePoseData(trajectory_id,
+                                                fixed_frame_pose_data);
+  });
 }
 
 void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
index 2b021d6..c8d1e17 100644
--- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
+++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
@@ -87,6 +87,16 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
   imu_data_[trajectory_id].push_back(imu_data);
 }
 
+void OptimizationProblem::AddFixedFramePoseData(
+    const int trajectory_id,
+    const sensor::FixedFramePoseData& fixed_frame_pose_data) {
+  CHECK_GE(trajectory_id, 0);
+  fixed_frame_pose_data_.resize(std::max(
+      fixed_frame_pose_data_.size(), static_cast<size_t>(trajectory_id) + 1));
+  fixed_frame_pose_data_[trajectory_id].Push(fixed_frame_pose_data.time,
+                                             fixed_frame_pose_data.pose);
+}
+
 void OptimizationProblem::AddTrajectoryNode(
     const int trajectory_id, const common::Time time,
     const transform::Rigid3d& point_cloud_pose) {
diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
index 6d05c89..dcb11a7 100644
--- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
+++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
@@ -29,7 +29,9 @@
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
 #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
+#include "cartographer/sensor/fixed_frame_pose_data.h"
 #include "cartographer/sensor/imu_data.h"
+#include "cartographer/transform/transform_interpolation_buffer.h"
 
 namespace cartographer {
 namespace mapping_3d {
@@ -61,6 +63,9 @@ class OptimizationProblem {
   OptimizationProblem& operator=(const OptimizationProblem&) = delete;
 
   void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
+  void AddFixedFramePoseData(
+      int trajectory_id,
+      const sensor::FixedFramePoseData& fixed_frame_pose_data);
   void AddTrajectoryNode(int trajectory_id, common::Time time,
                          const transform::Rigid3d& point_cloud_pose);
   void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose);
@@ -86,6 +91,7 @@ class OptimizationProblem {
   std::vector<std::vector<NodeData>> node_data_;
   std::vector<std::vector<SubmapData>> submap_data_;
   std::vector<TrajectoryData> trajectory_data_;
+  std::vector<transform::TransformInterpolationBuffer> fixed_frame_pose_data_;
 };
 
 }  // namespace sparse_pose_graph