void Waist_Velocity_Internal_Measurement_Model::
func_hv_and_dhv_by_dxv(const VNL::Vector<double> &xv)
{
assert(xv.Size() == motion_model->STATE_SIZE);
// Just pull out v part of state
VW::Vector3D vW;
vW.SetVNL3(xv.Extract(3, 7));
VW::Quaternion qWR(0.5,0.5,0.5,0.5);
// qWR.SetRXYZ(xv.Extract(4, 3));
VW::Quaternion qRW = qWR.Inverse();
VNL::MatrixFixed<3,3,double> RRW = qRW.RotationMatrix();
// Now hv = vR = RRW vW
VW::Vector3D vR = RRW * vW;
hvRES.Update(vR.GetVNL3());
// cout << "wv hvRES: " << hvRES << endl;
// dhv_by_dxv = (0 dhv_by_dqWR dhv_by_dvR 0)
dhv_by_dxvRES.Fill(0.0);
#if 0
// dhv_by_dqWR = d_by_dqRW(RRW vW) * dqRW_by_dqWR
VNL::MatrixFixed<3,4,double> dhv_by_dqWR =
dRq_times_a_by_dq(qRW, vW) * dqbar_by_dq();
dhv_by_dxvRES.Update(dhv_by_dqWR, 0, 3);
#endif
// dhv_by_dvR = RRW
dhv_by_dxvRES.Update(RRW, 0, 7);
// cout << "wv dhvRES: " << endl << dhv_by_dxvRES << endl;
}
void PartFeatureModel::func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(
const Eigen::VectorXd &yi, const Eigen::VectorXd &xp)
{
// Extract cartesian and quaternion components of xp
motion_model_->func_r(xp);
motion_model_->func_q(xp);
// Extract ri and hhati components of yi = ypi
func_ri(yi);
func_hhati(yi);
// ri part: transformation is just the same as in the normal point case
// zeroedri = RRW(rWi - rW)
// ri - r
Eigen::Vector3d yWiminusrW = riRES_ - motion_model_->rRES_;
Eigen::Quaterniond qRW = motion_model_->qRES_.inverse();
Eigen::Matrix4d dqRW_by_dq = dqbar_by_dq();
// Rotation RRW
Eigen::Matrix3d RRW = qRW.toRotationMatrix();
// RRW(rWi - rW)
Eigen::Vector3d zeroedri = RRW * yWiminusrW;
// Now calculate Jacobians
// dzeroedri_by_dri is RRW
// dzeroedri_by_dhhati = 0
Eigen::Matrix3d dzeroedri_by_dri = RRW;
// dzeroedyi_by_dxp:
// dzeroedri_by_dr = -RRW
// dzeroedri_by_dq = d_dq(RRW (ri - r))
Eigen::Matrix3d dzeroedri_by_dr = RRW * -1.0;
Eigen::Matrix<double, 3, 4> dzeroedri_by_dqRW = dRq_times_a_by_dq(qRW, yWiminusrW);
Eigen::Matrix<double, 3, 4> dzeroedri_by_dq = dzeroedri_by_dqRW * dqRW_by_dq;
// Now for the hhati part (easier...)
// zeroedhhati = RRW hhati
Eigen::Vector3d zeroedhhati = RRW * hhatiRES_;
// Jacobians
// dzeroedhhati_by_dr = 0
// dzeroedhhati_by_dq = d_dq(RRW hhati)
// dzeroedhhati_by_dhhati = RRW
// dzeroedhhati_by_dri = 0
Eigen::Matrix<double, 3, 4> dzeroedhhati_by_dqRW = dRq_times_a_by_dq(qRW, hhatiRES_);
Eigen::Matrix<double, 3, 4> dzeroedhhati_by_dq = dzeroedhhati_by_dqRW * dqRW_by_dq;
Eigen::Matrix<double, 3, 3> dzeroedhhati_by_dhhati = RRW;
// And put it all together
zeroedyiRES_ << zeroedri, zeroedhhati;
dzeroedyi_by_dxpRES_.setZero();
dzeroedyi_by_dxpRES_.block(0, 0, 3, 3) = dzeroedri_by_dr;
dzeroedyi_by_dxpRES_.block(0, 3, 3, 4) = dzeroedri_by_dq;
dzeroedyi_by_dxpRES_.block(3, 3, 3, 4) = dzeroedhhati_by_dq;
dzeroedyi_by_dyiRES_.setZero();
dzeroedyi_by_dyiRES_.block(0, 0, 3, 3) = dzeroedri_by_dri;
dzeroedyi_by_dyiRES_.block(3, 3, 3, 3) = dzeroedhhati_by_dhhati;
}
