From 955e1901669f042cc600e445837ec66d96032d4a Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 28 Jun 2017 18:12:24 +0200 Subject: [PATCH] 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 --- .../mapping_3d/acceleration_cost_function.h | 9 ++++-- .../mapping_3d/rotation_cost_function.h | 8 +++-- .../sparse_pose_graph/optimization_problem.cc | 29 +++++++++++++++---- .../sparse_pose_graph/optimization_problem.h | 7 ++++- 4 files changed, 43 insertions(+), 10 deletions(-) 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 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 eigen_imu_calibration( + imu_calibration[0], imu_calibration[1], imu_calibration[2], + imu_calibration[3]); const Eigen::Matrix imu_delta_velocity = - ToEigen(middle_rotation) * delta_velocity_imu_frame_.cast() - + ToEigen(middle_rotation) * eigen_imu_calibration * + delta_velocity_imu_frame_.cast() - *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 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 start(start_rotation[0], start_rotation[1], start_rotation[2], start_rotation[3]); const Eigen::Quaternion end(end_rotation[0], end_rotation[1], end_rotation[2], end_rotation[3]); + const Eigen::Quaternion eigen_imu_calibration( + imu_calibration[0], imu_calibration[1], imu_calibration[2], + imu_calibration[3]); const Eigen::Quaternion error = - end.conjugate() * start * delta_rotation_imu_frame_.cast(); + end.conjugate() * start * eigen_imu_calibration * + delta_rotation_imu_frame_.cast() * 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& 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& 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& constraints) { result_center_to_center.delta_velocity; problem.AddResidualBlock( new ceres::AutoDiffCostFunction( + 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& 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( + new ceres::AutoDiffCostFunction( 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& 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>& submap_data() const; private: + struct TrajectoryData { + double gravity_constant = 9.8; + std::array imu_calibration{{1., 0., 0., 0.}}; + }; + mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; FixZ fix_z_; std::vector> imu_data_; std::vector> node_data_; std::vector> submap_data_; - double gravity_constant_ = 9.8; + std::vector trajectory_data_; }; } // namespace sparse_pose_graph