Numerical derivatives inaccurate for mathutils::atan2()

[tmcg0]

See test of the jacobians numerically in testHipIntExtRotAngFactor.cpp.

The derivatives pass most of the time. But sometimes they fail, with error about 1e-3. I can't tell if that's too high in that our derivatives are incorrect. Sometimes the numerical derivatives also blow up and return unlikely values.

This issue is likely either due to either

1. incorrect analytical jacobians or
2. choosing ill-conditioned values for your input variables to the function for numerical derivative approximation, leading to numerical derivatives which are ill-conditioned as well. this would be a problem in the unit test.

[tmcg0]

Some progress: when the numerical derivative blows up, it's because the calculated angle is near pi or -pi. See screenshot below. Will update unit test to only check derivatives if angle is a little bit away from the boundary.

[tmcg0]

It seems like it's multiple jacobians that will randomly fail this, whereas I was trying to target a specific one. From running a few times, the following will fail at threshold 1e-3:
1,4,6
(and note that 7 probably also fails since 6 and 7 are nearly identical but 6 will error first)
What do these have in common? They're the derivatives w.r.t. the two poses and the distal imu vectors v1 and v2 (from the imu to the joints)

[tmcg0]

On second thought, I couldn't get unsignedAngle() derive to pass with any tighter tolerance than 1.0e-4. I think that function is the underlying problem here. Changing title of issue, investigate this. See unit test testGenericMath.cpp.

[tmcg0]

For unsignedAngle(v1,v2), this does seem to be related to norm(cross(v1,v2)). If this norm is small, the numerical derivatives don't pass. Maybe this is just a classical numerical conditioning issue?

[tmcg0]

Bug identified: it's related to the derivative of atan(y,x). When y or x gets really small, this causes atan2's derivative to blow up. See testGenericMath.cpp, function testatan2() for more info.

I'm not gonna close this for now. It leads to the question: what do we do about this? Do we just make sure we are always testing with non-small x and y?

To be clear: This is a problem not with bioslam's analytical derivative for atan2, but rather with numerical approximation of that derivative for unit testing. We test with random values, so it's possible that it would best to just put in some simple if/then logic to only test with non-small values of x and y.

[tmcg0]

I didn't find the real reason this test was failing. However, since it was obviously just due to the numerical derivative blowing up I've added a hack fix: just disregard tests where the numerical derivative blew up. Not great form, but fixes the test for now.