diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 34b86cd80..0a3b4fd9f 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -33,6 +33,16 @@ void Rot3::print(const std::string& s) const { gtsam::print((Matrix)matrix(), s); } +/* ************************************************************************* */ +Rot3 Rot3::rodriguez(const Point3& w, double theta) { + return rodriguez((Vector)w.vector(),theta); +} + +/* ************************************************************************* */ +Rot3 Rot3::rodriguez(const Sphere2& w, double theta) { + return rodriguez(w.point3(),theta); +} + /* ************************************************************************* */ Rot3 Rot3::rodriguez(const Vector& w) { double t = w.norm(); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index b86a3e899..5bb382a3e 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -150,7 +150,10 @@ namespace gtsam { static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);} /** Create from Quaternion coefficients */ - static Rot3 quaternion(double w, double x, double y, double z) { Quaternion q(w, x, y, z); return Rot3(q); } + 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 @@ -160,6 +163,22 @@ namespace gtsam { */ static Rot3 rodriguez(const Vector& w, double theta); + /** + * Rodriguez' formula to compute an incremental rotation matrix + * @param w is the rotation axis, unit length + * @param theta rotation angle + * @return incremental rotation matrix + */ + static Rot3 rodriguez(const Point3& w, double theta); + + /** + * Rodriguez' formula to compute an incremental rotation matrix + * @param w is the rotation axis + * @param theta rotation angle + * @return incremental rotation matrix + */ + static Rot3 rodriguez(const Sphere2& w, double theta); + /** * Rodriguez' formula to compute an incremental rotation matrix * @param v a vector of incremental roll,pitch,yaw