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

Rot3 Quaternion BUG #1209

Closed
HViktorTsoi opened this issue Jun 1, 2022 · 1 comment · Fixed by #1219
Closed

Rot3 Quaternion BUG #1209

HViktorTsoi opened this issue Jun 1, 2022 · 1 comment · Fixed by #1219

Comments

@HViktorTsoi
Copy link
Contributor

HViktorTsoi commented Jun 1, 2022

Description

There is an issue when using Rot3().quaternion().x(), Rot3().quaternion().y(), Rot3().quaternion().z(), or Rot3().quaternion().w() to access the components of a quaternion. For example, when using Rot3().quaternion().w() to access qw, it actually returns qz.

Steps to reproduce

Using the flowing code to reproduce the bug:

double qw=0.782240, qx=0.589819, qy=0.199939, qz=0.000000;
std::printf("Correct quaternion:\n");
std::printf(
        "qw: %f  qx: %f  qy: %f  qz: %f\n",
        qw, qx, qy, qz
);

std::printf("\nIn GTSAM Rot3:\n");
gtsam::Rot3 a_rotation(qw, qx, qy, qz);
std::printf(
        "qw: %f  qx: %f  qy: %f  qz: %f\n",
        a_rotation.quaternion().w(),
        a_rotation.quaternion().x(),
        a_rotation.quaternion().y(),
        a_rotation.quaternion().z()
);
std::printf(
        "q[0]: %f  q[1]: %f  q[2]: %f  q[3]: %f\n",
        a_rotation.quaternion()[0],
        a_rotation.quaternion()[1],
        a_rotation.quaternion()[2],
        a_rotation.quaternion()[3]
);
std::printf("\n\n\n\n");

Expected behavior

Screenshot from 2022-06-01 22-05-55

From the first 2 lines of the result above we can see that, the correct components of the quaternion should be
qw: 0.782240 qx: 0.589819 qy: 0.199939 qz: 0.000000

However, when I construct a Rot3() instance with the quaternion constructor and access the components through the getters such as quaternion().x(), it returns the wrong component(it returns quaternion().w()).

Though using the index I can get the component in the right order(see the last line of the result), where quaternion()[0],[1],[2],[3] corresponding to qw, qx, qy, and qz.

Additionally, after checking the source code, I'm sure the quaternion passed to the Rot3() constructor is correct, as shown below:

Rot3(double w, double x, double y, double z) : Rot3(Quaternion(w, x, y, z)) {}

Environment

  1. GTSAM 4.0.2
  2. Ubuntu 18.04
  3. GCC & G++ 7.5.0

Additional information

@HViktorTsoi
Copy link
Contributor Author

HViktorTsoi commented Jun 7, 2022

Update:
Rot3().toQuaternion() will return correct component.

double qw=0.782240, qx=0.589819, qy=0.199939, qz=0.000000;
std::printf("Correct quaternion:\n");
std::printf(
        "qw: %f  qx: %f  qy: %f  qz: %f\n",
        qw, qx, qy, qz
);

std::printf("\nIn GTSAM Rot3:\n");
gtsam::Rot3 a_rotation(qw, qx, qy, qz);
std::printf(
        "qw: %f  qx: %f  qy: %f  qz: %f\n",
        a_rotation.quaternion().w(),
        a_rotation.quaternion().x(),
        a_rotation.quaternion().y(),
        a_rotation.quaternion().z()
);
std::printf(
        "to_qw: %f  to_qx: %f  to_qy: %f  to_qz: %f\n",
        a_rotation.toQuaternion().w(),
        a_rotation.toQuaternion().x(),
        a_rotation.toQuaternion().y(),
        a_rotation.toQuaternion().z()
);
std::printf(
        "q[0]: %f  q[1]: %f  q[2]: %f  q[3]: %f\n",
        a_rotation.quaternion()[0],
        a_rotation.quaternion()[1],
        a_rotation.quaternion()[2],
        a_rotation.quaternion()[3]
);
std::printf("\n\n\n\n");

Screenshot from 2022-06-07 22-32-51

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 a pull request may close this issue.

1 participant