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