jaelrod
commented
3 years ago Somehow (I think it is maybe happening when the LM optimizer is initialized) the left operand of Rot2*Rot2 has an uninitialized c_ and s_ during an invocation before the optimizer takes its first iteration. I think what's happening is that when this uninitialized rotation gets inverted and multiplied during the optimization iterations, the cosine and/or sine of the rotation component of pose estimates becomes extremely positive or negative, the result of which is atan2() for the pose heading angle returning one of its inflection points, as documented in cppreference. Have not figured out from where this uninitialized rotation is coming, but given that reproducing it repeatedly yields wildly different figures for c_ and s_ for that rotation that shows up before the optimization, that does appear to be what is happening.
Debug output from Rot2 before optimizer's first linearization:
Rot2::operator*(), c_: 3.03428333399e-86, s_: 1.14371993161e-71
Initial values
Values with 1 values:
Value 0: (gtsam::Pose2)
(10.0808679401, 9.99346051488, 0.428400412304)
ProfFan
jlblancoc
dellaert
varunagrawal
asasine
Description
I have some unit tests that chain together some BetweenFactor using an odometry mock, inject some slightly contradictory supplemental evidence in the form of other factors, such as a unary factor akin to the "GPS-like" factor from LocalizationExample.cpp, and finally use LevenbergMarquardtOptimizer to
optimize()the factor graph and assert the result.For example, a test might start with some rather noisy mocked odometry moving forward at a consistent speed with a heading of pi/4, qualify the noisy estimates with some very not-noisy GPS-like factors that suggest movement is actually happening in the pi/2 direction, rather than the pi/4 direction, then optimize the graph and make sure that the optimized poses were along the trajectory suggested by the more reliable GPS-like factors.
Such a test and several others work fine on my desktop (x86_64, Ubuntu 18.04) but fail on my Jetson Xavier NX (aarch64, Ubuntu 18.04).
Checking out borglab gtsam source from github and running
make checkto invoke unit tests on the aarch64 device results in many failures, which I expect are related to my own, the following example of which I think is similar to one of the more basic included examples in gtsam :I am in the process of debugging gtsam source through verbose optimizer output and will provide updates as I make progress, but I am suspicious of Eigen anti-aliasing or use of
autokeyword and type resolution on aarch64.One thing I have observed so far only in aarch64 is:
Another thing I have noticed in aarch64 is that the initial total error of the same system seems much lower than that of amd64, possibly resulting in less/no optimization iterations.
Update 1: Seems like the calculation of error of
BetweenFactor<Pose2>is indeed incorrect on aarch64.Debug output from amd64 of BetweenFactor::evaluateError for the very first error calculation based on estimates:
Debug output from aarch64 of BetweenFactor::evaluateError for almost the exact same poses:
Playing around inside of
BetweenFactorandLieGroupbut haven't had much luck so far.Update 2: I haven't 100% figured out why, but
Rot2 inverse()in geometry/Rot2.h was returning an inverse with incorrect heading angle when inverting Pose2 as a part of factorization ofBetweenFactor<Pose2>.Changing this line, https://github.com/borglab/gtsam/blob/aec2cf06a5501840f0302344bca03c197522a335/gtsam/geometry/Rot2.h#L110 to read the following fixes at least that issue (seems like there are more still to find):
Rot2 inverse() const { return fromCosSin(c_, -s_);}Like I said, not yet sure why this makes a difference, as all fromCosSin appears to be doing is normalizing c & s, and those should have already been normalized...
Update 3: Also, Rot2::operator*(const Rot2& R) does not behave as expected in aarch64. The following seems to relieve the problem: return fromAngle(theta() + R.theta());
Although, again, I am not sure why this works, as making use of the trig identity for cos(th1+th2) and sin(th1+th2) as is done in master branch should be identical in terms of resulting behavior...
Steps to reproduce
BetweenFactor<Pose2>and some initial estimatesExpected behavior
Factor graph optimizes in the same way it does on amd64.
Environment
Jetson Xavier NX, Ubuntu 18.04