/// <summary>
/// Obtain a linearization for the AddOdometry operation around this pose.
/// It follows the form
/// f(x[k-1], u) - x[k] ~ F dx[k-1] + G dx[k] - a.
/// </summary>
/// <param name="delta">Odometry reading.</param>
/// <returns>Jacobian (F).</returns>
public double[][] AddOdometryJacobian(double[] delta)
{
Quaternion dorientation = Quaternion.Identity.Add(new double[3] {
delta[3], delta[4], delta[5]
});
Quaternion sqrt = dorientation.Sqrt();
double[][] Cmid = (Orientation * sqrt).ToMatrix();
double[][] Cdelta = dorientation.ToMatrix();
double[][] Csqrt = sqrt.ToMatrix();
double[][] crossdX = Util.CrossProductMatrix(new double[3] {
delta[0], delta[1], delta[2]
});
double[][] identity = new double[3][] { new double[3] {
1, 0, 0
},
new double[3] {
0, 1, 0
},
new double[3] {
0, 0, 1
} };
double[][] dxdq = (-1.0).Multiply(Cmid).Multiply(crossdX).Multiply(Csqrt.Transpose());
return(identity.Concatenate(dxdq).VConcatenate(
MatrixExtensions.Zero(3).Concatenate(Cdelta.Transpose())));
}
/// <summary>
/// Obtain the jacobian of the measurement model wrt. the pose.
/// </summary>
/// <param name="pose">Pose from which the measurement was made.</param>
/// <param name="landmark">Landmark 3d location against which the measurement is performed.</param>
/// <returns>Measurement model linearization jacobian.</returns>
public double[][] MeasurementJacobianP(Pose3D pose, double[] landmark)
{
double[] diff = landmark.Subtract(pose.Location);
Quaternion local = pose.Orientation.Conjugate() *
new Quaternion(0, diff[0], diff[1], diff[2]) * pose.Orientation;
// the jacobian of the homography projection part is given by
// f/z I 0
// sgn(loc.z) X/|X|
// (arbitrarily choosing -1 as the sign for zero)
double mag = ((local.Z > 0) ? 1 : -1) * Math.Sqrt(local.X * local.X + local.Y * local.Y + local.Z * local.Z);
double[][] jprojection = { new double[3] {
VisionFocal / local.Z, 0, -VisionFocal * local.X / (local.Z * local.Z)
},
new double[3] {
0, VisionFocal / local.Z, -VisionFocal * local.Y / (local.Z * local.Z)
},
new double[3] {
local.X / mag, local.Y / mag, local.Z / mag
} };
// the jacobian of the change of coordinates part of
// the measurement process is given by
// -Prot' -Prot' [l - Ploc]_x
double[][] jlocation = (-1.0).Multiply(pose.Orientation.Conjugate().ToMatrix());
double[][] jrotation = jlocation.Multiply(Util.CrossProductMatrix(diff));
double[][] jlocal = jlocation.Concatenate(jrotation);
return(jprojection.Multiply(jlocal));
}