diff --git a/.cproject b/.cproject index 6e8ee94ac..a69893058 100644 --- a/.cproject +++ b/.cproject @@ -2521,6 +2521,14 @@ true true + + make + -j5 + testBADFactor.run + true + true + true + make -j2 @@ -2641,6 +2649,14 @@ true true + + make + -j5 + testPoseRotationPrior.run + true + true + true + make -j2 diff --git a/gtsam_unstable/nonlinear/BADFactor.h b/gtsam_unstable/nonlinear/BADFactor.h new file mode 100644 index 000000000..e8d32cd08 --- /dev/null +++ b/gtsam_unstable/nonlinear/BADFactor.h @@ -0,0 +1,87 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Expression.h + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief Expressions for Block Automatic Differentiation + */ + +#include +#include + +namespace gtsam { + +/** + * 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/nonlinear/tests/testBADFactor.cpp b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp new file mode 100644 index 000000000..3a4c021ed --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testBADFactor.cpp + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief unit tests for Block Automatic Differentiation + */ + +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ + +Point3 transformTo(const Pose3& x, const Point3& p, + boost::optional Dpose, boost::optional Dpoint) { + return x.transform_to(p, Dpose, Dpoint); +} + +Point2 project(const Point3& p, boost::optional Dpoint) { + return PinholeCamera::project_to_camera(p, Dpoint); +} + +template +Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional Dcal, + boost::optional Dp) { + return K.uncalibrate(p, Dcal, Dp); +} + +/* ************************************************************************* */ + +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); + + // Create leaves + Expression x(1); + Expression p(2); + Expression K(3); + + // Create expression tree + Expression p_cam(transformTo, x, p); + Expression projection(project, p_cam); + Expression uv_hat(uncalibrate, K, projection); + + // 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)); +} + +/* ************************************************************************* */ + +TEST(BAD, compose) { + + // Create expression + 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)); +} + +/* ************************************************************************* */ +// Test compose with arguments referring to the same rotation +TEST(BAD, compose2) { + + // Create expression + 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)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +