diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index ccbc303f2..8364a498a 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -184,139 +184,139 @@ TEST(ExpressionFactor, test2) { EXPECT( assert_equal(*expected, *gf3, 1e-9)); } -/* ************************************************************************* */ - -TEST(ExpressionFactor, compose1) { - - // Create expression - Rot3_ R1(1), R2(2); - Rot3_ R3 = R1 * R2; - - // Create factor - ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); - - // Create some values - Values values; - values.insert(1, Rot3()); - values.insert(2, Rot3()); - - // 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)); - - // 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)); -} - -/* ************************************************************************* */ -// Test compose with arguments referring to the same rotation -TEST(ExpressionFactor, compose2) { - - // Create expression - Rot3_ R1(1), R2(1); - Rot3_ R3 = R1 * R2; - - // Create factor - ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); - - // Create some values - Values values; - values.insert(1, Rot3()); - - // 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)); - - // 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)); -} - -/* ************************************************************************* */ -// Test compose with one arguments referring to a constant same rotation -TEST(ExpressionFactor, compose3) { - - // Create expression - Rot3_ R1(Rot3::identity()), R2(3); - Rot3_ R3 = R1 * R2; - - // Create factor - ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); - - // 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(ExpressionFactor, composeTernary) { - - // Create expression - Rot3_ A(1), B(2), C(3); - Rot3_ ABC(composeThree, A, B, C); - - // Create factor - ExpressionFactor 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)); -} +///* ************************************************************************* */ +// +//TEST(ExpressionFactor, compose1) { +// +// // Create expression +// Rot3_ R1(1), R2(2); +// Rot3_ R3 = R1 * R2; +// +// // Create factor +// ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); +// +// // Create some values +// Values values; +// values.insert(1, Rot3()); +// values.insert(2, Rot3()); +// +// // 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)); +// +// // 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)); +//} +// +///* ************************************************************************* */ +//// Test compose with arguments referring to the same rotation +//TEST(ExpressionFactor, compose2) { +// +// // Create expression +// Rot3_ R1(1), R2(1); +// Rot3_ R3 = R1 * R2; +// +// // Create factor +// ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); +// +// // Create some values +// Values values; +// values.insert(1, Rot3()); +// +// // 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)); +// +// // 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)); +//} +// +///* ************************************************************************* */ +//// Test compose with one arguments referring to a constant same rotation +//TEST(ExpressionFactor, compose3) { +// +// // Create expression +// Rot3_ R1(Rot3::identity()), R2(3); +// Rot3_ R3 = R1 * R2; +// +// // Create factor +// ExpressionFactor f(noiseModel::Unit::Create(3), Rot3(), R3); +// +// // 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(ExpressionFactor, composeTernary) { +// +// // Create expression +// Rot3_ A(1), B(2), C(3); +// Rot3_ ABC(composeThree, A, B, C); +// +// // Create factor +// ExpressionFactor 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)); +//} /* ************************************************************************* */ @@ -328,7 +328,7 @@ template struct Incomplete; typedef mpl::vector MyTypes; typedef GenerateTrace::type Generated; //Incomplete incomplete; -BOOST_MPL_ASSERT((boost::is_same< Matrix25, Generated::JacobianTA >)); +//BOOST_MPL_ASSERT((boost::is_same< Matrix25, Generated::JacobianTA >)); BOOST_MPL_ASSERT((boost::is_same< Matrix2, Generated::Jacobian2T >)); Generated generated;