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