12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

remove LinearConstraint. It is replaced by LinearEquality

release/4.3a0
thduynguyen 2014-12-13 01:04:08 -05:00
parent 18481f21d0
commit c4b574774a
2 changed files with 32 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

107
gtsam_unstable/linear/LinearConstraint.h
View File

@ -1,107 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 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() {
}
/** equals */
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const {
return Base::equals(lf, tol);
}
/** print */
virtual void print(const std::string& s = "", const KeyFormatter& formatter =
DefaultKeyFormatter) const {
Base::print(s, formatter);
}
/** 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

64
gtsam_unstable/linear/tests/testLinearConstraint.cpp → gtsam_unstable/linear/tests/testLinearEquality.cpp
View File

@ -10,12 +10,12 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file testLinearConstraint.cpp * @file testLinearEquality.cpp
* @brief Unit tests for LinearConstraint * @brief Unit tests for LinearEquality
* @author thduynguyen * @author thduynguyen
**/ **/
#include <gtsam_unstable/linear/LinearConstraint.h> #include <gtsam_unstable/linear/LinearEquality.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/linear/HessianFactor.h> #include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
@ -28,7 +28,7 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace boost::assign; using namespace boost::assign;
GTSAM_CONCEPT_TESTABLE_INST(LinearConstraint) GTSAM_CONCEPT_TESTABLE_INST(LinearEquality)
namespace { namespace {
namespace simple { namespace simple {
@ -45,16 +45,16 @@ namespace {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LinearConstraint, constructors_and_accessors) TEST(LinearEquality, constructors_and_accessors)
{ {
using namespace simple; using namespace simple;
// Test for using different numbers of terms // Test for using different numbers of terms
{ {
// One term constructor // One term constructor
LinearConstraint expected( LinearEquality expected(
boost::make_iterator_range(terms.begin(), terms.begin() + 1), b); boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0);
LinearConstraint actual(terms[0].first, terms[0].second, b); LinearEquality actual(terms[0].first, terms[0].second, b, 0);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back()); LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back());
EXPECT(assert_equal(terms[0].second, actual.getA(actual.end() - 1))); EXPECT(assert_equal(terms[0].second, actual.getA(actual.end() - 1)));
@ -64,10 +64,10 @@ TEST(LinearConstraint, constructors_and_accessors)
} }
{ {
// Two term constructor // Two term constructor
LinearConstraint expected( LinearEquality expected(
boost::make_iterator_range(terms.begin(), terms.begin() + 2), b); boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0);
LinearConstraint actual(terms[0].first, terms[0].second, LinearEquality actual(terms[0].first, terms[0].second,
terms[1].first, terms[1].second, b); terms[1].first, terms[1].second, b, 0);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back()); LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back());
EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1))); EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1)));
@ -77,10 +77,10 @@ TEST(LinearConstraint, constructors_and_accessors)
} }
{ {
// Three term constructor // Three term constructor
LinearConstraint expected( LinearEquality expected(
boost::make_iterator_range(terms.begin(), terms.begin() + 3), b); boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0);
LinearConstraint actual(terms[0].first, terms[0].second, LinearEquality actual(terms[0].first, terms[0].second,
terms[1].first, terms[1].second, terms[2].first, terms[2].second, b); terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
@ -91,7 +91,7 @@ TEST(LinearConstraint, constructors_and_accessors)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LinearConstraint, Hessian_conversion) { TEST(LinearEquality, Hessian_conversion) {
HessianFactor hessian(0, (Matrix(4,4) << HessianFactor hessian(0, (Matrix(4,4) <<
1.57, 2.695, -1.1, -2.35, 1.57, 2.695, -1.1, -2.35,
2.695, 11.3125, -0.65, -10.225, 2.695, 11.3125, -0.65, -10.225,
@ -101,7 +101,7 @@ TEST(LinearConstraint, Hessian_conversion) {
73.1725); 73.1725);
try { try {
LinearConstraint actual(hessian); LinearEquality actual(hessian);
EXPECT(false); EXPECT(false);
} }
catch (const std::runtime_error& exception) { catch (const std::runtime_error& exception) {
@ -110,9 +110,9 @@ TEST(LinearConstraint, Hessian_conversion) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LinearConstraint, error) TEST(LinearEquality, error)
{ {
LinearConstraint factor(simple::terms, simple::b); LinearEquality factor(simple::terms, simple::b, 0);
VectorValues values; VectorValues values;
values.insert(5, Vector::Constant(3, 1.0)); values.insert(5, Vector::Constant(3, 1.0));
@ -134,10 +134,10 @@ TEST(LinearConstraint, error)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LinearConstraint, matrices_NULL) TEST(LinearEquality, matrices_NULL)
{ {
// Make sure everything works with NULL noise model // Make sure everything works with NULL noise model
LinearConstraint factor(simple::terms, simple::b); LinearEquality factor(simple::terms, simple::b, 0);
Matrix AExpected(3, 9); Matrix AExpected(3, 9);
AExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second; AExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second;
@ -157,10 +157,10 @@ TEST(LinearConstraint, matrices_NULL)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LinearConstraint, matrices) TEST(LinearEquality, matrices)
{ {
// And now witgh a non-unit noise model // And now witgh a non-unit noise model
LinearConstraint factor(simple::terms, simple::b); LinearEquality factor(simple::terms, simple::b, 0);
Matrix jacobianExpected(3, 9); Matrix jacobianExpected(3, 9);
jacobianExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second; jacobianExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second;
@ -184,11 +184,11 @@ TEST(LinearConstraint, matrices)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LinearConstraint, operators ) TEST(LinearEquality, operators )
{ {
Matrix I = eye(2); Matrix I = eye(2);
Vector b = (Vector(2) << 0.2,-0.1).finished(); Vector b = (Vector(2) << 0.2,-0.1).finished();
LinearConstraint lf(1, -I, 2, I, b); LinearEquality lf(1, -I, 2, I, b, 0);
VectorValues c; VectorValues c;
c.insert(1, (Vector(2) << 10.,20.).finished()); c.insert(1, (Vector(2) << 10.,20.).finished());
@ -210,25 +210,25 @@ TEST(LinearConstraint, operators )
// test gradient at zero // test gradient at zero
Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian(); Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian();
VectorValues expectedG; VectorValues expectedG;
expectedG.insert(1, (Vector(2) << -1,-1).finished()); expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished());
expectedG.insert(2, (Vector(2) << 1, 1).finished()); expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished());
VectorValues actualG = lf.gradientAtZero(); VectorValues actualG = lf.gradientAtZero();
EXPECT(assert_equal(expectedG, actualG)); EXPECT(assert_equal(expectedG, actualG));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LinearConstraint, default_error ) TEST(LinearEquality, default_error )
{ {
LinearConstraint f; LinearEquality f;
double actual = f.error(VectorValues()); double actual = f.error(VectorValues());
DOUBLES_EQUAL(0.0, actual, 1e-15); DOUBLES_EQUAL(0.0, actual, 1e-15);
} }
//* ************************************************************************* */ //* ************************************************************************* */
TEST(LinearConstraint, empty ) TEST(LinearEquality, empty )
{ {
// create an empty factor // create an empty factor
LinearConstraint f; LinearEquality f;
EXPECT(f.empty()); EXPECT(f.empty());
} }