diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 5a16895d0..869bff2ea 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -310,7 +310,7 @@ class UnaryExpression: public ExpressionNode { public: - typedef Eigen::Matrix JacobianTA; + typedef Eigen::Matrix JacobianTA; typedef boost::function)> Function; private: @@ -383,8 +383,8 @@ class BinaryExpression: public ExpressionNode { public: - typedef Eigen::Matrix JacobianTA1; - typedef Eigen::Matrix JacobianTA2; + typedef Eigen::Matrix JacobianTA1; + typedef Eigen::Matrix JacobianTA2; typedef boost::function< T(const A1&, const A2&, boost::optional, boost::optional)> Function; @@ -476,9 +476,12 @@ class TernaryExpression: public ExpressionNode { public: + typedef Eigen::Matrix JacobianTA1; + typedef Eigen::Matrix JacobianTA2; + typedef Eigen::Matrix JacobianTA3; typedef boost::function< - T(const A1&, const A2&, const A3&, boost::optional, - boost::optional, boost::optional)> Function; + T(const A1&, const A2&, const A3&, boost::optional, + boost::optional, boost::optional)> Function; private: @@ -528,11 +531,13 @@ public: Augmented a1 = this->expressionA1_->forward(values); Augmented a2 = this->expressionA2_->forward(values); Augmented a3 = this->expressionA3_->forward(values); - Matrix dTdA1, dTdA2, dTdA3; + JacobianTA1 dTdA1; + JacobianTA2 dTdA2; + JacobianTA3 dTdA3; T t = function_(a1.value(), a2.value(), a3.value(), - a1.constant() ? none : boost::optional(dTdA1), - a2.constant() ? none : boost::optional(dTdA2), - a3.constant() ? none : boost::optional(dTdA3)); + a1.constant() ? none : boost::optional(dTdA1), + a2.constant() ? none : boost::optional(dTdA2), + a3.constant() ? none : boost::optional(dTdA3)); return Augmented(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians(), dTdA3, a3.jacobians()); } @@ -542,7 +547,9 @@ public: boost::shared_ptr > trace1; boost::shared_ptr > trace2; boost::shared_ptr > trace3; - Matrix dTdA1, dTdA2, dTdA3; + JacobianTA1 dTdA1; + JacobianTA2 dTdA2; + JacobianTA3 dTdA3; /// Start the reverse AD process virtual void reverseAD(JacobianMap& jacobians) const { trace1->reverseAD(dTdA1, jacobians); diff --git a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp index 4a5b1a76f..0eb806c98 100644 --- a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp @@ -58,162 +58,217 @@ TEST(BADFactor, test) { } /* ************************************************************************* */ - // Unary(Binary(Leaf,Leaf)) - TEST(BADFactor, test1) { +// Unary(Binary(Leaf,Leaf)) +TEST(BADFactor, test1) { - // Create some values - Values values; - values.insert(1, Pose3()); - values.insert(2, Point3(0, 0, 1)); + // Create some values + Values values; + values.insert(1, Pose3()); + values.insert(2, Point3(0, 0, 1)); - // Create old-style factor to create expected value and derivatives - GenericProjectionFactor old(measured, model, 1, 2, - boost::make_shared()); - double expected_error = old.error(values); - GaussianFactor::shared_ptr expected = old.linearize(values); + // Create old-style factor to create expected value and derivatives + GenericProjectionFactor old(measured, model, 1, 2, + boost::make_shared()); + double expected_error = old.error(values); + GaussianFactor::shared_ptr expected = old.linearize(values); - // Create leaves - Pose3_ x(1); - Point3_ p(2); + // Create leaves + Pose3_ x(1); + Point3_ p(2); - // Try concise version - BADFactor f2(model, measured, project(transform_to(x, p))); - EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); - EXPECT_LONGS_EQUAL(2, f2.dim()); - boost::shared_ptr gf2 = f2.linearize(values); - EXPECT( assert_equal(*expected, *gf2, 1e-9)); - } + // Try concise version + BADFactor f2(model, measured, project(transform_to(x, p))); + EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f2.dim()); + boost::shared_ptr gf2 = f2.linearize(values); + EXPECT( assert_equal(*expected, *gf2, 1e-9)); +} - /* ************************************************************************* */ - // Binary(Leaf,Unary(Binary(Leaf,Leaf))) - TEST(BADFactor, test2) { +/* ************************************************************************* */ +// Binary(Leaf,Unary(Binary(Leaf,Leaf))) +TEST(BADFactor, test2) { - // Create some values - Values values; - values.insert(1, Pose3()); - values.insert(2, Point3(0, 0, 1)); - values.insert(3, Cal3_S2()); + // Create some values + Values values; + values.insert(1, Pose3()); + values.insert(2, Point3(0, 0, 1)); + values.insert(3, Cal3_S2()); - // Create old-style factor to create expected value and derivatives - GeneralSFMFactor2 old(measured, model, 1, 2, 3); - double expected_error = old.error(values); - GaussianFactor::shared_ptr expected = old.linearize(values); + // Create old-style factor to create expected value and derivatives + GeneralSFMFactor2 old(measured, model, 1, 2, 3); + double expected_error = old.error(values); + GaussianFactor::shared_ptr expected = old.linearize(values); - // Create leaves - Pose3_ x(1); - Point3_ p(2); - Cal3_S2_ K(3); + // Create leaves + Pose3_ x(1); + Point3_ p(2); + Cal3_S2_ K(3); - // Create expression tree - Point3_ p_cam(x, &Pose3::transform_to, p); - Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); - Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); + // Create expression tree + Point3_ p_cam(x, &Pose3::transform_to, p); + Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); + Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); - // Create factor and check value, dimension, linearization - BADFactor f(model, measured, uv_hat); - EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); - EXPECT_LONGS_EQUAL(2, f.dim()); - boost::shared_ptr gf = f.linearize(values); - EXPECT( assert_equal(*expected, *gf, 1e-9)); + // Create factor and check value, dimension, linearization + BADFactor f(model, measured, uv_hat); + EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f.dim()); + boost::shared_ptr gf = f.linearize(values); + EXPECT( assert_equal(*expected, *gf, 1e-9)); - // Try concise version - BADFactor f2(model, measured, - uncalibrate(K, project(transform_to(x, p)))); - EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); - EXPECT_LONGS_EQUAL(2, f2.dim()); - boost::shared_ptr gf2 = f2.linearize(values); - EXPECT( assert_equal(*expected, *gf2, 1e-9)); - } + // Try concise version + BADFactor f2(model, measured, + uncalibrate(K, project(transform_to(x, p)))); + EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f2.dim()); + boost::shared_ptr gf2 = f2.linearize(values); + EXPECT( assert_equal(*expected, *gf2, 1e-9)); - /* ************************************************************************* */ + TernaryExpression::Function fff = project6; - TEST(BADFactor, compose1) { + // Try ternary version + BADFactor f3(model, measured, project3(x, p, K)); + EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f3.dim()); + boost::shared_ptr gf3 = f3.linearize(values); + EXPECT( assert_equal(*expected, *gf3, 1e-9)); +} - // Create expression - Rot3_ R1(1), R2(2); - Rot3_ R3 = R1 * R2; +/* ************************************************************************* */ - // Create factor - BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); +TEST(BADFactor, compose1) { - // Create some values - Values values; - values.insert(1, Rot3()); - values.insert(2, Rot3()); + // Create expression + Rot3_ R1(1), R2(2); + Rot3_ R3 = R1 * R2; - // Check unwhitenedError - std::vector H(2); - Vector actual = f.unwhitenedError(values, H); - EXPECT( assert_equal(eye(3), H[0],1e-9)); - EXPECT( assert_equal(eye(3), H[1],1e-9)); + // Create factor + BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); - // Check linearization - JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); - } + // Create some values + Values values; + values.insert(1, Rot3()); + values.insert(2, Rot3()); - /* ************************************************************************* */ - // Test compose with arguments referring to the same rotation - TEST(BADFactor, compose2) { + // Check unwhitenedError + std::vector H(2); + Vector actual = f.unwhitenedError(values, H); + EXPECT( assert_equal(eye(3), H[0],1e-9)); + EXPECT( assert_equal(eye(3), H[1],1e-9)); - // Create expression - Rot3_ R1(1), R2(1); - Rot3_ R3 = R1 * R2; + // Check linearization + JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} - // Create factor - BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); +/* ************************************************************************* */ +// Test compose with arguments referring to the same rotation +TEST(BADFactor, compose2) { - // Create some values - Values values; - values.insert(1, Rot3()); + // Create expression + Rot3_ R1(1), R2(1); + Rot3_ R3 = R1 * R2; - // Check unwhitenedError - std::vector H(1); - Vector actual = f.unwhitenedError(values, H); - EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(2*eye(3), H[0],1e-9)); + // Create factor + BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); - // Check linearization - JacobianFactor expected(1, 2 * eye(3), zero(3)); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); - } + // Create some values + Values values; + values.insert(1, Rot3()); - /* ************************************************************************* */ - // Test compose with one arguments referring to a constant same rotation - TEST(BADFactor, compose3) { + // Check unwhitenedError + std::vector H(1); + Vector actual = f.unwhitenedError(values, H); + EXPECT_LONGS_EQUAL(1, H.size()); + EXPECT( assert_equal(2*eye(3), H[0],1e-9)); - // Create expression - Rot3_ R1(Rot3::identity()), R2(3); - Rot3_ R3 = R1 * R2; + // Check linearization + JacobianFactor expected(1, 2 * eye(3), zero(3)); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} - // Create factor - BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); +/* ************************************************************************* */ +// Test compose with one arguments referring to a constant same rotation +TEST(BADFactor, compose3) { - // Create some values - Values values; - values.insert(3, Rot3()); + // Create expression + Rot3_ R1(Rot3::identity()), R2(3); + Rot3_ R3 = R1 * R2; - // Check unwhitenedError - std::vector H(1); - Vector actual = f.unwhitenedError(values, H); - EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(eye(3), H[0],1e-9)); + // Create factor + BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); - // Check linearization - JacobianFactor expected(3, eye(3), zero(3)); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); - } + // Create some values + Values values; + values.insert(3, Rot3()); - /* ************************************************************************* */ + // Check unwhitenedError + std::vector H(1); + Vector actual = f.unwhitenedError(values, H); + EXPECT_LONGS_EQUAL(1, H.size()); + EXPECT( assert_equal(eye(3), H[0],1e-9)); + + // Check linearization + JacobianFactor expected(3, eye(3), zero(3)); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} + +/* ************************************************************************* */ +// Test compose with three arguments +Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, + boost::optional H1, boost::optional H2, + boost::optional H3) { + // return dummy derivatives (not correct, but that's ok for testing here) + if (H1) + *H1 = eye(3); + if (H2) + *H2 = eye(3); + if (H3) + *H3 = eye(3); + return R1 * (R2 * R3); +} + +TEST(BADFactor, composeTernary) { + + // Create expression + Rot3_ A(1), B(2), C(3); + Rot3_ ABC(composeThree, A, B, C); + + // Create factor + BADFactor f(noiseModel::Unit::Create(3), Rot3(), ABC); + + // Create some values + Values values; + values.insert(1, Rot3()); + values.insert(2, Rot3()); + values.insert(3, Rot3()); + + // Check unwhitenedError + std::vector H(3); + Vector actual = f.unwhitenedError(values, H); + EXPECT_LONGS_EQUAL(3, H.size()); + EXPECT( assert_equal(eye(3), H[0],1e-9)); + EXPECT( assert_equal(eye(3), H[1],1e-9)); + EXPECT( assert_equal(eye(3), H[2],1e-9)); + + // Check linearization + JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3)); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} + +/* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); diff --git a/gtsam_unstable/slam/expressions.h b/gtsam_unstable/slam/expressions.h index 1acde67d3..d9cbd5716 100644 --- a/gtsam_unstable/slam/expressions.h +++ b/gtsam_unstable/slam/expressions.h @@ -10,6 +10,7 @@ #include #include #include +#include namespace gtsam { @@ -48,6 +49,16 @@ Point2_ project(const Point3_& p_cam) { return Point2_(PinholeCamera::project_to_camera, p_cam); } +Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, + boost::optional Dpose, boost::optional Dpoint, + boost::optional Dcal) { + return PinholeCamera(x, K).project(p, Dpose, Dpoint, Dcal); +} + +Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) { + return Point2_(project6, x, p, K); +} + template Point2_ uncalibrate(const Expression& K, const Point2_& xy_hat) { return Point2_(K, &CAL::uncalibrate, xy_hat); diff --git a/gtsam_unstable/timing/timeCameraExpression.cpp b/gtsam_unstable/timing/timeCameraExpression.cpp index 4e4e31d18..3b5d85b72 100644 --- a/gtsam_unstable/timing/timeCameraExpression.cpp +++ b/gtsam_unstable/timing/timeCameraExpression.cpp @@ -26,7 +26,6 @@ #include #include #include // std::setprecision - using namespace std; using namespace gtsam; @@ -74,37 +73,43 @@ int main() { // Dedicated factor // Oct 3, 2014, Macbook Air // 4.2 musecs/call - GeneralSFMFactor2 oldFactor2(z, model, 1, 2, 3); - time(oldFactor2, values); + GeneralSFMFactor2 f1(z, model, 1, 2, 3); + time(f1, values); // BADFactor // Oct 3, 2014, Macbook Air // 20.3 musecs/call - BADFactor newFactor2(model, z, + BADFactor f2(model, z, uncalibrate(K, project(transform_to(x, p)))); - time(newFactor2, values); + time(f2, values); + + // BADFactor ternary + // Oct 3, 2014, Macbook Air + // 20.3 musecs/call + BADFactor f3(model, z, project3(x, p, K)); + time(f3, values); // CALIBRATED // Dedicated factor // Oct 3, 2014, Macbook Air // 3.4 musecs/call - GenericProjectionFactor oldFactor1(z, model, 1, 2, fixedK); - time(oldFactor1, values); + GenericProjectionFactor g1(z, model, 1, 2, fixedK); + time(g1, values); // BADFactor // Oct 3, 2014, Macbook Air // 16.0 musecs/call - BADFactor newFactor1(model, z, + BADFactor g2(model, z, uncalibrate(Cal3_S2_(*fixedK), project(transform_to(x, p)))); - time(newFactor1, values); + time(g2, values); // BADFactor, optimized // Oct 3, 2014, Macbook Air // 9.0 musecs/call typedef PinholeCamera Camera; typedef Expression Camera_; - BADFactor newFactor3(model, z, Point2_(myProject, x, p)); - time(newFactor3, values); + BADFactor g3(model, z, Point2_(myProject, x, p)); + time(g3, values); return 0; }