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