compute angle in degrees between 2 Rot3 matrices

gtsam.h

@@ -677,6 +677,7 @@ class Rot3 {
 //  Vector toQuaternion() const;  // FIXME: Can't cast to Vector properly
   Vector quaternion() const;
   gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const;
+  double angle(const gtsam::Rot3& other) const;
 
   // enabling serialization functionality
   void serialize() const;

gtsam/geometry/Rot3.cpp

@@ -228,6 +228,11 @@ Rot3 Rot3::slerp(double t, const Rot3& other) const {
   return interpolate(*this, other, t);
 }
 
+/* ************************************************************************* */
+double Rot3::angle(const Rot3& other) const {
+  return Rot3::Logmap(this->between(other)).norm() * 180.0 / M_PI;
+}
+
 /* ************************************************************************* */
 
 } // namespace gtsam

gtsam/geometry/Rot3.h

@@ -479,6 +479,12 @@ namespace gtsam {
      */
     Rot3 slerp(double t, const Rot3& other) const;
 
+    /**
+     * @brief Compute the angle (in degrees) between *this and other
+     * @param other Rot3 element
+     */
+    double angle(const Rot3& other) const;
+
     /// Output stream operator
     GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
 

gtsam/geometry/tests/testRot3.cpp

@@ -661,6 +661,21 @@ TEST(Rot3, ClosestTo) {
   EXPECT(assert_equal(expected, actual.matrix(), 1e-6));
 }
 
+/* ************************************************************************* */
+TEST(Rot3, angle) {
+  Rot3 R1(0.996136,  -0.0564186, -0.0672982, //
+          0.0419472,  0.978941,  -0.199787, //
+          0.0771527,  0.196192,   0.977525);
+  
+  Rot3 R2(0.99755,  -0.0377748, -0.0588831, //
+          0.0238455, 0.974883,  -0.221437, //
+          0.0657689, 0.21949,    0.973395);
+
+  double expected = 1.7765686464446904;
+
+  EXPECT(assert_equal(expected, R1.angle(R2), 1e-6));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;