diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index fa09ddc21..926590ee1 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -33,30 +33,13 @@ 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 Unit3& w, double theta) { - return rodriguez(w.point3(),theta); -} - /* ************************************************************************* */ Rot3 Rot3::Random(boost::mt19937 & rng) { // TODO allow any engine without including all of boost :-( Unit3 w = Unit3::Random(rng); boost::uniform_real randomAngle(-M_PI,M_PI); double angle = randomAngle(rng); - return rodriguez(w,angle); -} - -/* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Vector3& w) { - double t = w.norm(); - if (t < 1e-10) return Rot3(); - return rodriguez(w/t, t); + return Rodrigues(w,angle); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 881c58579..d8a5cdb07 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -22,8 +22,9 @@ #pragma once -#include #include +#include +#include #include #include // Get GTSAM_USE_QUATERNIONS macro @@ -149,45 +150,58 @@ namespace gtsam { } /** - * Rodriguez' formula to compute an incremental rotation matrix + * Rodrigues' formula to compute an incremental rotation matrix * @param w is the rotation axis, unit length - * @param theta rotation angle + * @param angle rotation angle * @return incremental rotation matrix */ - static Rot3 rodriguez(const Vector3& w, double theta); + static Rot3 Rodrigues(const Vector3& axis, double angle) { +#ifdef GTSAM_USE_QUATERNIONS + return Quaternion(Eigen::AngleAxis(angle, axis)); +#else + return SO3::Rodrigues(axis,angle); +#endif + } /** - * Rodriguez' formula to compute an incremental rotation matrix + * Rodrigues' formula to compute an incremental rotation matrix * @param w is the rotation axis, unit length - * @param theta rotation angle + * @param angle rotation angle * @return incremental rotation matrix */ - static Rot3 rodriguez(const Point3& w, double theta); + static Rot3 Rodrigues(const Point3& axis, double angle) { + return Rodrigues(axis.vector(),angle); + } /** - * Rodriguez' formula to compute an incremental rotation matrix + * Rodrigues' formula to compute an incremental rotation matrix * @param w is the rotation axis - * @param theta rotation angle + * @param angle rotation angle * @return incremental rotation matrix */ - static Rot3 rodriguez(const Unit3& w, double theta); + static Rot3 Rodrigues(const Unit3& axis, double angle) { + return Rodrigues(axis.unitVector(),angle); + } /** - * Rodriguez' formula to compute an incremental rotation matrix - * @param v a vector of incremental roll,pitch,yaw + * Rodrigues' formula to compute an incremental rotation matrix + * @param w a vector of incremental roll,pitch,yaw * @return incremental rotation matrix */ - static Rot3 rodriguez(const Vector3& v); + static Rot3 Rodrigues(const Vector3& w) { + return Rot3::Expmap(w); + } /** - * Rodriguez' formula to compute an incremental rotation matrix + * Rodrigues' formula to compute an incremental rotation matrix * @param wx Incremental roll (about X) * @param wy Incremental pitch (about Y) * @param wz Incremental yaw (about Z) * @return incremental rotation matrix */ - static Rot3 rodriguez(double wx, double wy, double wz) - { return rodriguez(Vector3(wx, wy, wz));} + static Rot3 Rodrigues(double wx, double wy, double wz) { + return Rodrigues(Vector3(wx, wy, wz)); + } /// @} /// @name Testable @@ -272,11 +286,15 @@ namespace gtsam { /** * Exponential map at identity - create a rotation from canonical coordinates - * \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula + * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula */ static Rot3 Expmap(const Vector& v, OptionalJacobian<3,3> H = boost::none) { if(H) *H = Rot3::ExpmapDerivative(v); - if (zero(v)) return Rot3(); else return rodriguez(v); +#ifdef GTSAM_USE_QUATERNIONS + return traits::Expmap(v); +#else + return traits::Expmap(v); +#endif } /** @@ -422,6 +440,14 @@ namespace gtsam { GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p); /// @} + /// @name Deprecated + /// @{ + static Rot3 rodriguez(const Vector3& axis, double angle) { return Rodrigues(axis, angle); } + static Rot3 rodriguez(const Point3& axis, double angle) { return Rodrigues(axis, angle); } + static Rot3 rodriguez(const Unit3& axis, double angle) { return Rodrigues(axis, angle); } + static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); } + static Rot3 rodriguez(double wx, double wy, double wz) { return Rodrigues(wx, wy, wz); } + /// @} private: diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index b4c79de3b..18628aec3 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -117,11 +117,6 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) { ); } -/* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Vector3& w, double theta) { - return SO3::Rodrigues(w,theta); -} - /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { return Rot3(Matrix3(rot_*R2.rot_)); diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 52fb4f262..c97e76718 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -83,10 +83,6 @@ namespace gtsam { Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); } - /* ************************************************************************* */ - Rot3 Rot3::rodriguez(const Vector3& w, double theta) { - return Quaternion(Eigen::AngleAxis(theta, w)); - } /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { return Rot3(quaternion_ * R2.quaternion_);