From 147c8166b68e5debd399fa0d69f04286f77bc2be Mon Sep 17 00:00:00 2001 From: gaschler Date: Thu, 23 Nov 2017 13:50:44 +0100 Subject: [PATCH] ImuTracker uses conjugate. (#693) Normalized quaternions can be inverted with conjugate, which is faster. The motivation for optimizing ImuTracker::Advance is that per-point unwarping will call this for every point. --- cartographer/mapping/imu_tracker.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cartographer/mapping/imu_tracker.cc b/cartographer/mapping/imu_tracker.cc index a6df73e..39cac08 100644 --- a/cartographer/mapping/imu_tracker.cc +++ b/cartographer/mapping/imu_tracker.cc @@ -42,7 +42,7 @@ void ImuTracker::Advance(const common::Time time) { transform::AngleAxisVectorToRotationQuaternion( Eigen::Vector3d(imu_angular_velocity_ * delta_t)); orientation_ = (orientation_ * rotation).normalized(); - gravity_vector_ = rotation.inverse() * gravity_vector_; + gravity_vector_ = rotation.conjugate() * gravity_vector_; time_ = time; } @@ -61,7 +61,7 @@ void ImuTracker::AddImuLinearAccelerationObservation( // Change the 'orientation_' so that it agrees with the current // 'gravity_vector_'. const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors( - gravity_vector_, orientation_.inverse() * Eigen::Vector3d::UnitZ()); + gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ()); orientation_ = (orientation_ * rotation).normalized(); CHECK_GT((orientation_ * gravity_vector_).z(), 0.); CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);