inline void applyOriToState(TrackingSystem const &sys, BodyState &state,
BodyProcessModel &processModel,
CannedIMUMeasurement const &meas) {
Eigen::Quaterniond quat;
meas.restoreQuat(quat);
Eigen::Vector3d var;
meas.restoreQuatVariance(var);
/// Rotate it into camera space
/// @todo do this without rotating into camera space?
quat = getQuatToCameraSpace(sys) * quat;
/// @todo transform variance?
kalman::AbsoluteOrientationMeasurement<BodyState> kalmanMeas{quat, var};
kalman::correct(state, processModel, kalmanMeas);
}
inline void applyAngVelToState(TrackingSystem const &sys, BodyState &state,
BodyProcessModel &processModel,
CannedIMUMeasurement const &meas) {
Eigen::Vector3d angVel;
meas.restoreAngVel(angVel);
Eigen::Vector3d var;
meas.restoreAngVelVariance(var);
#if 0
static const double dt = 0.02;
/// Rotate it into camera space - it's bTb and we want cTc
/// @todo do this without rotating into camera space?
Eigen::Quaterniond cTb = state.getQuaternion();
// Eigen::Matrix3d bTc(state.getQuaternion().conjugate());
/// Turn it into an incremental quat to do the space transformation
Eigen::Quaterniond incrementalQuat =
cTb * angVelVecToIncRot(angVel, dt) * cTb.conjugate();
/// Then turn it back into an angular velocity vector
angVel = incRotToAngVelVec(incrementalQuat, dt);
// angVel = (getRotationMatrixToCameraSpace(sys) * angVel).eval();
/// @todo transform variance?
kalman::AngularVelocityMeasurement<BodyState> kalmanMeas{angVel, var};
kalman::correct(state, processModel, kalmanMeas);
#else
kalman::IMUAngVelMeasurement kalmanMeas{angVel, var};
auto correctionInProgress =
kalman::beginUnscentedCorrection(state, kalmanMeas);
auto outputMeas = [&] {
std::cout << "state: " << state.angularVelocity().transpose()
<< " and measurement: " << angVel.transpose();
};
if (!correctionInProgress.stateCorrectionFinite) {
std::cout
<< "Non-finite state correction in applying angular velocity: ";
outputMeas();
std::cout << "\n";
return;
}
if (!correctionInProgress.finishCorrection(true)) {
std::cout << "Non-finite error covariance after applying angular "
"velocity: ";
outputMeas();
std::cout << "\n";
}
#endif
}
void applyIMUToState(TrackingSystem const &sys,
util::time::TimeValue const &initialTime,
BodyState &state, BodyProcessModel &processModel,
util::time::TimeValue const &newTime,
CannedIMUMeasurement const &meas) {
if (newTime != initialTime) {
auto dt = osvrTimeValueDurationSeconds(&newTime, &initialTime);
kalman::predict(state, processModel, dt);
state.externalizeRotation();
}
if (meas.orientationValid()) {
applyOriToState(sys, state, processModel, meas);
} else if (meas.angVelValid()) {
applyAngVelToState(sys, state, processModel, meas);
} else {
// unusually, the measurement is totally invalid. Just normalize and
// go on.
state.postCorrect();
}
}
inline void applyAngVelToState(TrackingSystem const &sys, BodyState &state,
BodyProcessModel &processModel,
CannedIMUMeasurement const &meas) {
Eigen::Vector3d angVel;
meas.restoreAngVel(angVel);
Eigen::Vector3d var;
meas.restoreAngVelVariance(var);
/// Rotate it into camera space - it's bTb and we want cTc
/// @todo do this without rotating into camera space?
Eigen::Quaterniond cTb = state.getQuaternion();
//Eigen::Matrix3d bTc(state.getQuaternion().conjugate());
Eigen::Quaterniond incrementalQuat = cTb * util::quat_exp_map(angVel).exp() *
cTb.conjugate();
angVel = util::quat_exp_map(incrementalQuat).ln();
// angVel = (getRotationMatrixToCameraSpace(sys) * angVel).eval();
/// @todo transform variance?
kalman::AngularVelocityMeasurement<BodyState> kalmanMeas{angVel, var};
kalman::correct(state, processModel, kalmanMeas);
}
