diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 93810d436..d35fa52f4 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -87,6 +87,9 @@ namespace gtsam { /** constructor from a rotation matrix */ Rot3(const Matrix3& R); + /** constructor from a rotation matrix */ + Rot3(const Matrix& R); + /** Constructor from a quaternion. This can also be called using a plain * Vector, due to implicit conversion from Vector to Quaternion * @param q The quaternion diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 9fffb0c2e..b4c79de3b 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -55,6 +55,13 @@ Rot3::Rot3(const Matrix3& R) { rot_ = R; } +/* ************************************************************************* */ +Rot3::Rot3(const Matrix& R) { + if (R.rows()!=3 || R.cols()!=3) + throw invalid_argument("Rot3 constructor expects 3*3 matrix"); + rot_ = R; +} + /* ************************************************************************* */ Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) { } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 81ea77fb7..52fb4f262 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -52,6 +52,10 @@ namespace gtsam { Rot3::Rot3(const Matrix3& R) : quaternion_(R) {} + /* ************************************************************************* */ + Rot3::Rot3(const Matrix& R) : + quaternion_(Matrix3(R)) {} + /* ************************************************************************* */ Rot3::Rot3(const Quaternion& q) : quaternion_(q) {