diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 5270de9c8..145ebed39 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -102,6 +102,9 @@ typedef Eigen::Quaterniond Quaternion; static Rot3 roll (double t) { return Rx(t);} // positive roll is to right (increasing yaw in aircraft) static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);} + /** Create from Quaternion parameters */ + static Rot3 quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3(q); } + /** * Rodriguez' formula to compute an incremental rotation matrix * @param w is the rotation axis, unit length diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index ec7a60f31..278745d3d 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -448,7 +448,9 @@ TEST(Rot3, quaternion) { // Check creating Rot3 from quaternion EXPECT(assert_equal(R1, Rot3(q1))); + EXPECT(assert_equal(R1, Rot3::quaternion(q1.w(), q1.x(), q1.y(), q1.z()))); EXPECT(assert_equal(R2, Rot3(q2))); + EXPECT(assert_equal(R2, Rot3::quaternion(q2.w(), q2.x(), q2.y(), q2.z()))); // Check converting Rot3 to quaterion EXPECT(assert_equal(Vector(R1.toQuaternion().coeffs()), Vector(q1.coeffs())));