From 4b538d9648139066c3332c343c5aeae219e207aa Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 3 Jan 2014 17:45:45 -0500 Subject: [PATCH] Conjugation --- gtsam/geometry/Rot3.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 980d35de1..ea59fab17 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -225,6 +225,16 @@ namespace gtsam { /** compose two rotations */ Rot3 operator*(const Rot3& R2) const; + /** + * Conjugation: given a rotation acting in frame B, compute rotation c1Rc2 acting in a frame C + * @param cRb rotation from B frame to C frame + * @return c1Rc2 = cRb * b1Rb2 * cRb' + */ + Rot3 conjugate(const Rot3& cRb) const { + // TODO: do more efficiently by using Eigen or quaternion properties + return cRb * (*this) * cRb.inverse(); + } + /** * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' */