diff --git a/gtsam_unstable/base/Expression-inl.h b/gtsam_unstable/base/Expression-inl.h index de6dbeb6f..66366ae14 100644 --- a/gtsam_unstable/base/Expression-inl.h +++ b/gtsam_unstable/base/Expression-inl.h @@ -17,7 +17,7 @@ * @brief Internals for Expression.h, not for general consumption */ -#include +#include #include namespace gtsam { diff --git a/gtsam_unstable/base/Expression.h b/gtsam_unstable/base/Expression.h index 24280db98..562554703 100644 --- a/gtsam_unstable/base/Expression.h +++ b/gtsam_unstable/base/Expression.h @@ -18,9 +18,7 @@ */ #include "Expression-inl.h" -#include #include -#include #include namespace gtsam { @@ -94,66 +92,5 @@ Expression operator*(const Expression& expression1, expression1, expression2); } -/** - * BAD Factor that supports arbitrary expressions via AD - */ -template -class BADFactor: NonlinearFactor { - - const T measurement_; - const Expression expression_; - - /// get value from expression and calculate error with respect to measurement - Vector unwhitenedError(const Values& values) const { - const T& value = expression_.value(values); - return value.localCoordinates(measurement_); - } - -public: - - /// Constructor - BADFactor(const T& measurement, const Expression& expression) : - measurement_(measurement), expression_(expression) { - } - /// Constructor - BADFactor(const T& measurement, const ExpressionNode& expression) : - measurement_(measurement), expression_(expression) { - } - /** - * Calculate the error of the factor. - * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. - * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model - * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. - */ - virtual double error(const Values& values) const { - if (this->active(values)) { - const Vector e = unwhitenedError(values); - return 0.5 * e.squaredNorm(); - } else { - return 0.0; - } - } - - /// get the dimension of the factor (number of rows on linearization) - size_t dim() const { - return 0; - } - - /// linearize to a GaussianFactor - boost::shared_ptr linearize(const Values& values) const { - // We will construct an n-ary factor below, where terms is a container whose - // value type is std::pair, specifying the - // collection of keys and matrices making up the factor. - std::map terms; - expression_.value(values, terms); - Vector b = unwhitenedError(values); - SharedDiagonal model = SharedDiagonal(); - return boost::shared_ptr( - new JacobianFactor(terms, b, model)); - } - -}; -// BADFactor - } diff --git a/gtsam_unstable/base/tests/testBAD.cpp b/gtsam_unstable/base/tests/testBAD.cpp index 95dd0a2de..207afa85e 100644 --- a/gtsam_unstable/base/tests/testBAD.cpp +++ b/gtsam_unstable/base/tests/testBAD.cpp @@ -17,7 +17,7 @@ * @brief unit tests for Block Automatic Differentiation */ -#include +#include #include #include #include @@ -49,19 +49,6 @@ Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional Dcal, TEST(BAD, test) { - // 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 - Point2 measured(-17, 30); - SharedNoiseModel model = noiseModel::Unit::Create(2); - GeneralSFMFactor2 old(measured, model, 1, 2, 3); - double expected_error = old.error(values); - GaussianFactor::shared_ptr expected = old.linearize(values); - // Test Constant expression Expression c(0); @@ -81,19 +68,6 @@ TEST(BAD, test) { expectedKeys.insert(2); expectedKeys.insert(3); EXPECT(expectedKeys == uv_hat.keys()); - - // Create factor - BADFactor f(measured, uv_hat); - - // Check value - EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); - - // Check dimension - EXPECT_LONGS_EQUAL(0, f.dim()); - - // Check linearization - boost::shared_ptr gf = f.linearize(values); - EXPECT( assert_equal(*expected, *gf, 1e-9)); } /* ************************************************************************* */ @@ -104,20 +78,11 @@ TEST(BAD, compose) { Expression R1(1), R2(2); Expression R3 = R1 * R2; - // Create factor - BADFactor f(Rot3(), R3); - - // Create some values - Values values; - values.insert(1, Rot3()); - values.insert(2, Rot3()); - - // 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)); + // Check keys + std::set expectedKeys; + expectedKeys.insert(1); + expectedKeys.insert(2); + EXPECT(expectedKeys == R3.keys()); } /* ************************************************************************* */ @@ -128,19 +93,10 @@ TEST(BAD, compose2) { Expression R1(1), R2(1); Expression R3 = R1 * R2; - // Create factor - BADFactor f(Rot3(), R3); - - // Create some values - Values values; - values.insert(1, Rot3()); - - // 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)); + // Check keys + std::set expectedKeys; + expectedKeys.insert(1); + EXPECT(expectedKeys == R3.keys()); } /* ************************************************************************* */