From 7083de35a4aea261ab5a2e16585a94307b890c23 Mon Sep 17 00:00:00 2001
From: John Lambert <johnlambert@gatech.edu>
Date: Thu, 7 Oct 2021 15:54:56 -0400
Subject: [PATCH] add failing unit test on axisAngle for Rot3 in c++

---
 gtsam/geometry/tests/testRot3.cpp | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp
index 34f90c8cc..5f0187ed9 100644
--- a/gtsam/geometry/tests/testRot3.cpp
+++ b/gtsam/geometry/tests/testRot3.cpp
@@ -122,6 +122,21 @@ TEST( Rot3, AxisAngle)
   CHECK(assert_equal(expected,actual3,1e-5));
 }
 
+/* ************************************************************************* */
+TEST( Rot3, AxisAngle2)
+{
+  // constructor from a rotation matrix, as doubles in *row-major* order.
+  Rot3 R1(-0.999957, 0.00922903, 0.00203116, 0.00926964, 0.999739, 0.0208927, -0.0018374, 0.0209105, -0.999781);
+  
+  Unit3 actualAxis;
+  double actualAngle;
+  // convert Rot3 to quaternion using GTSAM
+  std::tie(actualAxis, actualAngle) = R1.axisAngle();
+  
+  double expectedAngle = 3.1396582;
+  CHECK(assert_equal(expectedAngle, actualAngle, 1e-7));
+}
+
 /* ************************************************************************* */
 TEST( Rot3, Rodrigues)
 {