diff --git a/cartographer/mapping_3d/acceleration_cost_function.h b/cartographer/mapping_3d/acceleration_cost_function.h
index 33ddf84..a5a16d7 100644
--- a/cartographer/mapping_3d/acceleration_cost_function.h
+++ b/cartographer/mapping_3d/acceleration_cost_function.h
@@ -41,9 +41,14 @@ class AccelerationCostFunction {
   template <typename T>
   bool operator()(const T* const middle_rotation, const T* const start_position,
                   const T* const middle_position, const T* const end_position,
-                  const T* const gravity_constant, T* residual) const {
+                  const T* const gravity_constant,
+                  const T* const imu_calibration, T* residual) const {
+    const Eigen::Quaternion<T> eigen_imu_calibration(
+        imu_calibration[0], imu_calibration[1], imu_calibration[2],
+        imu_calibration[3]);
     const Eigen::Matrix<T, 3, 1> imu_delta_velocity =
-        ToEigen(middle_rotation) * delta_velocity_imu_frame_.cast<T>() -
+        ToEigen(middle_rotation) * eigen_imu_calibration *
+            delta_velocity_imu_frame_.cast<T>() -
         *gravity_constant *
             (0.5 * (first_delta_time_seconds_ + second_delta_time_seconds_) *
              Eigen::Vector3d::UnitZ())
diff --git a/cartographer/mapping_3d/rotation_cost_function.h b/cartographer/mapping_3d/rotation_cost_function.h
index 17abb02..1f5a0d5 100644
--- a/cartographer/mapping_3d/rotation_cost_function.h
+++ b/cartographer/mapping_3d/rotation_cost_function.h
@@ -36,13 +36,17 @@ class RotationCostFunction {
 
   template <typename T>
   bool operator()(const T* const start_rotation, const T* const end_rotation,
-                  T* residual) const {
+                  const T* const imu_calibration, T* residual) const {
     const Eigen::Quaternion<T> start(start_rotation[0], start_rotation[1],
                                      start_rotation[2], start_rotation[3]);
     const Eigen::Quaternion<T> end(end_rotation[0], end_rotation[1],
                                    end_rotation[2], end_rotation[3]);
+    const Eigen::Quaternion<T> eigen_imu_calibration(
+        imu_calibration[0], imu_calibration[1], imu_calibration[2],
+        imu_calibration[3]);
     const Eigen::Quaternion<T> error =
-        end.conjugate() * start * delta_rotation_imu_frame_.cast<T>();
+        end.conjugate() * start * eigen_imu_calibration *
+        delta_rotation_imu_frame_.cast<T>() * eigen_imu_calibration.conjugate();
     residual[0] = scaling_factor_ * error.x();
     residual[1] = scaling_factor_ * error.y();
     residual[2] = scaling_factor_ * error.z();
diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
index 6e1a30e..2a6864a 100644
--- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
+++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
@@ -191,8 +191,12 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
 
   // Add constraints based on IMU observations of angular velocities and
   // linear acceleration.
+  trajectory_data_.resize(imu_data_.size());
   for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
+    TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
+    problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4,
+                              new ceres::QuaternionParameterization());
     const std::deque<ImuData>& imu_data = imu_data_.at(trajectory_id);
     CHECK(!imu_data.empty());
     // TODO(whess): Add support for empty trajectories.
@@ -233,7 +237,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
             result_center_to_center.delta_velocity;
         problem.AddResidualBlock(
             new ceres::AutoDiffCostFunction<AccelerationCostFunction, 3, 4, 3,
-                                            3, 3, 1>(
+                                            3, 3, 1, 4>(
                 new AccelerationCostFunction(
                     options_.acceleration_weight(), delta_velocity,
                     common::ToSeconds(first_duration),
@@ -242,14 +246,16 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
             C_nodes[trajectory_id].at(node_index - 1).translation(),
             C_nodes[trajectory_id].at(node_index).translation(),
             C_nodes[trajectory_id].at(node_index + 1).translation(),
-            &gravity_constant_);
+            &trajectory_data.gravity_constant,
+            trajectory_data.imu_calibration.data());
       }
       problem.AddResidualBlock(
-          new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4>(
+          new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4, 4>(
               new RotationCostFunction(options_.rotation_weight(),
                                        result.delta_rotation)),
           nullptr, C_nodes[trajectory_id].at(node_index - 1).rotation(),
-          C_nodes[trajectory_id].at(node_index).rotation());
+          C_nodes[trajectory_id].at(node_index).rotation(),
+          trajectory_data.imu_calibration.data());
     }
   }
 
@@ -261,7 +267,20 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
 
   if (options_.log_solver_summary()) {
     LOG(INFO) << summary.FullReport();
-    LOG(INFO) << "Gravity was: " << gravity_constant_;
+    for (size_t trajectory_id = 0; trajectory_id != trajectory_data_.size();
+         ++trajectory_id) {
+      if (trajectory_id != 0) {
+        LOG(INFO) << "Trajectory " << trajectory_id << ":";
+      }
+      LOG(INFO) << "Gravity was: "
+                << trajectory_data_[trajectory_id].gravity_constant;
+      LOG(INFO) << "IMU correction was: "
+                << common::RadToDeg(
+                       2. *
+                       std::acos(
+                           trajectory_data_[trajectory_id].imu_calibration[0]))
+                << " deg";
+    }
   }
 
   // Store the result.
diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
index 25e0930..9f1ce3b 100644
--- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
+++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
@@ -75,12 +75,17 @@ class OptimizationProblem {
   const std::vector<std::vector<SubmapData>>& submap_data() const;
 
  private:
+  struct TrajectoryData {
+    double gravity_constant = 9.8;
+    std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
+  };
+
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
   FixZ fix_z_;
   std::vector<std::deque<ImuData>> imu_data_;
   std::vector<std::vector<NodeData>> node_data_;
   std::vector<std::vector<SubmapData>> submap_data_;
-  double gravity_constant_ = 9.8;
+  std::vector<TrajectoryData> trajectory_data_;
 };
 
 }  // namespace sparse_pose_graph