diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp index 65ca9e067..d7d43b33f 100644 --- a/gtsam/geometry/tests/testRot3Q.cpp +++ b/gtsam/geometry/tests/testRot3Q.cpp @@ -15,20 +15,65 @@ * @author Alireza Fathi */ -#include #include +#include #include #include -#include - -#include #include +using namespace std; +using namespace gtsam; + #ifdef GTSAM_USE_QUATERNIONS -// No quaternion only tests +//****************************************************************************** +TEST(Rot3Q , Compare) { + using boost::none; + + // We set up expected values with quaternions + typedef Quaternion Q; + typedef traits TQ; + typedef TQ::ChartJacobian OJ; + + // We check Rot3 expressions + typedef Rot3 R; + typedef traits TR; + + // Define test values + Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); + Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); + R R1(q1), R2(q2); + + // Check Compose + Q q3 = TQ::Compose(q1, q2, none, none); + R R3 = TR::Compose(R1, R2, none, none); + EXPECT(assert_equal(R(q3), R3)); + + // Check Retract/Local + Vector3 v(1e-5, 0, 0); + Vector3 vQ = TQ::Local(q3, TQ::Retract(q3, v)); + Vector3 vR = TR::Local(R3, TR::Retract(R3, v)); + EXPECT(assert_equal(vQ, vR)); + + // Check Retract/Local of Compose + Vector3 vQ1 = TQ::Local(q3, TQ::Compose(q1, TQ::Retract(q2, v))); + Vector3 vR1 = TR::Local(R3, TR::Compose(R1, TR::Retract(R2, v))); + EXPECT(assert_equal(vQ1, vR1)); + Vector3 vQ2 = TQ::Local(q3, TQ::Compose(q1, TQ::Retract(q2, -v))); + Vector3 vR2 = TR::Local(R3, TR::Compose(R1, TR::Retract(R2, -v))); + EXPECT(assert_equal(vQ2, vR2)); + EXPECT(assert_equal((vQ1 - vQ2) / 0.2, (vR1 - vR2) / 0.2)); + cout << (vR1 - vR2) / 0.2 << endl; + + // Check Compose Derivatives + Matrix HQ, HR; + HQ = numericalDerivative42(TQ::Compose, q1, q2, none, none); + HR = numericalDerivative42(TR::Compose, R1, R2, none, none); + EXPECT(assert_equal(HQ, HR)); + +} #endif