From 63327a7c9d6b86880e3ed63ded8295fa12bb7a4c Mon Sep 17 00:00:00 2001 From: Christian Berg Date: Mon, 31 Aug 2020 18:49:18 +0200 Subject: [PATCH] Also add jacobians to Rot3::RzRyRx for when quaternions are selected --- gtsam/geometry/Rot3Q.cpp | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) 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()))); } /* ************************************************************************* */