add LinearConstraint
thduynguyen
parent
88693e2129
commit
3aa7fd6d18
|
|
@ -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 <gtsam/linear/JacobianFactor.h>
|
||||
|
||||
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<This> 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<Key, Matrix>, specifying the
|
||||
* collection of keys and matrices making up the factor. */
|
||||
template<typename TERMS>
|
||||
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<GaussianFactor>(
|
||||
boost::make_shared<LinearConstraint>(*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
|
||||
|
||||
|
|
@ -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 <gtsam_unstable/linear/LinearConstraint.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/linear/HessianFactor.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
#include <boost/assign/list_of.hpp>
|
||||
//#include <boost/range/iterator_range.hpp>
|
||||
//#include <boost/range/adaptor/map.hpp>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace boost::assign;
|
||||
|
||||
namespace {
|
||||
namespace simple {
|
||||
// Terms we'll use
|
||||
const vector<pair<Key, Matrix> > terms = list_of<pair<Key,Matrix> >
|
||||
(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);}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue