Add IMU orientation correction to the pose graph optimization. (#361)

Orientation of the IMU, most importantly pitch, is crucial to correctly
track changes in altitude. Each trajectory gets optimization variables
to correct for small errors in the IMU orientation.

Also adds separate gravity constants for each trajectory. The idea is
that different trajectories might use different hardware and slightly
disagree about the strength of gravity.

PAIR=damonkohler

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())

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();

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.

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