/// <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 static MatrixFixed dq3_by_dq2(Quaternion q1)
{
float[] a = new float[16];
a[0] = q1.R();
a[1] = -q1.X();
a[2] = -q1.Y();
a[3] = -q1.Z();
a[4] = q1.X();
a[5] = q1.R();
a[6] = q1.Z();
a[7] = -q1.Y();
a[8] = q1.Y();
a[9] = -q1.Z();
a[10] = q1.R();
a[11] = q1.X();
a[12] = q1.Z();
a[13] = q1.Y();
a[14] = -q1.X();
a[15] = q1.R();
MatrixFixed M = new MatrixFixed(a);
return M;
}
// Combine two rotations (this is VERY costly).
// @param aa the inner rotation
// @returns the resulting rotation (like this*q for matrices)
public AngleAxis Multiply (AngleAxis aa)
{
// This is best done as quaternions
Quaternion q1 = new Quaternion(this);
Quaternion q2 = new Quaternion(aa);
return new AngleAxis(q1.Multiply(q2));
}
/// <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);
}
public static MatrixFixed dqnorm_by_dq(Quaternion q)
{
float qq = q.GetR()*q.GetR() +
q.GetX()*q.GetX() +
q.GetY()*q.GetY() +
q.GetZ()*q.GetZ();
float[] a = new float[16];
a[0] = dqi_by_dqi(q.GetR(), qq);
a[1] = dqi_by_dqj(q.GetR(), q.GetX(), qq);
a[2] = dqi_by_dqj(q.GetR(), q.GetY(), qq);
a[3] = dqi_by_dqj(q.GetR(), q.GetZ(), qq);
a[4] = dqi_by_dqj(q.GetX(), q.GetR(), qq);
a[5] = dqi_by_dqi(q.GetX(), qq);
a[6] = dqi_by_dqj(q.GetX(), q.GetY(), qq);
a[7] = dqi_by_dqj(q.GetX(), q.GetZ(), qq);
a[8] = dqi_by_dqj(q.GetY(), q.GetR(), qq);
a[9] = dqi_by_dqj(q.GetY(), q.GetX(), qq);
a[10] = dqi_by_dqi(q.GetY(), qq);
a[11] = dqi_by_dqj(q.GetY(), q.GetZ(), qq);
a[12] = dqi_by_dqj(q.GetZ(), q.GetR(), qq);
a[13] = dqi_by_dqj(q.GetZ(), q.GetX(), qq);
a[14] = dqi_by_dqj(q.GetZ(), q.GetY(), qq);
a[15] = dqi_by_dqi(q.GetZ(), qq);
MatrixFixed M = new MatrixFixed(a);
return M;
}
public bool EqualTo(Quaternion q)
{
return ((q._x == _x) && (q._y == _y) && (q._z == _z) && (q._r == _r));
}
/*
public RotationMatrix(AngleAxis angleaxis) : base(3,3)
{
Set(angleaxis);
}
*/
public RotationMatrix(Quaternion q) : base(3,3)
{
Set(q);
}
public static MatrixFixed dR_by_dqz(Quaternion q)
{
float q0 = q.R();
float qx = q.X();
float qy = q.Y();
float qz = q.Z();
float[] a = new float[9];
a[0] = -2*qz;
a[1] = -2*q0;
a[2] = 2*qx;
a[3] = 2*q0;
a[4] = -2*qz;
a[5] = 2*qy;
a[6] = 2*qx;
a[7] = 2*qy;
a[8] = 2*qz;
MatrixFixed M = new MatrixFixed(3,3);
return M;
}
/// <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 override void func_xpose_and_Rpose(Vector xv)
{
func_xp(xv);
// Turn xp vector (x, y, z, q0, q1, q2, q3) into pose
xposeRES = xpRES.Extract(3, 0);
Vector qvec = xpRES.Extract(4, 3);
Quaternion q = new Quaternion();
q.SetRXYZ(qvec);
RposeRES = q.RotationMatrix();
}
public void SetUp3DDisplays()
{
// Set display virtual camera parameters to match those of the real camera
// being used in MonoSLAM
Partially_Initialised_Feature_Measurement_Model default_pifmm =
monoslaminterface.GetDefaultFeatureTypeForInitialisation();
if (default_pifmm != null)
{
Line_Init_Wide_Point_Feature_Measurement_Model default_wide_pifmm =
(Line_Init_Wide_Point_Feature_Measurement_Model)default_pifmm;
Graphics_Fku = default_wide_pifmm.get_camera().Fku();
Graphics_Fkv = default_wide_pifmm.get_camera().Fkv();
Graphics_U0 = default_wide_pifmm.get_camera().U0();
Graphics_V0 = default_wide_pifmm.get_camera().V0();
Graphics_Kd1 = default_wide_pifmm.get_camera().Kd1();
// First display for external 3D view
/*
threedtool = new ThreeDToolGlowWidget(this,
controlpanel1->Width() + controlpanel2->Width(), 0,
monoslaminterface->GetCameraWidth(),
monoslaminterface->GetCameraHeight());
threedtool->DrawEvent.Attach(this, &MonoSLAMGlow::Draw3D);
threedtool->ProcessHitsEvent.Attach(this, &MonoSLAMGlow::ProcessHits);
threedtool->SetCameraParameters(Graphics_Fku, Graphics_Fkv,
Graphics_U0, Graphics_V0);
*/
// Set start position for GL camera in 3D tool
// This is x, y, z position
Vector rthreed = new Vector(0.0f, 0.2f, -1.5f);
// This is camera orientation in my normal coordinate system
// (z forward, y up, x left)
Quaternion qWRthreed = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
// We need to adjust by this rotation to fit GL coordinate frame
// (z backward, y up, x right)
// So rotate by pi about y axis
Quaternion qROthreed = new Quaternion(0.0f, 1.0f, 0.0f, 0.0f);
Quaternion qWOthreed = qWRthreed.Multiply(qROthreed);
//threedtool.SetCameraPositionOrientation(rthreed, qWOthreed);
// Second 3D display for images and augmented reality
/*
image_threedtool = new ThreeDToolGlowWidget(this,
controlpanel1->Width() + controlpanel2->Width(), threedtool->Height(),
monoslaminterface->GetCameraWidth(), monoslaminterface->GetCameraHeight());
image_threedtool.DrawEvent.Attach(this, &MonoSLAMGlow::ImageDraw3D);
image_threedtool.ProcessHitsEvent.Attach(this, &MonoSLAMGlow::ImageProcessHits);
image_threedtool.SetCameraParameters(Graphics_Fku, Graphics_Fkv, Graphics_U0, Graphics_V0);
*/
// Set up initial state of virtual camera for image display to match
// state vector
Scene_Single scene = monoslaminterface.GetScene();
if (scene != null)
{
Vector v = scene.get_xv();
if (v != null)
{
scene.get_motion_model().func_xp(v);
ThreeD_Motion_Model threed_motion_model = (ThreeD_Motion_Model)scene.get_motion_model();
Vector3D r = threed_motion_model.get_rRES();
Quaternion qWR = threed_motion_model.get_qRES();
// q is qWR between world frame and Scene robot frame
// We need to adjust by this rotation to fit GL coordinate frame
// (z backward, y up, x right)
// So rotate by pi about y axis
Quaternion qRO = new Quaternion(0.0f, 1.0f, 0.0f, 0.0f);
Quaternion qWO = qWR.Multiply(qRO);
//image_threedtool.SetCameraPositionOrientation(r, qWO);
}
else
{
Debug.WriteLine("Scene xp not defined");
}
}
else
{
Debug.WriteLine("No scene object defined");
}
}
else
{
Debug.WriteLine("No partially initialised feature measurement model found");
}
}
/// <summary>
/// Form the covariance matrix Q of the process noise associated with x_v .
/// </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)
{
// 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...
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
// Start by zeroing
MatrixFixed dxnew_by_dn = new MatrixFixed(7, 6);
dxnew_by_dn.Fill(0.0f);
// The translation part is just I \Delta t
MatrixFixed Temp33A = new MatrixFixed(3, 3);
Temp33A.SetIdentity();
Temp33A *= delta_t;
dxnew_by_dn.Update(Temp33A, 0, 0);
// qnew = q x \Omega \Deltat
// dqnew_by_d\Omega = dqnew_by_d\Omega\Delta t . d\Omega\Delta t_by_d\Omega
// Get the first part
Vector qRXYZ = xv.Extract(4, 3);
Quaternion qold = new Quaternion();
qold.SetRXYZ(qRXYZ);
MatrixFixed Temp44A = MatrixFixed.dq3_by_dq1(qold);
// Use function below for dqwt_by_dOmega
Vector Omega = new Vector(3);
Omega.Fill(SD_alpha_component_filter);
MatrixFixed Temp43A = new MatrixFixed(4, 3);
dqomegadt_by_domega(new Vector3D(Omega), 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());
}
public AngleAxis(Quaternion q)
{
Set(q);
}
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 void Set(Quaternion q)
{
this.Equals(q.RotationMatrix());
}
//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>
///
/// </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;
}
public Quaternion normalise_quaternion(Quaternion q)
{
// qq is current magnitude of q
float qq = (float)Math.Sqrt(q.GetR() * q.GetR() +
q.GetX()*q.GetX() + q.GetY()*q.GetY() + q.GetZ()*q.GetZ());
Quaternion qnorm = new Quaternion(
q.GetX() / qq,
q.GetY() / qq,
q.GetZ() / qq,
q.GetR() / qq);
return qnorm;
}
/// <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;
}
// Combine two rotations.
// @param q the inner rotation
// @returns the resulting rotation (like this*q for matrices)
public Quaternion Multiply (Quaternion q)
{
// real and img parts of args
float r1 = this._r;
float r2 = q._r;
Vector3D i1 = this.Imaginary();
Vector3D i2 = q.Imaginary();
// Real is product of real, and dot-product of imag
float real_v = (r1 * r2) - (i1 * i2);
// Imag is cross product of imag + real*imag for each
Point3D img = Point3D.VectorProduct(i1, i2);
//Point3D img2 = (img + (i2 * r1) + (i1 * r2));
// Finally, build the result
Quaternion prod = new Quaternion(img.GetX(), img.GetY(), img.GetZ(), real_v);
return prod;
}
/// <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));
}
public bool NotEqualTo (Quaternion q)
{
return ((q._x != _x) || (q._y != _y) || (q._z != _z) || (q._r != _r));
}
public void Set(Quaternion q)
{
float r = q.GetR();
_x = q.GetX();
_y = q.GetY();
_z = q.GetZ();
float norm = (float)Math.Sqrt(_x * _x + _y * _y + _z * _z);
if (norm > 0)
{
float scaling = 2.0f * (float)Math.Atan2(norm, r) / norm;
_x *= scaling;
_y *= scaling;
_z *= scaling;
}
}