Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix / Move Rot3::quaternion to deprecated block #1219

Merged
merged 3 commits into from
Jun 10, 2022

Conversation

HViktorTsoi
Copy link
Contributor

@HViktorTsoi HViktorTsoi commented Jun 7, 2022

This PR fixes #1209. More details about this issue can be found in the group discussion https://groups.google.com/g/gtsam-users/c/AxbAAEgMHTA.

Basically, this issue is because Rot3::quaternion() returns a Vector as [qw, qx, qy, qz], not a quaternion. Vector is a typedef for Eigen::VectorXd, for which .x() returns the first element (instead of qw), .y() the second (so here qx), etc... Therefore this Rot3::quaternion() API is very non-intuitive and it could lead to a bug in user code if they don't pay attention enough.

This fix modifies the Rot3::quaternion() API and makes sure the ordering of the returned [qw,qx,qy,qz] are consistent with gtsam::Quaternion.

@HViktorTsoi
Copy link
Contributor Author

HViktorTsoi commented Jun 7, 2022

We verified this fix by adding a group of unit tests in testRot3.cpp 5acd6ac#diff-989728ef3992dedd91cd1a2aa61ac013c4419bcd66c9d1dbe1240e6957d6c383R592:

// Check accessing the quaternion components via Rot3::quaternion
EXPECT(assert_equal(q1.w(), R1.quaternion().w()));
EXPECT(assert_equal(q1.x(), R1.quaternion().x()));
EXPECT(assert_equal(q1.y(), R1.quaternion().y()));
EXPECT(assert_equal(q1.z(), R1.quaternion().z()));
EXPECT(assert_equal(q2.w(), R2.quaternion().w()));
EXPECT(assert_equal(q2.x(), R2.quaternion().x()));
EXPECT(assert_equal(q2.y(), R2.quaternion().y()));
EXPECT(assert_equal(q2.z(), R2.quaternion().z()));

The original gtsam (at least before commit 9a1eb02 on 2022-06-08) would not pass these tests and give the following error log:

/home/hvt/Code/gtsam_develop/gtsam/cmake-build-debug/gtsam/geometry/tests/testRot3
Not equal:
expected:
0.710997
actual:
0.105395
/home/hvt/Code/gtsam_develop/gtsam/gtsam/geometry/tests/testRot3.cpp:593: Failure: "assert_equal(q1.w(), R1.quaternion().w())"
Not equal:
expected:
0.360544
actual:
0.710997
/home/hvt/Code/gtsam_develop/gtsam/gtsam/geometry/tests/testRot3.cpp:594: Failure: "assert_equal(q1.x(), R1.quaternion().x())"
Not equal:
expected:
0.59446
actual:
0.360544
/home/hvt/Code/gtsam_develop/gtsam/gtsam/geometry/tests/testRot3.cpp:595: Failure: "assert_equal(q1.y(), R1.quaternion().y())"
Not equal:
expected:
0.105395
actual:
0.59446
/home/hvt/Code/gtsam_develop/gtsam/gtsam/geometry/tests/testRot3.cpp:596: Failure: "assert_equal(q1.z(), R1.quaternion().z())"
Not equal:
expected:
0.263361
actual:
0.599136
/home/hvt/Code/gtsam_develop/gtsam/gtsam/geometry/tests/testRot3.cpp:597: Failure: "assert_equal(q2.w(), R2.quaternion().w())"
Not equal:
expected:
0.571813
actual:
0.263361
/home/hvt/Code/gtsam_develop/gtsam/gtsam/geometry/tests/testRot3.cpp:598: Failure: "assert_equal(q2.x(), R2.quaternion().x())"
Not equal:
expected:
0.494678
actual:
0.571813
/home/hvt/Code/gtsam_develop/gtsam/gtsam/geometry/tests/testRot3.cpp:599: Failure: "assert_equal(q2.y(), R2.quaternion().y())"
Not equal:
expected:
0.599136
actual:
0.494678
/home/hvt/Code/gtsam_develop/gtsam/gtsam/geometry/tests/testRot3.cpp:600: Failure: "assert_equal(q2.z(), R2.quaternion().z())"
There were 8 failures

Process finished with exit code 8

While the fix can pass the tests:

There were no test failures

Process finished with exit code 0

Copy link
Member

@dellaert dellaert left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry, no - we can deprecate methods (as I alluded to in the group) but definitely not change their behavior/meaning. I'd simply deprecate this method and make sure we use toQuaternion everywhere else.

@HViktorTsoi
Copy link
Contributor Author

HViktorTsoi commented Jun 7, 2022

Sorry, no - we can deprecate methods (as I alluded to in the group) but definitely not change their behavior/meaning. I'd simply deprecate this method and make sure we use toQuaternion everywhere else.

OK, in that case I will create another PR, try to move quaternion() into a deprecated block and replace possible occurrences with toQuaternion().

@dellaert
Copy link
Member

dellaert commented Jun 7, 2022

Note you could just re-use this PR.

@HViktorTsoi HViktorTsoi reopened this Jun 8, 2022
@HViktorTsoi HViktorTsoi changed the title Fix / The component ordering issue of Rot3::quaternion Fix / Move Rot3::quaternion to deprecated block Jun 8, 2022
@HViktorTsoi
Copy link
Contributor Author

HViktorTsoi commented Jun 8, 2022

Move Rot3::quaternion to deprecated block, and also replace its usage with toQuaternion in timeShonanAveraging.cpp

@HViktorTsoi HViktorTsoi requested a review from dellaert June 8, 2022 09:17
Copy link
Member

@dellaert dellaert left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Awesome, thanks ! Please respond to my comment and then I will merge after CI passes.

gtsam_unstable/timing/timeShonanAveraging.cpp Show resolved Hide resolved
@HViktorTsoi
Copy link
Contributor Author

HViktorTsoi commented Jun 8, 2022

Seems all Python CI failed with the same error:

/home/runner/work/gtsam/gtsam/build/python/geometry.cpp:290:62: error: ‘class gtsam::Rot3’ has no member named ‘quaternion’
.def("quaternion",[](gtsam::Rot3* self){return self->quaternion();})

It's because in the CI configuration .github/python.sh, the GTSAM_ALLOW_DEPRECATED_SINCE_V42 is set to OFF, therefore, the Rot3::quaternion API that I put into a GTSAM_ALLOW_DEPRECATED_SINCE_V42 block is ignored when compiling, while the wrap Module can still find that API from geometry.i, and then generates the geometry.cpp with it.

Currently, I have 2 possible solutions for this, and may need some advice from the project maintainers:

  1. Remove the GTSAM_ALLOW_DEPRECATED_SINCE_V42 macro around the Rot3::quaternion() API in this fix, and only add a GTSAM_DEPRECATED before Rot3::quaternion. This is less harmful, but when using the python/matlab bindings, the user can only get the deprecated warning once during compiling;

  2. Keep the GTSAM_ALLOW_DEPRECATED_SINCE_V42 macro, but remove the Rot3::quaternion() API from geometry.i, so there won't be quaternion() API in python/matlab bindings. This may cause some API-breaking issues, but is trackable.

@HViktorTsoi HViktorTsoi requested a review from dellaert June 9, 2022 14:40
@varunagrawal
Copy link
Collaborator

Something to keep note of, gtsam::Quaternion's coeffs method returns the quaternion as x y z w.

@varunagrawal
Copy link
Collaborator

I'm sorry but I am not seeing the argument for this. I just updated some code to use toQuaternion and it is less readable.

Previously I could do

q = wTc.rotation().quaternion()

but now we have to do

q = wTb.rotation().toQuaternion()
q = np.asarray([q.w(), q.x(), q.y(), q.z()])

which isn't functional since we can't chain methods together anymore e.g.:

do_something(wTb.rotation().quaternion())

Moreover, the impression that the current API is confusing doesn't have much evidence other than the single data point.

@johnwlambert
Copy link
Contributor

johnwlambert commented Jun 9, 2022

Just as a heads up, we also rely on the prior functionality in GTSFM. Although this change might make the coefficients more explicit in our particular use case, instead of having to go back and check the documentation for ordering. See: https://github.com/borglab/gtsfm/blob/master/gtsfm/utils/io.py#L374-L377

@HViktorTsoi
Copy link
Contributor Author

HViktorTsoi commented Jun 9, 2022

I'm sorry but I am not seeing the argument for this. I just updated some code to use toQuaternion and it is less readable.

Previously I could do

q = wTc.rotation().quaternion()

but now we have to do

q = wTb.rotation().toQuaternion()
q = np.asarray([q.w(), q.x(), q.y(), q.z()])

which isn't functional since we can't chain methods together anymore e.g.:

do_something(wTb.rotation().quaternion())

Moreover, the impression that the current API is confusing doesn't have much evidence other than the single data point.

As you suggest, for the python/matlab users, the quaternion API may be handy in certain circumstances, for it allows the users to use it as an np array;

However, things are different in C++, because the quaternion() actually returns an Eigen Vector with the similar getters w() x() y() z() as a quaternion, but the definition is totally different from qw,qx,qy,qz, and that is confusing.

Suppose the following use case: the users want to extract the pose estimation from gtsam and feed it to a ROS odometry message. Without checking the documentation, their IDE gives a coding completion that says: there is a quaternion method with w(),x(),y(),z() getters in the Rot3 object and it allows them to access the rotation data. If they don't know the difference between quaternion() and toQuaternion(), or they don't know how a quaternion is stored inside the object, it is VERY likely to write the following buggy code:

// pose estimation from previous step
gtsam::Point3 wtc;
gtsam::Rot3 wRc;

nav_msgs::Odometry odom;
// wrong quaternion component order!
odom.pose.orientation.w = wRc.quaternion().w();
odom.pose.orientation.x = wRc.quaternion().x();
odom.pose.orientation.y = wRc.quaternion().y();
odom.pose.orientation.z = wRc.quaternion().z();

Everything seems correct, but the ordering of the quaternion doesn't match. And for the debugging process, it will be a nightmare...

@varunagrawal
Copy link
Collaborator

@HViktorTsoi I see your argument, but the docstring for Rot3::quaternion states that it returns an Eigen matrix object.

/**
 * Converts to a generic matrix to allow for use with matlab
 * In format: w x y z
 */

Moreover, a user should be familiar that a Matrix/Vector is not semantically the same as a quaternion since a vector doesn't enforce the structure of a quaternion. Thus they should already be wary of calling .w() on a vector object and double check the documentation.

On the other hand, toQuaternion returns a gtsam::Quaternion object so this follows the semantics of a quaternion, and thus .w(), .x() etc would work as expected.

@varunagrawal
Copy link
Collaborator

Without checking the documentation, their IDE gives a coding completion that says: there is a quaternion method with w(),x(),y(),z() getters in the Rot3 object

This is the gist of the issue here. The object is not a Rot3 but an Eigen vector so one cannot assume the getters for the vector are the same as the getters for a Rot3/Quaternion object.

@HViktorTsoi
Copy link
Contributor Author

HViktorTsoi commented Jun 9, 2022

I'm sorry but I am not seeing the argument for this. I just updated some code to use toQuaternion and it is less readable.

Previously I could do

q = wTc.rotation().quaternion()

but now we have to do

q = wTb.rotation().toQuaternion()
q = np.asarray([q.w(), q.x(), q.y(), q.z()])

which isn't functional since we can't chain methods together anymore e.g.:

do_something(wTb.rotation().quaternion())

Moreover, the impression that the current API is confusing doesn't have much evidence other than the single data point.

Maybe one can still chain methods without quaternion()? If they need a quaternion with thewxzy order, it can be written with toQuaternion().coeffs() like this:

do_something(wTb.rotation().toQuaternion().coeffs()[[3,0,1,2]])

It looks a little bit tricky though.

@varunagrawal
Copy link
Collaborator

Yeah I thought about that but because Eigen uses [x, y, z, w], that is also error prone, exactly like the original issue you faced.

I am terribly sorry you are having difficulties with debugging and other issues, but hopefully we can figure something out that works for everyone!

@dellaert
Copy link
Member

dellaert commented Jun 9, 2022

The previous behavior is definitely problematic. Eigen (for better or worse) provides x/y/z/w on vectors, and they conflict with the order in the quaternion method. Just bad. I think the solution in this PR is good. @johnwlambert , let's just fix GTSFM to not use the deprecated method.

@dellaert
Copy link
Member

dellaert commented Jun 9, 2022

@varunagrawal are we ok to merge?

@varunagrawal
Copy link
Collaborator

I'm not so sure. @HViktorTsoi has done a brilliant job with this PR but @johnwlambert, myself and others would like to see some way to just get the quaternion vector directly without the 2 step process. :(

@varunagrawal
Copy link
Collaborator

The previous behavior is definitely problematic. Eigen (for better or worse) provides x/y/z/w on vectors, and they conflict with the order in the quaternion method. Just bad. I think the solution in this PR is good. @johnwlambert , let's just fix GTSFM to not use the deprecated method.

Okay if you feel this is the correct approach then let's go for it. We should probably decide on a fixed convention and make that the de facto (aka update constructors and other methods).

Thanks for the brilliant PR @HViktorTsoi. You've made our lives so much easier! 😊

@varunagrawal varunagrawal merged commit 08aed0b into borglab:develop Jun 10, 2022
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

Rot3 Quaternion BUG
4 participants