diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index b668fe547..ad25455ee 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -491,6 +491,25 @@ TEST(Expression, AutoDiff2) { EXPECT(assert_equal(E2,H2,1e-8)); } +/* ************************************************************************* */ +// zero::value is intended to be the origin of a canonical coordinate system +// with canonical(t) == DefaultChart(zero::value).apply(t) +template struct zero; +template class Canonical { + DefaultChart chart; +public: + typedef T type; + typedef typename DefaultChart::vector vector; + Canonical() : + chart(zero::value) { + } + vector vee(const T& t) { + return chart.apply(t); + } + T hat(const vector& v) { + return chart.retract(v); + } +}; /* ************************************************************************* */ // Adapt ceres-style autodiff template @@ -500,12 +519,12 @@ struct AutoDiff { static const int M1 = dimension::value; static const int M2 = dimension::value; - typedef DefaultChart ChartT; - typedef DefaultChart Chart1; - typedef DefaultChart Chart2; - typedef typename ChartT::vector VectorT; - typedef typename Chart1::vector Vector1; - typedef typename Chart2::vector Vector2; + typedef Canonical CanonicalT; + typedef Canonical Canonical1; + typedef Canonical Canonical2; + typedef typename CanonicalT::vector VectorT; + typedef typename Canonical1::vector Vector1; + typedef typename Canonical2::vector Vector2; typedef Eigen::Matrix JacobianTA1; typedef Eigen::Matrix JacobianTA2; @@ -513,20 +532,9 @@ struct AutoDiff { T operator()(const A1& a1, const A2& a2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { - // Instantiate function and charts - T z; - A1 z1; - A2 z2; // TODO, zero - ChartT chartT(z); - Chart1 chart1(z1); - Chart2 chart2(z2); - F f; - // Make arguments - Vector1 v1 = chart1.apply(a1); - Vector2 v2 = chart2.apply(a2); - cout << v1 << endl; - cout << v2 << endl; + Vector1 v1 = chart1.vee(a1); + Vector2 v2 = chart2.vee(a2); bool success; VectorT result; @@ -543,14 +551,40 @@ struct AutoDiff { // Apply the mapping, to get result success = f(v1.data(), v2.data(), result.data()); } - cout << result << endl; - return chartT.retract(result); + 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 typedef PinholeCamera Camera; +template<> +struct zero { + static const Camera value; +}; +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)); + +template<> +struct zero { + static const Point2 value; +}; +const Point2 zero::value(Point2(0,0)); + /* ************************************************************************* */ // Test AutoDiff wrapper Snavely TEST(Expression, AutoDiff3) {