diff --git a/gtsam/base/testLie.h b/gtsam/base/testLie.h index d41e3bbd9..1906ec439 100644 --- a/gtsam/base/testLie.h +++ b/gtsam/base/testLie.h @@ -54,6 +54,7 @@ void testLieGroupDerivatives(TestResult& result_, const std::string& name_, EXPECT(assert_equal(numericalDerivative41(T::Between, t1, t2, none, none), H1)); EXPECT(assert_equal(numericalDerivative42(T::Between, t1, t2, none, none), H2)); } + // Do a comprehensive test of Lie Group Chart derivatives template void testChartDerivatives(TestResult& result_, const std::string& name_, @@ -61,7 +62,7 @@ void testChartDerivatives(TestResult& result_, const std::string& name_, Matrix H1, H2; typedef traits T; - typedef typename G::TangentVector V; + typedef typename T::TangentVector V; typedef OptionalJacobian OJ; // Retract @@ -72,7 +73,7 @@ void testChartDerivatives(TestResult& result_, const std::string& name_, EXPECT(assert_equal(numericalDerivative42(T::Retract, t1, w12, none, none), H2)); // Local - EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2))); + EXPECT(assert_equal(w12, T::Local(t1, t2, H1, H2))); EXPECT(assert_equal(numericalDerivative41(T::Local, t1, t2, none, none), H1)); EXPECT(assert_equal(numericalDerivative42(T::Local, t1, t2, none, none), H2)); } diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 936a435ba..fd6219f00 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -40,19 +40,6 @@ struct traits { return Q::Identity(); } - static Q Compose(const Q &g, const Q & h) { - return g * h; - } - - static Q Between(const Q &g, const Q & h) { - Q d = g.inverse() * h; - return d; - } - - static Q Inverse(const Q &g) { - return g.inverse(); - } - /// @} /// @name Basic manifold traits /// @{ @@ -65,28 +52,29 @@ struct traits { /// @} /// @name Lie group traits /// @{ - static Q Compose(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh = - boost::none) { + static Q Compose(const Q &g, const Q & h, + ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) { if (Hg) - *Hg = h.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart ( h.toRotationMatrix().transpose() ? ) + *Hg = h.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart ( h.toRotationMatrix().transpose() ? ) if (Hh) - *Hh = I_3x3; // TODO : check Jacobian consistent with chart ( I(3)? ) + *Hh = I_3x3;// TODO : check Jacobian consistent with chart ( I(3)? ) return g * h; } - static Q Between(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh = - boost::none) { + static Q Between(const Q &g, const Q & h, + ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) { Q d = g.inverse() * h; if (Hg) - *Hg = -d.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart + *Hg = -d.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart if (Hh) - *Hh = I_3x3; // TODO : check Jacobian consistent with chart (my guess I(3) ) + *Hh = I_3x3;// TODO : check Jacobian consistent with chart (my guess I(3) ) return d; } - static Q Inverse(const Q &g, ChartJacobian H) { + static Q Inverse(const Q &g, + ChartJacobian H = boost::none) { if (H) - *H = -g.toRotationMatrix(); // TODO : check Jacobian consistent with chart + *H = -g.toRotationMatrix(); // TODO : check Jacobian consistent with chart return g.inverse(); } @@ -94,7 +82,7 @@ struct traits { static Q Expmap(const Eigen::Ref& omega, ChartJacobian H = boost::none) { if (omega.isZero()) - return Q::Identity(); + return Q::Identity(); else { _Scalar angle = omega.norm(); return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle)); @@ -109,7 +97,7 @@ struct traits { // define these compile time constants to avoid std::abs: static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10, - NearlyNegativeOne = -1.0 + 1e-10; + NearlyNegativeOne = -1.0 + 1e-10; Vector3 omega; @@ -127,9 +115,9 @@ struct traits { double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); // Important: convert to [-pi,pi] to keep error continuous if (angle > M_PI) - angle -= twoPi; + angle -= twoPi; else if (angle < -M_PI) - angle += twoPi; + angle += twoPi; omega = (angle / s) * q.vec(); } @@ -156,9 +144,9 @@ struct traits { /// @{ static void Print(const Q& q, const std::string& str = "") { if (str.size() == 0) - std::cout << "Eigen::Quaternion: "; + std::cout << "Eigen::Quaternion: "; else - std::cout << str << " "; + std::cout << str << " "; std::cout << q.vec().transpose() << std::endl; } static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) { diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 7302754d7..e6a49345d 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -17,6 +17,8 @@ #include #include +#include + #include using namespace std; @@ -37,14 +39,6 @@ TEST(Quaternion , Constructor) { Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } -//****************************************************************************** -TEST(Quaternion , Invariants) { - Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); - Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); - check_group_invariants(q1, q2); - check_manifold_invariants(q1, q2); -} - //****************************************************************************** TEST(Quaternion , Local) { Vector3 z_axis(0, 0, 1); @@ -74,47 +68,62 @@ TEST(Quaternion , Compose) { Q q2(Eigen::AngleAxisd(0.1, z_axis)); Q expected = q1 * q2; - Matrix actualH1, actualH2; - Q actual = traits::Compose(q1, q2, actualH1, actualH2); + Q actual = traits::Compose(q1, q2); EXPECT(traits::Equals(expected,actual)); - - Matrix numericalH1 = numericalDerivative21(traits::Compose, q1, q2); - EXPECT(assert_equal(numericalH1,actualH1)); - - Matrix numericalH2 = numericalDerivative22(traits::Compose, q1, q2); - EXPECT(assert_equal(numericalH2,actualH2)); } +//****************************************************************************** +Q id; +Vector3 z_axis(0, 0, 1); +Q R1(Eigen::AngleAxisd(1, z_axis)); +Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); + //****************************************************************************** TEST(Quaternion , Between) { - Vector3 z_axis(0, 0, 1); Q q1(Eigen::AngleAxisd(0.2, z_axis)); Q q2(Eigen::AngleAxisd(0.1, z_axis)); Q expected = q1.inverse() * q2; - Matrix actualH1, actualH2; - Q actual = traits::Between(q1, q2, actualH1, actualH2); + Q actual = traits::Between(q1, q2); EXPECT(traits::Equals(expected,actual)); - - Matrix numericalH1 = numericalDerivative21(traits::Between, q1, q2); - EXPECT(assert_equal(numericalH1,actualH1)); - - Matrix numericalH2 = numericalDerivative22(traits::Between, q1, q2); - EXPECT(assert_equal(numericalH2,actualH2)); } //****************************************************************************** TEST(Quaternion , Inverse) { - Vector3 z_axis(0, 0, 1); Q q1(Eigen::AngleAxisd(0.1, z_axis)); Q expected(Eigen::AngleAxisd(-0.1, z_axis)); - Matrix actualH; - Q actual = traits::Inverse(q1, actualH); + Q actual = traits::Inverse(q1); EXPECT(traits::Equals(expected,actual)); +} - Matrix numericalH = numericalDerivative11(traits::Inverse, q1); - EXPECT(assert_equal(numericalH,actualH)); +//****************************************************************************** +TEST(Quaternion , Invariants) { + check_group_invariants(id,id); + check_group_invariants(id,R1); + check_group_invariants(R2,id); + check_group_invariants(R2,R1); + + check_manifold_invariants(id,id); + check_manifold_invariants(id,R1); + check_manifold_invariants(R2,id); + check_manifold_invariants(R2,R1); +} + +//****************************************************************************** +TEST(Quaternion , LieGroupDerivatives) { + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,R2); + CHECK_LIE_GROUP_DERIVATIVES(R2,id); + CHECK_LIE_GROUP_DERIVATIVES(R2,R1); +} + +//****************************************************************************** +TEST(Quaternion , ChartDerivatives) { + CHECK_CHART_DERIVATIVES(id,id); + CHECK_CHART_DERIVATIVES(id,R2); + CHECK_CHART_DERIVATIVES(R2,id); + CHECK_CHART_DERIVATIVES(R2,R1); } //******************************************************************************