public void AddSubtract()
{
double[] recovered = a.AddOdometry(odometry).DiffOdometry(a);
Assert.AreEqual(odometry[0], recovered[0], 1e-3);
Assert.AreEqual(odometry[1], recovered[1], 1e-3);
Assert.AreEqual(odometry[2], recovered[2], 1e-3);
Assert.AreEqual(odometry[3], recovered[3], 1e-3);
Assert.AreEqual(odometry[4], recovered[4], 1e-3);
Assert.AreEqual(odometry[5], recovered[5], 1e-3);
}
public void Jacobian()
{
const double ds = 1e-5;
Pose3D f = a.AddOdometry(odometry2);
double[][] jacobian = a.AddOdometryJacobian(odometry2);
double[][] numerical = new double[6][];
for (int i = 0; i < 6; i++)
{
double[] da = new double[6];
da[i] = ds;
Pose3D a2 = a.AddGlobal(da);
//Pose3D f2 = a.AddOdometry(Pose3D.Identity.FromLinear(odometry2).AddGlobal(da).ToLinear());
Pose3D f2 = a2.AddOdometry(odometry2);
double[] df = f2.SubtractGlobal(f);
numerical[i] = df.Divide(ds);
}
numerical = numerical.Transpose();
Console.WriteLine(jacobian.ToString("f6"));
Console.WriteLine("---");
Console.WriteLine(numerical.ToString("f6"));
Assert.IsTrue(numerical.IsEqual(jacobian, 1e-4));
}
public void SubtractAdd()
{
Pose3D recovered = b.AddOdometry(a.DiffOdometry(b));
Assert.AreEqual(a.State[0], recovered.State[0], 1e-3);
Assert.AreEqual(a.State[1], recovered.State[1], 1e-3);
Assert.AreEqual(a.State[2], recovered.State[2], 1e-3);
Assert.AreEqual(a.State[3], recovered.State[3], 1e-3);
Assert.AreEqual(a.State[4], recovered.State[4], 1e-3);
Assert.AreEqual(a.State[5], recovered.State[5], 1e-3);
Assert.AreEqual(a.State[6], recovered.State[6], 1e-3);
}