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