Concern Regarding gtsam::Pose3::AttitudeFactor

Bradley Kohler

unread,

Aug 10, 2021, 4:05:59 PM8/10/21

to gtsam users

To whom it may concern,

I am fairly new to GTSAM. But am starting to get more comfortable with the library.

I thought I'd highlight what I believe to be a degeneracy case in the gtsam::Pose3AttitudeFactor.

I have uploaded the problem here: https://gist.github.com/studentbrad/91cbe5b13995f77fe44327960b10b8d2.

From what I can see, there exists two minimums in the following problem.

R = I = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] (upside up)
R = [[-1, 0, 0], [0, -1, 0], [0, 0, 1]] (upside down)

When there should only be one (upside up).

In the problem, if I set the initial estimate to a rotation about the x-axis of 3 * pi / 4 radians, the solution converges to upside down.

Alternatively, if I set the initial estimate to a rotation about the x-axis of pi / 4 radians, the solution converges to upside up.

I believe this would not happen if the error were computed as the arc-length distance from one point on the unit sphere to the next.

In this case, upside down would result in a maximum error, instead of a minimum.

Let me know what you think,

Bradley Kohler

unread,

Aug 10, 2021, 4:21:29 PM8/10/21

to gtsam users

Correction: The Rotation matrix for upside down is R = [[1, 0, 0], [0, -1, 0], [0, 0, -1]].

Bradley Kohler

unread,

Sep 1, 2021, 5:59:37 PM9/1/21

to gtsam users

Quick update:

I added a candidate solution called CustomPose3AttitudeFactor to my public Gist: https://gist.github.com/studentbrad/91cbe5b13995f77fe44327960b10b8d2

I also ran 100 test cases and posted the output as a file.

You will see that using the angle/arc-distance as the error results in the correct output regardless of the initial estimate where the gtsam::Pose3AttitudeFactor does not.