Open holmesco opened 1 year ago
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.
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.
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:
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:TODO:
vec2tran
for small angles issuetran2vec
issue encountered by David, Daniil