search

KumarRobotics / msckf_vio

Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight
Other
1.66k stars 593 forks source link

Simplifying the code a tad that initializes the IMU attitude #60

Closed chutsu closed 4 years ago

chutsu commented 5 years ago

In MsckfVio::initializeGravityAndBias(), from line 278, the idea is to initialize the IMU initial attitude by forming a quaternion from two vectors, the gravity vector IMUState::gravity and averaged accelerometer measurement gravity_imu.

Quaterniond q0_i_w = Quaterniond::FromTwoVectors(
    gravity_imu, -IMUState::gravity);
  state_server.imu_state.orientation =
    rotationToQuaternion(q0_i_w.toRotationMatrix().transpose());

Instead of transposing the rotation matrix produced from q0_i_w, would it not be simplier to just rearrange from

Quaterniond::FromTwoVectors(gravity_imu, -IMUState::gravity)

to:

Quaterniond::FromTwoVectors(-IMUState::gravity, gravity_imu)

swapping the order of the two vectors. Which would remove the need for transposing the matrix, resulting in:

Quaterniond q0_w_i = Quaterniond::FromTwoVectors(-IMUState::gravity, gravity_imu);
state_server.imu_state.orientation = rotationToQuaternion(q0_w_i.toRotationMatrix());

Small Octave script here

ke-sun commented 5 years ago

Yes, your way is simpler. Thanks for pointing that out.