diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index ad25455ee..002894acd 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -513,7 +513,7 @@ public: /* ************************************************************************* */ // Adapt ceres-style autodiff template -struct AutoDiff { +class AdaptAutoDiff { static const int N = dimension::value; static const int M1 = dimension::value; @@ -522,16 +522,27 @@ struct AutoDiff { typedef Canonical CanonicalT; typedef Canonical Canonical1; typedef Canonical Canonical2; + typedef typename CanonicalT::vector VectorT; typedef typename Canonical1::vector Vector1; typedef typename Canonical2::vector Vector2; + // Instantiate function and charts + CanonicalT chartT; + Canonical1 chart1; + Canonical2 chart2; + F f; + +public: + typedef Eigen::Matrix JacobianTA1; typedef Eigen::Matrix JacobianTA2; T operator()(const A1& a1, const A2& a2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { + using ceres::internal::AutoDiff; + // Make arguments Vector1 v1 = chart1.vee(a1); Vector2 v2 = chart2.vee(a2); @@ -540,13 +551,11 @@ struct AutoDiff { VectorT result; if (H1 || H2) { - // Get derivatives with AutoDiff double *parameters[] = { v1.data(), v2.data() }; double *jacobians[] = { H1->data(), H2->data() }; - success = ceres::internal::AutoDiff::Differentiate(f, - parameters, 2, result.data(), jacobians); - + success = AutoDiff::Differentiate(f, parameters, 2, + result.data(), jacobians); } else { // Apply the mapping, to get result success = f(v1.data(), v2.data(), result.data()); @@ -554,14 +563,6 @@ struct AutoDiff { return chartT.hat(result); } -private: - - // Instantiate function and charts - CanonicalT chartT; - Canonical1 chart1; - Canonical2 chart2; - F f; - }; // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector @@ -571,19 +572,19 @@ template<> struct zero { static const Camera value; }; -const Camera zero::value(Camera(Pose3(),Cal3Bundler(0,0,0))); +const Camera zero::value(Camera(Pose3(), Cal3Bundler(0, 0, 0))); template<> struct zero { static const Point3 value; }; -const Point3 zero::value(Point3(0,0,0)); +const Point3 zero::value(Point3(0, 0, 0)); template<> struct zero { static const Point2 value; }; -const Point2 zero::value(Point2(0,0)); +const Point2 zero::value(Point2(0, 0)); /* ************************************************************************* */ // Test AutoDiff wrapper Snavely @@ -593,7 +594,8 @@ TEST(Expression, AutoDiff3) { Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler(1, 0, 0)); Point3 X(10, 0, -5); // negative Z-axis convention of Snavely! - AutoDiff snavely; + typedef AdaptAutoDiff Adaptor; + Adaptor snavely; // Apply the mapping, to get image point b_x. Point2 expected(2, 1); @@ -601,20 +603,16 @@ TEST(Expression, AutoDiff3) { EXPECT(assert_equal(expected,actual,1e-9)); // // Get expected derivatives -// Matrix E1 = numericalDerivative21( -// SnavelyProjection(), P, X); -// Matrix E2 = numericalDerivative22( -// SnavelyProjection(), P, X); -// -// // Get derivatives with AutoDiff -// Vector2 actual2; -// MatrixRowMajor H1(2, 9), H2(2, 3); -// double *parameters[] = { P.data(), X.data() }; -// double *jacobians[] = { H1.data(), H2.data() }; -// CHECK( -// (AutoDiff::Differentiate( snavely, parameters, 2, actual2.data(), jacobians))); -// EXPECT(assert_equal(E1,H1,1e-8)); -// EXPECT(assert_equal(E2,H2,1e-8)); + Matrix E1 = numericalDerivative21(Adaptor(), P, X); + Matrix E2 = numericalDerivative22(Adaptor(), P, X); + + // Get derivatives with AutoDiff + Matrix29 H1; + Matrix23 H2; + Point2 actual2 = snavely(P, X, H1, H2); + EXPECT(assert_equal(expected,actual,1e-9)); + EXPECT(assert_equal(E1,H1,1e-8)); + EXPECT(assert_equal(E2,H2,1e-8)); } TEST(Expression, Snavely) { @@ -622,7 +620,7 @@ TEST(Expression, Snavely) { Expression X(2); // AutoDiff f; Expression expression( - AutoDiff(), P, X); + AdaptAutoDiff(), P, X); set expected = list_of(1)(2); EXPECT(expected == expression.keys()); }