diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index c66cc3b8b..cc3cf766c 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -224,6 +224,60 @@ TEST(Expression, ternary) { EXPECT(expected == ABC.keys()); } +/* ************************************************************************* */ +// Some Ceres Snippets copied for testing +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +template inline T &RowMajorAccess(T *base, int rows, int cols, + int i, int j) { + return base[cols * i + j]; +} + +inline double RandDouble() { + double r = static_cast(rand()); + return r / RAND_MAX; +} + +// A structure for projecting a 3x4 camera matrix and a +// homogeneous 3D point, to a 2D inhomogeneous point. +struct Projective { + // Function that takes P and X as separate vectors: + // P, X -> x + template + bool operator()(A const P[12], A const X[4], A x[2]) const { + A PX[3]; + for (int i = 0; i < 3; ++i) { + PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] + + RowMajorAccess(P, 3, 4, i, 1) * X[1] + + RowMajorAccess(P, 3, 4, i, 2) * X[2] + + RowMajorAccess(P, 3, 4, i, 3) * X[3]; + } + if (PX[2] != 0.0) { + x[0] = PX[0] / PX[2]; + x[1] = PX[1] / PX[2]; + return true; + } + return false; + } +}; + +/* ************************************************************************* */ +//#include "/Users/frank/include/ceres/autodiff_cost_function.h" +// Test Ceres AutoDiff +TEST(Expression, AutoDiff) { + + MatrixRowMajor P(3, 4); + P << 1, 0, 0, 0, 0, 1, 0, 5, 0, 0, 1, 0; + Vector4 X(10, 0, 5, 1); + + // Apply the mapping, to get image point b_x. + Projective projective; + Vector2 actual; + EXPECT(projective(P.data(), X.data(), actual.data())); + + Vector expected = Vector2(2, 1); + EXPECT(assert_equal(expected,actual,1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr;