Truncation Issue

[holmesco]

There is currently a truncation issue when initializing Transformation class with a Lie Algebra vector that has a small rotation component. This kinds of vectors show up frequently when applying retractions for SE3 optimization schemes. Example:

del_xi = np.array([[0,0,0,alpha,0,0]]).T
T_delta = pylgmath.se3.transformation.Transformation(xi_ab=del_xi)
T_bar = T_delta @ T_bar

Currently, if alpha is set to lower than approximately 1e-7, T_delta is set to identity. This is due to the default "analytical" solution used in the underlying se3op.vec2tran, which uses numpy.sin and numpy.cos. The tolerance for use of the analytical solution should be set to 1e-7 rather than the current 1e-12.

The issue can be avoided if num_terms argument is passed to the constructor:

T_delta = pylgmath.se3.transformation.Transformation(xi_ab=del_xi, num_terms=20)

TODO:

• create unit tests for Connor's vec2tran for small angles issue
• create unit tests for the tran2vec issue encountered by David, Daniil
• should the methods automatically default to numerical for small translations and rotations?
• propose a fix that covers both issues
• Apply fix to both pylgmath and C++/lgmath
• make sure the API is consistent between the C++ and Python code

[keenan-burnett]

We have a separate but related issue that I'll include here:

From David:

I think this problem really is an issue with lgmath, and not the T_applanix_lidar calib. Certain calib values are just coincidentally triggering the problem.
There are three matrices that we compose together to create T_robot_radar (yfwd2xfwd, T_applanix_lidar, and T_radar_lidar). The current implementation multiplies these as eigen matrices, and constructs an EdgeTransform (lgmath transform). I tried constructing lgmath transforms for each of these matrices and multiplying the lgmath transforms to create T_robot_radar, but the numerical problem still occurs. If the SO(3) "reprojection" method in lgmath is to be trusted (which runs from the constructor, so it always runs), this shouldn't happen
The quick temporary fix is to change the tolerance in the rot2vec function of lgmath. In line 85 of Operations.cpp (https://github.com/utiasASRL/lgmath/blob/c97d13a883bca8915d94093e0c5f80236a889274/src/so3/Operations.cpp#L85), changing the 1e-9 to 1e-7 avoids the problem.
I need more time to look into lgmath and come up with a real solution. I'm not satisfied with just changing the tolerance.

[keenan-burnett]

Connor's issue here is that he wants
x == tran2vec(vec2tran(x))
for small values of x

we also generally want

T ~= vec2tran(tran2vec(T))

We may sometimes get floating point rounding errors, the trick is to determine when this is acceptable.
Also, we don't want to set the tolerance so high that we're always using the numerical version since the analytical version is much faster.