Eigen::MatrixXd FeatureModel::dRq_times_a_by_dq (const Eigen::Quaterniond &q, const Eigen::Vector3d &a)
{
Eigen::MatrixXd aMat(3,1);
aMat(0,0) = a(0);
aMat(1,0) = a(1);
aMat(2,0) = a(2);
Eigen::Matrix3d TempR;
Eigen::MatrixXd Temp31(3,1);
Eigen::MatrixXd dRq_times_a_by_dq(3,4);
// Make Jacobian by stacking four vectors together side by side
TempR = dR_by_dq0(q);
Temp31 = TempR * aMat;
dRq_times_a_by_dq.block(0,0,3,1) = Temp31;
TempR = dR_by_dqx(q);
Temp31 = TempR * aMat;
dRq_times_a_by_dq.block(0,1,3,1) = Temp31;
TempR = dR_by_dqy(q);
Temp31 = TempR * aMat;
dRq_times_a_by_dq.block(0,2,3,1) = Temp31;
TempR = dR_by_dqz(q);
Temp31 = TempR * aMat;
dRq_times_a_by_dq.block(0,3,3,1) = Temp31;
return dRq_times_a_by_dq;
}
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;
}
// Redefined virtual functions from
// Partially_Initialised_Fully_Initialised_Feature_Measurement_Model
void PartFeatureModel::func_ypi_and_dypi_by_dxp_and_dypi_by_dhi_and_Ri(
const Eigen::VectorXd &hi, const Eigen::VectorXd &xp)
{
// Representation of a line here:
// ypi = (rWi )
// (hLhatWi)
// Form the ray (in camera co-ordinates by unprojecting this image location
Eigen::Vector3d hLRi = camera_->Unproject(hi);
// Form hLhatRi from hLRi
// Normalise
Eigen::Vector3d hLhatRi = hLRi;
hLhatRi.normalize();
Eigen::Matrix<double, 3, 3> dhLhatRi_by_dhLRi = dvnorm_by_dv(hLRi);
// Now convert this into a direction in world co-ordinates by rotating
// Form hLhatWi from hLhatRi
// Rotate
motion_model_->func_q(xp);
Eigen::Matrix3d RWR = motion_model_->qRES_.toRotationMatrix();
Eigen::Vector3d hLhatWi(RWR * hLhatRi);
// Extract rW from xp
motion_model_->func_r(xp);
// And form ypiRES
ypiRES_ << motion_model_->rRES_(0), motion_model_->rRES_(1), motion_model_->rRES_(2),
hLhatWi(0), hLhatWi(1), hLhatWi(2);
// Form Jacobians dypi_by_dxp and dypi_by_dhi
// dypi_by_dxp = (drWi_by_dr drWi_by_dq )
// (dhLhatWi_by_dr dhLhatWi_by_dq)
// = (I 0 )
// (0 dhLhatWi_by_dq)
// hLhatWi = RWR * hLhatRi
// => dhLhatWi_by_dq = d/dq ( R(qWR) * hLhatRi)
Eigen::Matrix<double, 3, 4> dhLhatWi_by_dq = dRq_times_a_by_dq(motion_model_->qRES_, hLhatRi);
// Put dypi_by_dxp together
dypi_by_dxpRES_.setZero();
dypi_by_dxpRES_(0,0) = 1.0;
dypi_by_dxpRES_(1,1) = 1.0;
dypi_by_dxpRES_(2,2) = 1.0;
dypi_by_dxpRES_.block(3,3,3,4) = dhLhatWi_by_dq;
// dypi_by_dhi = (drWi_by_dhi )
// (dhLhatWi_by_dhi)
// = (0 )
// (dhLhatWi_by_dhi)
// hLhatWi = RWR * hLhatRi
// Need to work out derivative for this
// dhLhatWi_by_dhi = RWR * dhLhatRi_by_dhLRi * dhLRi_by_dhi
Eigen::MatrixXd dhLhatWi_by_dhi(3,2);
dhLhatWi_by_dhi = RWR * dhLhatRi_by_dhLRi * camera_->UnprojectionJacobian();
dypi_by_dhiRES_.setZero();
dypi_by_dhiRES_.block(3,0,3,2) = dhLhatWi_by_dhi;
// And construct Ri
func_Ri(hi);
}
