public AngleAxis(Vector3D axis, float angle)
{
Set(axis,angle);
}
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);
}
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);
}
// @returns the axis of rotation. */
public Vector3D Axis()
{
Vector3D _i = Imaginary();
float l = _i.Norm();
if (l < 1e-8)
{
_i.Set(0.0f, 0.0f, 0.0f);
}
else
{
Point3D _i2 = (Point3D)_i / l;
_i = new Vector3D(_i2);
}
return _i;
}
// Rotate a vector
// @param v the vector to be rotated
// @returns the resulting rotated vector
public Vector3D Rotate(Vector3D v)
{
Vector3D _i = this.Imaginary();
Vector3D _n = new Vector3D(Vector3D.VectorProduct(_i, v));
Vector3D delta = new Vector3D(_r * _n - (Vector3D.VectorProduct(_n, _i)));
return (new Vector3D(v + 2.0f * delta));
}
public float Multiply(LineSeg3D ls1, LineSeg3D ls2)
{
Vector3D d1 = new Vector3D(ls1._ep - ls1._sp);
Vector3D d2 = new Vector3D(ls2._ep - ls2._sp);
return (d1.GetX() * d2.GetX() + d1.GetY() * d2.GetY() + d1.GetZ() * d2.GetZ());
}
public static MatrixFixed dRq_times_a_by_dq(Quaternion q, Vector a)
{
Vector3D v = new Vector3D(a);
return (dRq_times_a_by_dq(q, v));
}
public Vector3D Axis()
{
Vector3D axis = new Vector3D(0,0,0);
float d0 = this[0,0], d1 = this[1,1], d2 = this[2,2];
// The trace determines the method of decomposition
float rr = 1.0f + d0 + d1 + d2;
if (rr > 0)
{
axis.SetX( this[2,1] - this[1,2] );
axis.SetY( this[0,2] - this[2,0] );
axis.SetZ( this[1,0] - this[0,1] );
}
else
{
// Trace is less than zero, so need to determine which
// major diagonal is largest
if ((d0 > d1) && (d0 > d2))
{
axis.SetX( 0.5f );
axis.SetY( this[0,1] + this[1,0] );
axis.SetZ( this[0,2] + this[2,0] );
}
else
if (d1 > d2)
{
axis.SetX( this[0,1] + this[1,0] );
axis.SetY( 0.5f );
axis.SetZ( this[1,2] + this[2,1] );
}
else
{
axis.SetX( this[0,2] + this[2,0] );
axis.SetY( this[1,2] + this[2,1] );
axis.SetZ( 0.5f );
}
}
axis.Normalise();
return (axis);
}
public Vector3D InverseRotate(Vector3D v)
{
return
new Vector3D(this[0,0]*v.GetX()+this[1,0]*v.GetY()+this[2,0]*v.GetZ(),
this[0,1]*v.GetX()+this[1,1]*v.GetY()+this[2,1]*v.GetZ(),
this[0,2]*v.GetX()+this[1,2]*v.GetY()+this[2,2]*v.GetZ());
}
public void Set(Vector3D angleaxis)
{
float angle = angleaxis.Norm();
Set(angleaxis, angle);
}
/// <summary>
/// Note that the axis doesn't have to be a unit vector.
/// </summary>
/// <param name="axis">the axis around which to rotate</param>
/// <param name="angle">the angle of rotation in radians</param>
public void Set(Vector3D axis, float angle)
{
// start with an identity matrix.
this.SetIdentity();
// Identity is all that can be deduced from zero angle
if (angle == 0)
return;
// normalize to a unit vector u
float[] u = new float[3];
float norm;
norm = 1.0f / axis.Norm();
u[0]=axis.GetX()*norm;
u[1]=axis.GetY()*norm;
u[2]=axis.GetZ()*norm;
// add (cos(angle)-1)*(1 - u u').
float cos_angle = (float)Math.Cos(angle);
for (uint i=0; i<3; ++i)
for (uint j=0; j<3; ++j)
this[(int)i,(int)j] += (cos_angle-1) * ((i==j ? 1:0) - u[i]*u[j]);
// add sin(angle) * [u]
float sin_angle = (float)Math.Sin(angle);
this[0,1] -= sin_angle*u[2];
this[0,2] += sin_angle*u[1];
this[1,0] += sin_angle*u[2];
this[1,2] -= sin_angle*u[0];
this[2,0] -= sin_angle*u[1];
this[2,1] += sin_angle*u[0];
}
public override void func_xpredef_and_dxpredef_by_dxp_and_dxpredef_by_dxpdef
(Vector xp, Vector xpdef)
{
// Split position state into cartesian and quaternion
// r0, q0: position in original frame
func_r(xp);
func_q(xp);
Vector3D r0 = rRES;
Quaternion q0 = qRES;
// rn, qn: definition of new frame
func_r(xpdef);
func_q(xpdef);
Vector3D rn = rRES;
Quaternion qn = qRES;
Quaternion qnbar = qn.Conjugate();
// Calculate new cartesian part of xp
Vector3D Temp31a = new Vector3D(r0 - rn); // Temp31a is r0 - rn
RotationMatrix Temp33a = qnbar.RotationMatrix();
Vector3D Temp31b = new Vector3D(Temp33a * Temp31a);
// Copy into xpredefRES
xpredefRES[0] = Temp31b.GetX();
xpredefRES[1] = Temp31b.GetY();
xpredefRES[2] = Temp31b.GetZ();
// Calculate new quaternion part of xp
Quaternion Tempqa = qnbar.Multiply(q0);
// Copy into xpredefRES
Vector vecTempqa = Tempqa.GetRXYZ();
xpredefRES.Update(vecTempqa, 3);
// Form Jacobian dxpredef_by_dxpRES
dxpredef_by_dxpRES.Fill(0.0f);
dxpredef_by_dxpRES.Update(Temp33a, 0, 0);
MatrixFixed Temp44a = MatrixFixed.dq3_by_dq1(qnbar);
dxpredef_by_dxpRES.Update(Temp44a, 3, 3);
// Form Jacobian dxpredef_by_dxpdefRES
dxpredef_by_dxpdefRES.Fill(0.0f);
//Temp33a *= -1.0f;
Temp33a = (RotationMatrix)(Temp33a * - 1.0f);
dxpredef_by_dxpdefRES.Update(Temp33a, 0, 0);
Temp44a = MatrixFixed.dq3_by_dq2(q0);
MatrixFixed Temp44b = MatrixFixed.dqbar_by_dq();
MatrixFixed Temp44c = Temp44a * Temp44b;
dxpredef_by_dxpdefRES.Update(Temp44c, 3, 3);
// Top right corner of this Jacobian is tricky because we have to
// differentiate a rotation matrix
// Uses function that does this in bits.cc
// Build this corner in Temp34a
//MatrixFixed temp = new MatrixFixed(3,4);
MatrixFixed Temp34a = MatrixFixed.dRq_times_a_by_dq(qnbar, Temp31a.GetVNL3());
// So far we have drN_by_dqnbar; want _by_dqn
MatrixFixed Temp34b = Temp34a * Temp44b;
// Finally copy into result matrix
dxpredef_by_dxpdefRES.Update(Temp34b, 0, 3);
// cout << "dxpredef_by_dxpdefRES" << dxpredef_by_dxpdefRES;
}
/// <summary>
/// returns the current camera position and orientation
/// </summary>
/// <param name="position"></param>
/// <param name="orientation"></param>
public void getCameraPositionOrientation(ref Vector3D position, ref Quaternion orientation)
{
monoslaminterface.GetCameraPositionOrientation(ref position, ref orientation);
}
public AngleAxis(Vector3D angleaxis)
{
Set(angleaxis);
}
/// <summary>
/// returns the position and orientation of the camera
/// </summary>
/// <param name="position"></param>
/// <param name="orientation"></param>
public void GetCameraPositionOrientation(ref Vector3D position, ref Quaternion orientation)
{
/*
Quaternion qRO = new Quaternion(0.0f, 1.0f, 0.0f, 0.0f);
ThreeD_Motion_Model threed_motion_model = (ThreeD_Motion_Model)scene.get_motion_model();
threed_motion_model.func_xp(scene.get_xv());
threed_motion_model.func_r(threed_motion_model.get_xpRES());
threed_motion_model.func_q(threed_motion_model.get_xpRES());
orientation = threed_motion_model.get_qRES(); //.Multiply(qRO);
position = threed_motion_model.get_rRES();
*/
Vector state = scene.get_xv();
position = new Vector3D(state[0], state[1], state[2]);
//orientation = new Quaternion(state[4], state[5], state[6], state[3]);
orientation = new Quaternion(state[3], state[4], state[5], state[6]); //.Multiply(qRO);
}
public RotationMatrix(Vector3D axis, float angle) : base(3,3)
{
Set(axis, angle);
}
/// <summary>
/// turns a postion and quaternion based orientation into a matrix format
/// </summary>
/// <param name="position"></param>
/// <param name="orientation"></param>
/// <returns></returns>
private Matrix QuaternionToMatrixLookAt(Vector3D position, SceneLibrary.Quaternion orientation)
{
Microsoft.DirectX.Quaternion q_orientation =
new Microsoft.DirectX.Quaternion((float)orientation.GetX(),
(float)orientation.GetY(),
(float)orientation.GetZ(),
(float)orientation.GetR());
Matrix _matrixRotation = Matrix.RotationQuaternion(q_orientation);
Vector3 camPos = new Vector3((float)position.GetX(), -(float)position.GetY(), (float)position.GetZ());
Vector3 camTY = new Vector3(_matrixRotation.M21, _matrixRotation.M22, _matrixRotation.M23);
Vector3 camTZ = new Vector3(-(_matrixRotation.M31), -(_matrixRotation.M32), -(_matrixRotation.M33));
return (Matrix.LookAtLH(camPos, camPos + camTZ, camTY));
//float yaw = 0, pitch = 0, roll = 0;
//DecomposeRollPitchYawZXYMatrix(_matrixRotation, ref pitch, ref yaw, ref roll);
//return (Matrix.RotationYawPitchRoll((float)yaw, (float)pitch, (float)roll) * Matrix.LookAtLH(camPos, camPos + camTZ, camTY));
//return (Matrix.RotationYawPitchRoll(0, 0, (float)roll) * Matrix.LookAtLH(camPos, camPos + camTZ, camTY));
}
// Function to find a region in an image guided by current motion prediction
// which doesn't overlap existing features
public static bool FindNonOverlappingRegion(Scene_Single scene,
Vector local_u,
float delta_t,
Partially_Initialised_Feature_Measurement_Model default_feature_type_for_initialisation,
uint camera_width,
uint camera_height,
uint BOXSIZE,
ref int init_feature_search_ustart,
ref int init_feature_search_vstart,
ref int init_feature_search_ufinish,
ref int init_feature_search_vfinish,
uint FEATURE_INIT_STEPS_TO_PREDICT,
float FEATURE_INIT_DEPTH_HYPOTHESIS,
Random rnd)
{
ThreeD_Motion_Model threed_motion_model = (ThreeD_Motion_Model)scene.get_motion_model();
Vector local_xv = new Vector(scene.get_xv());
for (uint i = 0; i < FEATURE_INIT_STEPS_TO_PREDICT; i++)
{
scene.get_motion_model().func_fv_and_dfv_by_dxv(local_xv, local_u, delta_t);
local_xv.Update(scene.get_motion_model().get_fvRES());
}
threed_motion_model.func_xp(local_xv);
Vector local_xp = new Vector(threed_motion_model.get_xpRES());
threed_motion_model.func_r(local_xp);
Vector3D rW = threed_motion_model.get_rRES();
threed_motion_model.func_q(local_xp);
Quaternion qWR = threed_motion_model.get_qRES();
// yW = rW + RWR hR
Vector3D hR = new Vector3D(0.0f, 0.0f, FEATURE_INIT_DEPTH_HYPOTHESIS);
// Used Inverse + transpose because this was't compiling the normal way
Vector3D yW = new Vector3D(rW + qWR.RotationMatrix() * hR);
// Then project that point into the current camera position
scene.get_motion_model().func_xp(scene.get_xv());
Fully_Initialised_Feature_Measurement_Model fully_init_fmm =
(Fully_Initialised_Feature_Measurement_Model)(default_feature_type_for_initialisation.more_initialised_feature_measurement_model);
Vector yWVNL = yW.GetVNL3();
fully_init_fmm.func_hi_and_dhi_by_dxp_and_dhi_by_dyi(yWVNL, scene.get_motion_model().get_xpRES());
// Now, this defines roughly how much we expect a feature initialised to move
float suggested_u = fully_init_fmm.get_hiRES()[0];
float suggested_v = fully_init_fmm.get_hiRES()[1];
float predicted_motion_u = camera_width / 2.0f - suggested_u;
float predicted_motion_v = camera_height / 2.0f - suggested_v;
// So, the limits of a "safe" region within which we can initialise
// features so that they end up staying within the screen
// (Making the approximation that the whole screen moves like the
// centre point)
int safe_feature_search_ustart = (int)(-predicted_motion_u);
int safe_feature_search_vstart = (int)(-predicted_motion_v);
int safe_feature_search_ufinish = (int)(camera_width - predicted_motion_u);
int safe_feature_search_vfinish = (int)(camera_height - predicted_motion_v);
if (safe_feature_search_ustart < ((int)((BOXSIZE - 1) / 2) + 1))
safe_feature_search_ustart = (int)((BOXSIZE - 1) / 2 + 1);
if (safe_feature_search_ufinish > (int)camera_width - ((int)((BOXSIZE - 1) / 2) + 1))
safe_feature_search_ufinish = (int)(camera_width - (BOXSIZE - 1) / 2 - 1);
if (safe_feature_search_vstart < ((int)((BOXSIZE - 1) / 2) + 1))
safe_feature_search_vstart = (int)((BOXSIZE - 1) / 2 + 1);
if (safe_feature_search_vfinish > (int)camera_height - ((int)((BOXSIZE - 1) / 2) + 1))
safe_feature_search_vfinish = (int)(camera_height - (BOXSIZE - 1) / 2 - 1);
return FindNonOverlappingRegionNoPredict(safe_feature_search_ustart,
safe_feature_search_vstart,
safe_feature_search_ufinish,
safe_feature_search_vfinish,
scene,
ref init_feature_search_ustart,
ref init_feature_search_vstart,
ref init_feature_search_ufinish,
ref init_feature_search_vfinish, rnd);
}
public static MatrixFixed dvnorm_by_dv(Vector3D v)
{
float vv = v.GetX()*v.GetX() + v.GetY()*v.GetY() + v.GetZ()*v.GetZ();
// We can use dqi_by_dqi and dqi_by_dqj functions because they are the same
float[] a = new float[9];
a[0] = dqi_by_dqi(v.GetX(), vv);
a[1] = dqi_by_dqj(v.GetX(), v.GetY(), vv);
a[2] = dqi_by_dqj(v.GetX(), v.GetZ(), vv);
a[3] = dqi_by_dqj(v.GetY(), v.GetX(), vv);
a[4] = dqi_by_dqi(v.GetY(), vv);
a[5] = dqi_by_dqj(v.GetY(), v.GetZ(), vv);
a[6] = dqi_by_dqj(v.GetZ(), v.GetX(), vv);
a[7] = dqi_by_dqj(v.GetZ(), v.GetY(), vv);
a[8] = dqi_by_dqi(v.GetZ(), vv);
MatrixFixed M = new MatrixFixed(3,3);
return M;
}
/// <summary>
///
/// </summary>
/// <param name="xv">position and orientation of the camera</param>
/// <param name="u">Control vector of accelerations</param>
/// <param name="delta_t">time step in seconds</param>
public override void func_fv_and_dfv_by_dxv(Vector xv, Vector u, float delta_t)
{
Vector3D rold, vold, omegaold, rnew, vnew, omeganew;
Quaternion qold, qnew;
// Separate things out to make it clearer
rold = new Vector3D(0, 0, 0);
vold = new Vector3D(0, 0, 0);
omegaold = new Vector3D(0, 0, 0);
qold = new Quaternion();
extract_r_q_v_omega(xv, rold, qold, vold, omegaold);
Vector3D acceleration = new Vector3D(u);
// rnew = r + v * delta_t
//rnew = (Vector3D)((Point3D)rold + (Point3D)vold * delta_t);
rnew = new Vector3D(rold + vold * delta_t);
// qnew = q x q(omega * delta_t)
// Keep qwt ( = q(omega * delta_t)) for later use
Quaternion qwt = new Quaternion(omegaold * delta_t);
qnew = qold.Multiply(qwt);
// vnew = v
vnew = new Vector3D(vold + acceleration * delta_t);
// omeganew = omega
omeganew = omegaold;
// Put it all together
compose_xv(ref rnew, ref qnew, ref vnew, ref omeganew, ref fvRES);
// cout << "rold qold vold omegaold" << rold << qold
// << vold << omegaold;
// cout << "rnew qnew vnew omeganew" << rnew << qnew
// << vnew << omeganew;
// Now on to the Jacobian...
// Identity is a good place to start since overall structure is like this
// I 0 I * delta_t 0
// 0 dqnew_by_dq 0 dqnew_by_domega
// 0 0 I 0
// 0 0 0 I
dfv_by_dxvRES.SetIdentity();
// Fill in dxnew_by_dv = I * delta_t
MatrixFixed Temp33A = new MatrixFixed(3,3);
Temp33A.SetIdentity();
Temp33A *= delta_t;
dfv_by_dxvRES.Update(Temp33A, 0, 7);
// Fill in dqnew_by_dq
// qnew = qold x qwt ( = q3 = q2 x q1 in Scene/newbits.cc language)
MatrixFixed Temp44A = MatrixFixed.dq3_by_dq2(qwt); //4,4
dfv_by_dxvRES.Update(Temp44A, 3, 3);
// Fill in dqnew_by_domega = d(q x qwt)_by_dqwt . dqwt_by_domega
Temp44A = MatrixFixed.dq3_by_dq1(qold); // Temp44A is d(q x qwt) by dqwt
// Use function below for dqwt_by_domega
MatrixFixed Temp43A = new MatrixFixed(4,3);
dqomegadt_by_domega(omegaold, delta_t, Temp43A);
// Multiply them together
MatrixFixed Temp43B = Temp44A * Temp43A;
// And plug it in
dfv_by_dxvRES.Update(Temp43B, 3, 10);
// cout << "dfv_by_dxvRES" << dfv_by_dxvRES;
}
//returns matrix of dimensions (3,4)
public static MatrixFixed dRq_times_a_by_dq (Quaternion q, Vector3D a)
{
MatrixFixed aMat = new MatrixFixed(3,1);
aMat[0, 0] = a.GetX();
aMat[1, 0] = a.GetY();
aMat[2, 0] = a.GetZ();
MatrixFixed TempR = new MatrixFixed(3,3);
MatrixFixed Temp31 = new MatrixFixed(3,1);
MatrixFixed dRq_times_a_by_dq = new MatrixFixed(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.Update(Temp31, 0, 0);
TempR = dR_by_dqx(q);
Temp31 = TempR * aMat;
dRq_times_a_by_dq.Update(Temp31, 0, 1);
TempR = dR_by_dqy(q);
Temp31 = TempR * aMat;
dRq_times_a_by_dq.Update(Temp31, 0, 2);
TempR = dR_by_dqz(q);
Temp31 = TempR * aMat;
dRq_times_a_by_dq.Update(Temp31, 0, 3);
return dRq_times_a_by_dq;
}
/// <summary>
/// Fill noise covariance matrix Pnn: this is the covariance of
/// the noise vector (V)
/// (Omega)
/// that gets added to the state.
/// Form of this could change later, but for now assume that
/// V and Omega are independent, and that each of their components is
/// independent...
/// </summary>
/// <param name="xv"></param>
/// <param name="v"></param>
/// <param name="delta_t"></param>
public override void func_Q(Vector xv, Vector v, float delta_t)
{
float linear_velocity_noise_variance =
SD_A_component_filter * SD_A_component_filter * delta_t * delta_t;
float angular_velocity_noise_variance =
SD_alpha_component_filter * SD_alpha_component_filter * delta_t * delta_t;
// Independence means that the matrix is diagonal
MatrixFixed Pnn = new MatrixFixed(6,6);
Pnn.Fill(0.0f);
Pnn.Put(0, 0, linear_velocity_noise_variance);
Pnn.Put(1, 1, linear_velocity_noise_variance);
Pnn.Put(2, 2, linear_velocity_noise_variance);
Pnn.Put(3, 3, angular_velocity_noise_variance);
Pnn.Put(4, 4, angular_velocity_noise_variance);
Pnn.Put(5, 5, angular_velocity_noise_variance);
// Form Jacobian dxnew_by_dn
// Is like this:
// I * delta_t 0
// 0 dqnew_by_dOmega
// I 0
// 0 I
// Start by zeroing
MatrixFixed dxnew_by_dn = new MatrixFixed(13,6);
dxnew_by_dn.Fill(0.0f);
// Fill in easy bits first
MatrixFixed Temp33A = new MatrixFixed(3,3);
Temp33A.SetIdentity();
dxnew_by_dn.Update(Temp33A, 7, 0);
dxnew_by_dn.Update(Temp33A, 10, 3);
Temp33A *= delta_t;
dxnew_by_dn.Update(Temp33A, 0, 0);
// Tricky bit is dqnew_by_dOmega
// Is actually the same calculation as in func_fv...
// Since omega and Omega are additive...?
Vector3D rold = new Vector3D(0, 0, 0);
Vector3D vold = new Vector3D(0, 0, 0);
Vector3D omegaold = new Vector3D(0, 0, 0);
Quaternion qold=new Quaternion();
extract_r_q_v_omega(xv, rold, qold, vold, omegaold); // overkill but easy
// Fill in dqnew_by_domega = d(q x qwt)_by_dqwt . dqwt_by_domega
// Temp44A is d(q x qwt) by dqwt
MatrixFixed Temp44A = MatrixFixed.dq3_by_dq1(qold);
// Use function below for dqwt_by_domega
MatrixFixed Temp43A = new MatrixFixed(4,3);
dqomegadt_by_domega(omegaold, delta_t, Temp43A);
// Multiply them together
MatrixFixed Temp43B = Temp44A * Temp43A;
// And then plug into Jacobian
dxnew_by_dn.Update(Temp43B, 3, 3);
// Finally do Q = dxnew_by_dn . Pnn . dxnew_by_dnT
QxRES.Update( dxnew_by_dn * Pnn * dxnew_by_dn.Transpose() );
// cout << "QxRES" << QxRES;
}
// @param v the vector to be rotated
// @returns the resulting rotated vector
public Vector3D Multiply (Vector3D v)
{
return (this.Rotate(v));
}
/// <summary>
/// Extract the component parts of the state x_v . Fills matrices r, q, v, omega with values.
/// </summary>
/// <param name="xv"></param>
/// <param name="r"></param>
/// <param name="q"></param>
/// <param name="v"></param>
/// <param name="omega"></param>
public void extract_r_q_v_omega(Vector xv, Vector3D r, Quaternion q, Vector3D v, Vector3D omega)
{
r.SetVNL3(xv.Extract(3, 0));
Vector qRXYZ = xv.Extract(4, 3);
q.SetRXYZ(qRXYZ);
v.SetVNL3(xv.Extract(3, 7));
omega.SetVNL3(xv.Extract(3, 10));
}
// Rotate a line segment.
// param l the line segment to be rotated
// @returns the resulting rotated line segment
public LineSeg3D Rotate(LineSeg3D l)
{
// Simply rotate both lines
Vector3D i = this.Imaginary();
Vector3D ns = new Vector3D(Vector3D.VectorProduct(i, l.StartPoint()));
Vector3D ne = new Vector3D(Vector3D.VectorProduct(i, l.EndPoint()));
Vector3D deltas = new Vector3D(_r * ns - (Vector3D)(Vector3D.VectorProduct(ns, i)));
Vector3D deltae = new Vector3D(_r * ne - (Vector3D)(Vector3D.VectorProduct(ne, i)));
LineSeg3D result = new LineSeg3D(l.StartPoint() + 2.0f * deltas, l.EndPoint() + 2.0f * deltae);
return (result);
}
/// <summary>
/// Create a state vector x_v from its component parts. Puts the matrices r, q, v, omega into their right places.
/// </summary>
/// <param name="r"></param>
/// <param name="q"></param>
/// <param name="v"></param>
/// <param name="omega"></param>
/// <param name="xv"></param>
public void compose_xv(ref Vector3D r, ref Quaternion q, ref Vector3D v, ref Vector3D omega, ref Vector xv)
{
xv.Update(r.GetVNL3(), 0);
xv.Update(q.GetRXYZ(), 3);
xv.Update(v.GetVNL3(), 7);
xv.Update(omega.GetVNL3(), 10);
}
/// <summary>
///
/// </summary>
/// <param name="xp">current robot position state</param>
/// <param name="yi">3D position of the feature</param>
/// <param name="xp_orig">robot position state when the feature was initially observed</param>
/// <param name="hi">image coordinates of the feature</param>
/// <returns></returns>
public override uint visibility_test(Vector xp, Vector yi, Vector xp_orig, Vector hi)
{
float image_search_boundary = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).IMAGE_SEARCH_BOUNDARY;
uint wdth = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera.ImageWidth();
uint hght = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera.ImageHeight();
uint cant_see_flag = 0;
// Test image boundaries
float x = hi[0];
float y = hi[1];
if ((x < 0.0f + image_search_boundary) ||
(x > (float)(wdth - 1 - image_search_boundary)))
{
cant_see_flag |= Wide_Camera_Point_Feature_Measurement_Model.LEFT_RIGHT_FAIL;
//if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("Visibility test left / right fail.");
}
if ((y < 0.0f + image_search_boundary) ||
(y > (float)(hght - 1 - image_search_boundary)))
{
cant_see_flag |= Wide_Camera_Point_Feature_Measurement_Model.UP_DOWN_FAIL;
//if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("Visibility test up / down fail.");
}
// Do tests on length and angle of predicted view
// hLWi is current predicted vector from head to feature in
// world frame
// This function gives relative position of feature
func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(yi, xp);
// Test the feature's not behind the camera (because projection
// may do strange things)
if (zeroedyiRES[2] <= 0)
{
cant_see_flag |= Wide_Camera_Point_Feature_Measurement_Model.BEHIND_CAMERA_FAIL;
//if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("Behind camera fail.");
}
((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 hLWi = new Vector3D(RWR * (new Point3D(zeroedyiRES)));
// hLWi_orig is vector from head to feature in world frame
// WHEN THAT FEATURE WAS FIRST MEASURED: i.e. when the image
// patch was saved
func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(yi, xp_orig);
((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.func_q(xp_orig);
RotationMatrix RWR_orig = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.get_qRES().RotationMatrix();
Vector3D hLWi_orig = new Vector3D(RWR_orig * (new Point3D(zeroedyiRES)));
// Compare hLWi and hLWi_orig for length and orientation
float mod_hLWi = hLWi.Norm();
float mod_hLWi_orig = hLWi_orig.Norm();
float length_ratio = mod_hLWi / mod_hLWi_orig;
float max_length_ratio = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).MAXIMUM_LENGTH_RATIO;
if ((length_ratio > max_length_ratio) ||
(length_ratio < (1.0f / max_length_ratio)))
{
cant_see_flag |= Wide_Camera_Point_Feature_Measurement_Model.DISTANCE_FAIL;
//if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("Distance fail.");
}
float dot_prod = hLWi * hLWi_orig;
float angle = (float)Math.Acos(dot_prod / (mod_hLWi * mod_hLWi_orig));
if (angle == float.NaN) Debug.WriteLine("Maths error: " + dot_prod + " / " + (mod_hLWi * mod_hLWi_orig));
angle = (angle >= 0.0f ? angle : -angle); // Make angle positive
if (angle > ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).MAXIMUM_ANGLE_DIFFERENCE)
{
cant_see_flag |= Wide_Camera_Point_Feature_Measurement_Model.ANGLE_FAIL;
//if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("Angle fail.");
}
return cant_see_flag; // 0 if OK, otherwise error code
}
/// <summary>
/// Calculate commonly used Jacobian part \partial q(\omega * \Delta t) / \partial \omega .
/// </summary>
/// <param name="omega"></param>
/// <param name="delta_t"></param>
/// <param name="dqomegadt_by_domega"></param>
public void dqomegadt_by_domega(Vector3D omega, float delta_t, MatrixFixed dqomegadt_by_domega)
{
// Modulus
float omegamod = (float)Math.Sqrt(omega.GetX() * omega.GetX() +
omega.GetY() * omega.GetY() +
omega.GetZ() * omega.GetZ());
// Use generic ancillary functions to calculate components of Jacobian
dqomegadt_by_domega.Put(0, 0, dq0_by_domegaA(omega.GetX(), omegamod, delta_t));
dqomegadt_by_domega.Put(0, 1, dq0_by_domegaA(omega.GetY(), omegamod, delta_t));
dqomegadt_by_domega.Put(0, 2, dq0_by_domegaA(omega.GetZ(), omegamod, delta_t));
dqomegadt_by_domega.Put(1, 0, dqA_by_domegaA(omega.GetX(), omegamod, delta_t));
dqomegadt_by_domega.Put(1, 1, dqA_by_domegaB(omega.GetX(), omega.GetY(), omegamod, delta_t));
dqomegadt_by_domega.Put(1, 2, dqA_by_domegaB(omega.GetX(), omega.GetZ(), omegamod, delta_t));
dqomegadt_by_domega.Put(2, 0, dqA_by_domegaB(omega.GetY(), omega.GetX(), omegamod, delta_t));
dqomegadt_by_domega.Put(2, 1, dqA_by_domegaA(omega.GetY(), omegamod, delta_t));
dqomegadt_by_domega.Put(2, 2, dqA_by_domegaB(omega.GetY(), omega.GetZ(), omegamod, delta_t));
dqomegadt_by_domega.Put(3, 0, dqA_by_domegaB(omega.GetZ(), omega.GetX(), omegamod, delta_t));
dqomegadt_by_domega.Put(3, 1, dqA_by_domegaB(omega.GetZ(), omega.GetY(), omegamod, delta_t));
dqomegadt_by_domega.Put(3, 2, dqA_by_domegaA(omega.GetZ(), omegamod, delta_t));
}
/// <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);
*/
}
// Rotate a line segment.
// @param l the line segment to be rotated
// @returns the resulting rotated line segment
public LineSeg3D Rotate(LineSeg3D l)
{
// Simply rotate both lines
float angle = (float)Math.Sqrt(_x * _x + _y * _y + _z * _z);
if (angle < 1e-8)
{
return l;
}
float sn = (float)Math.Sin(angle);
float cs = (float)Math.Cos(angle);
Vector3D axis = new Vector3D(_x / angle, _y / angle, _z / angle);
Vector3D ns = new Vector3D(Point3D.VectorProduct(axis, l.StartPoint()));
Vector3D ne = new Vector3D(Point3D.VectorProduct(axis, l.EndPoint()));
Vector3D deltas = new Vector3D(sn * ns + (cs - 1.0f) * (Point3D.VectorProduct(ns, axis)));
Vector3D deltae = new Vector3D(sn * ne + (cs - 1.0f) * (Point3D.VectorProduct(ne, axis)));
return new LineSeg3D(l.StartPoint() + deltas, l.EndPoint() + deltae);
}
