From 3aa7fd6d1829dbbf7cbb96a3398f7d05668d8def Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Thu, 27 Nov 2014 10:45:23 -0500 Subject: [PATCH] add LinearConstraint --- gtsam_unstable/linear/LinearConstraint.h | 124 +++++++++ .../linear/tests/testLinearConstraint.cpp | 237 ++++++++++++++++++ 2 files changed, 361 insertions(+) create mode 100644 gtsam_unstable/linear/LinearConstraint.h create mode 100644 gtsam_unstable/linear/tests/testLinearConstraint.cpp diff --git a/gtsam_unstable/linear/LinearConstraint.h b/gtsam_unstable/linear/LinearConstraint.h new file mode 100644 index 000000000..8cb190016 --- /dev/null +++ b/gtsam_unstable/linear/LinearConstraint.h @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * LinearConstraint.h + * @brief: LinearConstraint derived from Base with constrained noise model + * @date: Nov 27, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * This class defines Linear constraints by inherit Base + * with the special Constrained noise model + */ +class LinearConstraint: public JacobianFactor { +public: + typedef LinearConstraint This; ///< Typedef to this class + typedef JacobianFactor Base; ///< Typedef to base class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + +public: + /** default constructor for I/O */ + LinearConstraint() : Base() {} + + /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ + explicit LinearConstraint(const HessianFactor& hf) { + throw std::runtime_error("Cannot convert HessianFactor to LinearConstraint"); + } + + /** Construct unary factor */ + LinearConstraint(Key i1, const Matrix& A1, const Vector& b) : + Base(i1, A1, b, noiseModel::Constrained::All(b.rows())) { + } + + /** Construct binary factor */ + LinearConstraint(Key i1, const Matrix& A1, Key i2, const Matrix& A2, + const Vector& b) : + Base(i1, A1, i2, A2, b, noiseModel::Constrained::All(b.rows())) { + } + + /** Construct ternary factor */ + LinearConstraint(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, const Vector& b) : + Base(i1, A1, i2, A2, i3, A3, b, + noiseModel::Constrained::All(b.rows())) { + } + + /** Construct four-ary factor */ + LinearConstraint(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, Key i4, const Matrix& A4, const Vector& b) : + Base(i1, A1, i2, A2, i3, A3, i4, A4, b, + noiseModel::Constrained::All(b.rows())) { + } + + /** Construct five-ary factor */ + LinearConstraint(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, Key i4, const Matrix& A4, Key i5, const Matrix& A5, + const Vector& b) : + Base(i1, A1, i2, A2, i3, A3, i4, A4, i5, A5, b, + noiseModel::Constrained::All(b.rows())) { + } + + /** Construct six-ary factor */ + LinearConstraint(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, Key i4, const Matrix& A4, Key i5, const Matrix& A5, + Key i6, const Matrix& A6, const Vector& b) : + Base(i1, A1, i2, A2, i3, A3, i4, A4, i5, A5, i6, A6, b, + noiseModel::Constrained::All(b.rows())) { + } + + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. */ + template + LinearConstraint(const TERMS& terms, const Vector& b) : + Base(terms, b, noiseModel::Constrained::All(b.rows())) { + } + + /** Virtual destructor */ + virtual ~LinearConstraint() { + } + + // Testable interface + virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { + return Base::equals(lf, tol); + } + + /** Clone this LinearConstraint */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + boost::make_shared(*this)); + } + + /** Special error_vector for constraints (A*x-b) */ + Vector error_vector(const VectorValues& c) const { + return unweighted_error(c); + } + + /** Special error for constraints. + * I think it should be zero, as this function is meant for objective cost. + * But the name "error" can be misleading. + * TODO: confirm with Frank!! */ + virtual double error(const VectorValues& c) const { + return 0.0; + } + +}; // LinearConstraint + +} // gtsam + diff --git a/gtsam_unstable/linear/tests/testLinearConstraint.cpp b/gtsam_unstable/linear/tests/testLinearConstraint.cpp new file mode 100644 index 000000000..489af1db3 --- /dev/null +++ b/gtsam_unstable/linear/tests/testLinearConstraint.cpp @@ -0,0 +1,237 @@ +/* ---------------------------------------------------------------------------- + + * 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 testLinearConstraint.cpp + * @brief Unit tests for LinearConstraint + * @author thduynguyen + **/ + +#include +#include +#include +#include +#include + +#include +#include +//#include +//#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +namespace { + namespace simple { + // Terms we'll use + const vector > terms = list_of > + (make_pair(5, Matrix3::Identity())) + (make_pair(10, 2*Matrix3::Identity())) + (make_pair(15, 3*Matrix3::Identity())); + + // RHS and sigmas + const Vector b = (Vector(3) << 1., 2., 3.); + const SharedDiagonal noise = noiseModel::Constrained::All(3); + } +} + +/* ************************************************************************* */ +TEST(LinearConstraint, constructors_and_accessors) +{ + using namespace simple; + + // Test for using different numbers of terms + { + // One term constructor + LinearConstraint expected( + boost::make_iterator_range(terms.begin(), terms.begin() + 1), b); + LinearConstraint actual(terms[0].first, terms[0].second, b); + EXPECT(assert_equal(expected, actual)); + LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[0].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(assert_equal(*noise, *actual.get_model())); + } + { + // Two term constructor + LinearConstraint expected( + boost::make_iterator_range(terms.begin(), terms.begin() + 2), b); + LinearConstraint actual(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, b); + EXPECT(assert_equal(expected, actual)); + LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(assert_equal(*noise, *actual.get_model())); + } + { + // Three term constructor + LinearConstraint expected( + boost::make_iterator_range(terms.begin(), terms.begin() + 3), b); + LinearConstraint actual(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b); + EXPECT(assert_equal(expected, actual)); + LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(assert_equal(*noise, *actual.get_model())); + } +} + +/* ************************************************************************* */ +TEST(LinearConstraint, Hessian_conversion) { + HessianFactor hessian(0, (Matrix(4,4) << + 1.57, 2.695, -1.1, -2.35, + 2.695, 11.3125, -0.65, -10.225, + -1.1, -0.65, 1, 0.5, + -2.35, -10.225, 0.5, 9.25), + (Vector(4) << -7.885, -28.5175, 2.75, 25.675), + 73.1725); + + try { + LinearConstraint actual(hessian); + EXPECT(false); + } + catch (const std::runtime_error& exception) { + EXPECT(true); + } +} + +/* ************************************************************************* */ +TEST(LinearConstraint, error) +{ + LinearConstraint factor(simple::terms, simple::b); + + VectorValues values; + values.insert(5, Vector::Constant(3, 1.0)); + values.insert(10, Vector::Constant(3, 0.5)); + values.insert(15, Vector::Constant(3, 1.0/3.0)); + + Vector expected_unwhitened(3); expected_unwhitened << 2.0, 1.0, 0.0; + Vector actual_unwhitened = factor.unweighted_error(values); + EXPECT(assert_equal(expected_unwhitened, actual_unwhitened)); + + // whitened is meaningless in constraints + Vector expected_whitened(3); expected_whitened = expected_unwhitened; + Vector actual_whitened = factor.error_vector(values); + EXPECT(assert_equal(expected_whitened, actual_whitened)); + + double expected_error = 0.0; + double actual_error = factor.error(values); + DOUBLES_EQUAL(expected_error, actual_error, 1e-10); +} + +/* ************************************************************************* */ +TEST(LinearConstraint, matrices_NULL) +{ + // Make sure everything works with NULL noise model + LinearConstraint factor(simple::terms, simple::b); + + Matrix AExpected(3, 9); + AExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second; + Vector rhsExpected = simple::b; + Matrix augmentedJacobianExpected(3, 10); + augmentedJacobianExpected << AExpected, rhsExpected; + + // Whitened Jacobian + EXPECT(assert_equal(AExpected, factor.jacobian().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobian().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian())); + + // Unwhitened Jacobian + EXPECT(assert_equal(AExpected, factor.jacobianUnweighted().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted())); +} + +/* ************************************************************************* */ +TEST(LinearConstraint, matrices) +{ + // And now witgh a non-unit noise model + LinearConstraint factor(simple::terms, simple::b); + + Matrix jacobianExpected(3, 9); + jacobianExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second; + Vector rhsExpected = simple::b; + Matrix augmentedJacobianExpected(3, 10); + augmentedJacobianExpected << jacobianExpected, rhsExpected; + + Matrix augmentedHessianExpected = + augmentedJacobianExpected.transpose() * simple::noise->R().transpose() + * simple::noise->R() * augmentedJacobianExpected; + + // Whitened Jacobian + EXPECT(assert_equal(jacobianExpected, factor.jacobian().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobian().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian())); + + // Unwhitened Jacobian + EXPECT(assert_equal(jacobianExpected, factor.jacobianUnweighted().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted())); +} + +/* ************************************************************************* */ +TEST(LinearConstraint, operators ) +{ + Matrix I = eye(2); + Vector b = (Vector(2) << 0.2,-0.1); + LinearConstraint lf(1, -I, 2, I, b); + + VectorValues c; + c.insert(1, (Vector(2) << 10.,20.)); + c.insert(2, (Vector(2) << 30.,60.)); + + // test A*x + Vector expectedE = (Vector(2) << 20.,40.); + Vector actualE = lf * c; + EXPECT(assert_equal(expectedE, actualE)); + + // test A^e + VectorValues expectedX; + expectedX.insert(1, (Vector(2) << -20.,-40.)); + expectedX.insert(2, (Vector(2) << 20., 40.)); + VectorValues actualX = VectorValues::Zero(expectedX); + lf.transposeMultiplyAdd(1.0, actualE, actualX); + EXPECT(assert_equal(expectedX, actualX)); + + // test gradient at zero + Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian(); + VectorValues expectedG; + expectedG.insert(1, (Vector(2) << -1,-1)); + expectedG.insert(2, (Vector(2) << 1, 1)); + VectorValues actualG = lf.gradientAtZero(); + EXPECT(assert_equal(expectedG, actualG)); +} + +/* ************************************************************************* */ +TEST(LinearConstraint, default_error ) +{ + LinearConstraint f; + double actual = f.error(VectorValues()); + DOUBLES_EQUAL(0.0, actual, 1e-15); +} + +//* ************************************************************************* */ +TEST(LinearConstraint, empty ) +{ + // create an empty factor + LinearConstraint f; + EXPECT(f.empty()); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */