diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 33d120a85..ae69fc57f 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -154,12 +154,23 @@ namespace gtsam { static Rot3 Rz(double t); /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. - static Rot3 RzRyRx(double x, double y, double z); + static Rot3 RzRyRx(double x, double y, double z, + OptionalJacobian<3, 1> Hx = boost::none, + OptionalJacobian<3, 1> Hy = boost::none, + OptionalJacobian<3, 1> Hz = boost::none); /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. - inline static Rot3 RzRyRx(const Vector& xyz) { + inline static Rot3 RzRyRx(const Vector& xyz, + OptionalJacobian<3, 3> H = boost::none) { assert(xyz.size() == 3); - return RzRyRx(xyz(0), xyz(1), xyz(2)); + Rot3 out; + if (H) { + Vector3 Hx, Hy, Hz; + out = RzRyRx(xyz(0), xyz(1), xyz(2), Hx, Hy, Hz); + (*H) << Hx, Hy, Hz; + } else + out = RzRyRx(xyz(0), xyz(1), xyz(2)); + return out; } /// Positive yaw is to right (as in aircraft heading). See ypr @@ -185,7 +196,12 @@ namespace gtsam { * Positive pitch is down (decreasing aircraft altitude). * Positive roll is to right (decreasing yaw in aircraft). */ - static Rot3 Ypr(double y, double p, double r) { return RzRyRx(r,p,y);} + static Rot3 Ypr(double y, double p, double r, + OptionalJacobian<3, 1> Hy = boost::none, + OptionalJacobian<3, 1> Hp = boost::none, + OptionalJacobian<3, 1> Hr = boost::none) { + return RzRyRx(r, p, y, Hr, Hp, Hy); + } /** Create from Quaternion coefficients */ static Rot3 Quaternion(double w, double x, double y, double z) { diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 46a07e50a..500941a16 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -82,7 +82,8 @@ Rot3 Rot3::Rz(double t) { /* ************************************************************************* */ // Considerably faster than composing matrices above ! -Rot3 Rot3::RzRyRx(double x, double y, double z) { +Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx, + OptionalJacobian<3, 1> Hy, OptionalJacobian<3, 1> Hz) { double cx=cos(x),sx=sin(x); double cy=cos(y),sy=sin(y); double cz=cos(z),sz=sin(z); @@ -97,6 +98,9 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) { double s_c = sx * cz; double c_c = cx * cz; double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; + if (Hx) (*Hx) << 1, 0, 0; + if (Hy) (*Hy) << 0, cx, -sx; + if (Hz) (*Hz) << -sy, sc_, cc_; return Rot3( _cc,- c_s + ssc, s_s + csc, _cs, c_c + sss, -s_c + css, diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index d609c289c..6e1871c64 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -67,10 +67,23 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3( - gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) * - gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) * - gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); + Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx, + OptionalJacobian<3, 1> Hy, OptionalJacobian<3, 1> Hz) { + if (Hx) (*Hx) << 1, 0, 0; + + if (Hy or Hz) { + const auto cx = cos(x), sx = sin(x); + if (Hy) (*Hy) << 0, cx, -sx; + if (Hz) { + const auto cy = cos(y), sy = sin(y); + (*Hz) << -sy, sx * cy, cx * cy; + } + } + + return Rot3( + gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) * + gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) * + gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index d5400494e..fba84c724 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -743,6 +743,57 @@ TEST(Rot3, axisAngle) { CHECK_AXIS_ANGLE(_axis, theta165, Rot3::AxisAngle(axis, theta195)) } +/* ************************************************************************* */ +Rot3 RzRyRx_proxy(double const& a, double const& b, double const& c) { + return Rot3::RzRyRx(a, b, c); +} + +TEST(Rot3, RzRyRx_scalars_derivative) { + const auto x = 0.1, y = 0.4, z = 0.2; + const auto num_x = numericalDerivative31(RzRyRx_proxy, x, y, z); + const auto num_y = numericalDerivative32(RzRyRx_proxy, x, y, z); + const auto num_z = numericalDerivative33(RzRyRx_proxy, x, y, z); + + Vector3 act_x, act_y, act_z; + Rot3::RzRyRx(x, y, z, act_x, act_y, act_z); + + CHECK(assert_equal(num_x, act_x)); + CHECK(assert_equal(num_y, act_y)); + CHECK(assert_equal(num_z, act_z)); +} + +/* ************************************************************************* */ +Rot3 RzRyRx_proxy(Vector3 const& xyz) { return Rot3::RzRyRx(xyz); } + +TEST(Rot3, RzRyRx_vector_derivative) { + const auto xyz = Vector3{-0.3, 0.1, 0.7}; + const auto num = numericalDerivative11(RzRyRx_proxy, xyz); + + Matrix3 act; + Rot3::RzRyRx(xyz, act); + + CHECK(assert_equal(num, act)); +} + +/* ************************************************************************* */ +Rot3 Ypr_proxy(double const& y, double const& p, double const& r) { + return Rot3::Ypr(y, p, r); +} + +TEST(Rot3, Ypr_derivative) { + const auto y = 0.7, p = -0.3, r = 0.1; + const auto num_y = numericalDerivative31(Ypr_proxy, y, p, r); + const auto num_p = numericalDerivative32(Ypr_proxy, y, p, r); + const auto num_r = numericalDerivative33(Ypr_proxy, y, p, r); + + Vector3 act_y, act_p, act_r; + Rot3::Ypr(y, p, r, act_y, act_p, act_r); + + CHECK(assert_equal(num_y, act_y)); + CHECK(assert_equal(num_p, act_p)); + CHECK(assert_equal(num_r, act_r)); +} + /* ************************************************************************* */ int main() { TestResult tr;