public override void func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(Vector yi, Vector xp)
{
ThreeD_Motion_Model mm = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model;
// Extract cartesian and quaternion components of xp
mm.func_r(xp);
mm.func_q(xp);
Vector3D yWiminusrW = new Vector3D((new Vector3D(yi)) - (mm.get_rRES()));
Quaternion qRES = mm.get_qRES();
Quaternion qRW = qRES.Inverse();
MatrixFixed dqRW_by_dq = MatrixFixed.dqbar_by_dq();
// Rotation RRW
RotationMatrix RRW = qRW.RotationMatrix();
// Position of feature relative to robot in robot frame
Vector3D zeroedyi = new Vector3D(RRW * yWiminusrW);
zeroedyiRES.Update(zeroedyi.GetVNL3());
// Now calculate Jacobians
// dzeroedyi_by_dyi is RRW
dzeroedyi_by_dyiRES.Update(RRW);
// dzeroedyi_by_dxp has 2 partitions:
// dzeroedyi_by_dr (3 * 3)
// dzeroedyi_by_dq (3 * 4)
MatrixFixed dzeroedyi_by_dr = (MatrixFixed)(RRW);
dzeroedyi_by_dr *= -1.0f;
//These should be 3x4 dimension
MatrixFixed dzeroedyi_by_dqRW = MatrixFixed.dRq_times_a_by_dq(qRW, yWiminusrW);
MatrixFixed dzeroedyi_by_dq = dzeroedyi_by_dqRW * dqRW_by_dq;
dzeroedyi_by_dxpRES.Update(dzeroedyi_by_dr, 0, 0);
dzeroedyi_by_dxpRES.Update(dzeroedyi_by_dq, 0, 3);
}
/// <summary>
/// Partial initialisation function
/// Calculates the state vector \vct{y}_{pi} and Jacobians for a new
/// partially-initialised feature from an initial observation.
/// @param hi The initial measurement \vct{h}_i , in this case the (x,y)
/// image location of the feature.
/// </summary>
/// <param name="hi"></param>
/// <param name="xp">The robot position state \vct{x}_p from which the initial observation was made.</param>
public override void func_ypi_and_dypi_by_dxp_and_dypi_by_dhi_and_Ri(Vector hi, Vector xp)
{
// Representation of a line here:
// ypi = (rWi )
// (hLhatWi)
// Form the ray (in camera co-ordinates by unprojecting this image location
Vector3D hLRi = new Vector3D(((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera.Unproject(hi));
//if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("debug hLRi");
// Form hLhatRi from hLRi
// Normalise
Vector3D hLhatRi = hLRi;
hLhatRi.Normalise();
MatrixFixed dhLhatRi_by_dhLRi = MatrixFixed.dvnorm_by_dv(hLRi);
//if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("debug hLhatRi" + hLhatRi);
// Now convert this into a direction in world co-ordinates by rotating
// Form hLhatWi from hLhatRi
// Rotate
((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.func_q(xp);
RotationMatrix RWR = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.get_qRES().RotationMatrix();
Vector3D hLhatWi = new Vector3D(RWR * hLhatRi);
//if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("debug hLhatWi" + hLhatWi);
// Extract rW from xp
((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.func_r(xp);
// And form ypiRES
ypiRES.Update(((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.get_rRES().GetVNL3(), 0);
ypiRES.Update(hLhatWi.GetVNL3(), 3);
//if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("debug ypiRES" + ypiRES);
// 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)
MatrixFixed dhLhatWi_by_dq = MatrixFixed.dRq_times_a_by_dq(((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.get_qRES(), hLhatRi);
// Put dypi_by_dxp together
dypi_by_dxpRES.Fill(0.0f);
dypi_by_dxpRES.Put(0, 0, 1.0f);
dypi_by_dxpRES.Put(1, 1, 1.0f);
dypi_by_dxpRES.Put(2, 2, 1.0f);
dypi_by_dxpRES.Update(dhLhatWi_by_dq, 3, 3);
// cout << "dypi_by_dxpRES" << dypi_by_dxpRES;
// 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
MatrixFixed dhLhatWi_by_dhi = RWR * dhLhatRi_by_dhLRi * ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera.UnprojectionJacobian();
dypi_by_dhiRES.Fill(0.0f);
dypi_by_dhiRES.Update(dhLhatWi_by_dhi, 3, 0);
// cout << "dypi_by_dhiRES" << dypi_by_dhiRES;
// And construct Ri
func_Ri(hi);
/*
* if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("func_ypi: hi = " + hi + "," +
* "xp = " + xp + "," +
* "ypiRES = " + ypiRES);
*/
}
public override void func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(Vector yi, Vector xp)
{
Wide_Camera_Point_Feature_Measurement_Model wm = (Wide_Camera_Point_Feature_Measurement_Model)wide_model;
// Extract cartesian and quaternion components of xp
wm.threed_motion_model.func_r(xp);
wm.threed_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) //commented out in original code
// ri - r
Vector3D yWiminusrW = new Vector3D(riRES - wm.threed_motion_model.get_rRES());
Quaternion qRW = wm.threed_motion_model.get_qRES().Inverse();
MatrixFixed dqRW_by_dq = MatrixFixed.dqbar_by_dq();
// Rotation RRW
RotationMatrix RRW = qRW.RotationMatrix();
// RRW(rWi - rW)
Vector3D zeroedri = new Vector3D(RRW * yWiminusrW);
// Now calculate Jacobians
// dzeroedri_by_dri is RRW
// dzeroedri_by_dhhati = 0
MatrixFixed dzeroedri_by_dri = new MatrixFixed(RRW);
// dzeroedyi_by_dxp:
// dzeroedri_by_dr = -RRW
// dzeroedri_by_dq = d_dq(RRW (ri - r))
MatrixFixed dzeroedri_by_dr = RRW * -1.0f;
MatrixFixed dzeroedri_by_dqRW = MatrixFixed.dRq_times_a_by_dq(qRW, yWiminusrW);
MatrixFixed dzeroedri_by_dq = dzeroedri_by_dqRW * dqRW_by_dq;
// Now for the hhati part (easier...)
// zeroedhhati = RRW hhati
Vector3D zeroedhhati = new Vector3D(RRW * hhatiRES);
// Jacobians
// dzeroedhhati_by_dr = 0
// dzeroedhhati_by_dq = d_dq(RRW hhati)
// dzeroedhhati_by_dhhati = RRW
// dzeroedhhati_by_dri = 0
MatrixFixed dzeroedhhati_by_dqRW = MatrixFixed.dRq_times_a_by_dq(qRW, hhatiRES);
MatrixFixed dzeroedhhati_by_dq = dzeroedhhati_by_dqRW * dqRW_by_dq;
MatrixFixed dzeroedhhati_by_dhhati = new MatrixFixed(RRW);
// And put it all together
zeroedyiRES.Update(zeroedri.GetVNL3(), 0);
zeroedyiRES.Update(zeroedhhati.GetVNL3(), 3);
//cout << "Line: zeroedri = " << zeroedri << "zeroedhhati = " << zeroedhhati;
dzeroedyi_by_dxpRES.Fill(0.0f);
dzeroedyi_by_dxpRES.Update(dzeroedri_by_dr, 0, 0);
dzeroedyi_by_dxpRES.Update(dzeroedri_by_dq, 0, 3);
dzeroedyi_by_dxpRES.Update(dzeroedhhati_by_dq, 3, 3);
dzeroedyi_by_dyiRES.Fill(0.0f);
dzeroedyi_by_dyiRES.Update(dzeroedri_by_dri, 0, 0);
dzeroedyi_by_dyiRES.Update(dzeroedhhati_by_dhhati, 3, 3);
}