OdometryExample.cpp covariance question

[Nghia]

I'm trying to learn GTSAM from the examples and noticed something odd, maybe due to my lack of understanding. When I run OdometryExample.cpp it appears to run as expected with the last output being

x3 covariance:
   0.17 8.6e-18 2.7e-18
8.6e-18    0.37    0.06
2.7e-18    0.06    0.03

The initial pose is Pose2(0, 0, 0). The covariance is effectively an unrotated ellipse for (x,y) if I was to plot it.

If I then modify line 66 to

  Pose2 priorMean(0.0, 0.0, M_PI_2); // change orientation

and run the example again I get

x3 covariance:
    0.17 -1.7e-10 -5.2e-11
-1.7e-10     0.37     0.06
-5.2e-11     0.06     0.03

which is nearly identical to the first run with Pose2(0, 0, 0), an unrotated ellipse. From my brief experience with EKF SLAM I expect the ellipse to be rotated with the robot's orientation.

What's going on?

[Duy-Nguyen Ta]

That's the uncertainty of the tangent vector, in short, in the pose's local coordinate frame. You need to transpose it to plot in the world frame. See, e.g., "matlab/+gtsam/plotPose2.m"

[Nghia]

Thanks!