Is the evaluateError function in UnaryFactor correct?

[navid km]

Hi all,

I was wondering if the provided evaluateError function in the gtsam.pdf documentation is accurate:

https://github.com/borglab/gtsam/blob/4.0.3/doc/gtsam.pdf

As I have already posted in the github, a simple test case with an initial 90 degrees orientation fails: 

https://github.com/borglab/gtsam/issues/704

This has been tested both with simple test cases as well as real gps data and they all give wrong results.

Discussion:

In the above function the error is calculated in the global frame without considering the orientation of the node q, while as I understand from the gtsam documentations, the measurement function h(q) in this case should represent the measurement from the node's local (body) frame. 

In other words, the orientation of pose q affects the measurement prediction which implies that the error function should take q.theta() into account.

New evaluateError function

If we instead test with the following error function, the aforementioned test case gives pretty accurate results. This is also the case with the real data. Note that the identity Jacobian is used (which is wrong in theory):

However adding the correct Jacobian gives incorrect results.

I wonder if the error calculation is correct or am I missing something?

Thanks!

Navid