diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp index 3875bfa97..ae7c0de06 100644 --- a/cpp/Matrix.cpp +++ b/cpp/Matrix.cpp @@ -286,6 +286,7 @@ void householder_update(Matrix &A, int j, double beta, const Vector& vjm) { /* ************************************************************************* */ std::pair weighted_eliminate(Matrix& A, const Vector& sigmas) { + bool verbose = false; // get sizes size_t m = A.size1(); size_t n = A.size2(); @@ -301,21 +302,26 @@ std::pair weighted_eliminate(Matrix& A, const Vector& sigmas) { for (int j=0; j weightedPseudoinverse(const Vector& v, const Vector& sigmas) { if (v.size() != sigmas.size()) throw invalid_argument("V and precisions have different sizes!"); - double normV = 0; - Vector precisions(sigmas.size()); - for(int i = 0; i 1e-9) { + if (constraint_index != -1) + throw invalid_argument("Multiple constraints on a single node!"); + else + constraint_index = i; + } + } + + // compute pseudoinverse + if (constraint_index != -1) { + // constrained case + Vector sol = zero(sigmas.size()); + sol(constraint_index) = 1.0; + return make_pair(sol, 1.0/0.0); + } else { + // normal case + double normV = 0.; + Vector precisions(sigmas.size()); + for(int i = 0; i 1e-5) + sol[i] = precisions[i]*v[i]; + return make_pair(sol/normV, normV); } - Vector sol(v.size()); - for(int i = 0; i house(Vector &x); /** * Weighted Householder solution vector, * a.k.a., the pseudoinverse of the column + * NOTE: if any sigmas are zero (indicating a constraint) + * the pseudoinverse will be a selection vector, and the + * precision will be infinite * @param v is the first column of the matrix to solve * @param simgas is a vector of standard deviations * @return a pair of the pseudoinverse of v and the precision diff --git a/cpp/testLinearFactor.cpp b/cpp/testLinearFactor.cpp index 7e2e55af0..add53c679 100644 --- a/cpp/testLinearFactor.cpp +++ b/cpp/testLinearFactor.cpp @@ -639,28 +639,29 @@ TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian ) /* ************************************************************************* */ TEST ( LinearFactor, constraint_eliminate1 ) { -// // construct a linear constraint -// Vector v(2); v(0)=1.2; v(1)=3.4; -// string key = "x0"; -// LinearFactor lc(key, eye(2), v, 0.0); -// -// // eliminate it -// ConditionalGaussian::shared_ptr actualCG; -// LinearFactor::shared_ptr actualLF; -// boost::tie(actualCG,actualLF) = lc.eliminate("x0"); -// -// // verify linear factor -// CHECK(actualLF->size() == 0); -// -// // verify conditional Gaussian -// Vector sigmas = Vector_(2, 0.0, 0.0); -// ConditionalGaussian expCG("x0", v, eye(2), sigmas); -// CHECK(assert_equal(expCG, *actualCG)); // FAILS - gets NaN values + // construct a linear constraint + Vector v(2); v(0)=1.2; v(1)=3.4; + string key = "x0"; + LinearFactor lc(key, eye(2), v, 0.0); + + // eliminate it + ConditionalGaussian::shared_ptr actualCG; + LinearFactor::shared_ptr actualLF; + boost::tie(actualCG,actualLF) = lc.eliminate("x0"); + + // verify linear factor + CHECK(actualLF->size() == 0); + + // verify conditional Gaussian + Vector sigmas = Vector_(2, 0.0, 0.0); + ConditionalGaussian expCG("x0", v, eye(2), sigmas); + CHECK(assert_equal(expCG, *actualCG)); } +// This test fails due to multiple constraints on a node /* ************************************************************************* */ -TEST ( LinearFactor, constraint_eliminate2 ) -{ +//TEST ( LinearFactor, constraint_eliminate2 ) +//{ // // Construct a linear constraint // // RHS // Vector b(2); b(0)=3.0; b(1)=4.0; @@ -685,9 +686,12 @@ TEST ( LinearFactor, constraint_eliminate2 ) // Vector expected = Vector_(2, -3.3333, 0.6667); // // // eliminate x for basic check -// ConditionalGaussian::shared_ptr actual = lc.eliminate("x"); -// CHECK(assert_equal(expected, actual->solve(fg1), 1e-4)); // +// ConditionalGaussian::shared_ptr actualCG; +// LinearFactor::shared_ptr actualLF; +// boost::tie(actualCG, actualLF) = lc.eliminate("x"); +// CHECK(assert_equal(expected, actualCG->solve(fg1), 1e-4)); + // // eliminate y to test thrown error // VectorConfig fg2; // fg2.insert("x", expected); @@ -698,7 +702,7 @@ TEST ( LinearFactor, constraint_eliminate2 ) // } catch (...) { // CHECK(true); // } -} +//} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/cpp/testLinearFactorGraph.cpp b/cpp/testLinearFactorGraph.cpp index 88204fbe6..ecfe46837 100644 --- a/cpp/testLinearFactorGraph.cpp +++ b/cpp/testLinearFactorGraph.cpp @@ -26,7 +26,7 @@ using namespace gtsam; double tol=1e-4; /* ************************************************************************* */ -/* unit test for equals (LinearFactorGraph1 == LinearFactorGraph2) */ +/* unit test for equals (LinearFactorGraph1 == LinearFactorGraph2) */ /* ************************************************************************* */ TEST( LinearFactorGraph, equals ){ @@ -49,10 +49,10 @@ TEST( LinearFactorGraph, error ) } /* ************************************************************************* */ -/* unit test for find seperator */ +/* unit test for find seperator */ /* ************************************************************************* */ TEST( LinearFactorGraph, find_separator ) -{ +{ LinearFactorGraph fg = createLinearFactorGraph(); set separator = fg.find_separator("x2"); @@ -68,7 +68,7 @@ TEST( LinearFactorGraph, find_separator ) /* ************************************************************************* */ TEST( LinearFactorGraph, combine_factors_x1 ) -{ +{ // create a small example for a linear factor graph LinearFactorGraph fg = createLinearFactorGraph(); @@ -77,8 +77,8 @@ TEST( LinearFactorGraph, combine_factors_x1 ) double sigma2 = 0.1; double sigma3 = 0.2; Vector sigmas = Vector_(6, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3); - - // combine all factors + + // combine all factors LinearFactor::shared_ptr actual = fg.removeAndCombineFactors("x1"); // the expected linear factor @@ -131,7 +131,7 @@ TEST( LinearFactorGraph, combine_factors_x1 ) /* ************************************************************************* */ TEST( LinearFactorGraph, combine_factors_x2 ) -{ +{ // create a small example for a linear factor graph LinearFactorGraph fg = createLinearFactorGraph(); @@ -214,7 +214,7 @@ TEST( LinearFactorGraph, eliminateOne_x1 ) } /* ************************************************************************* */ - + TEST( LinearFactorGraph, eliminateOne_x2 ) { LinearFactorGraph fg = createLinearFactorGraph(); @@ -587,23 +587,25 @@ TEST( LinearFactorGraph, variables ) /* ************************************************************************* */ // Tests ported from ConstrainedLinearFactorGraph +/* ************************************************************************* */ -///* ************************************************************************* */ -//TEST( LinearFactorGraph, constrained_simple ) -//{ -// // get a graph with a constraint in it -// LinearFactorGraph fg = createSimpleConstraintGraph(); -// -// // eliminate and solve -// Ordering ord; -// ord += "x", "y"; -// VectorConfig actual = fg.optimize(ord); -// -// // verify -// VectorConfig expected = createSimpleConstraintConfig(); -// CHECK(assert_equal(actual, expected)); -//} -// +/* ************************************************************************* */ +TEST( LinearFactorGraph, constrained_simple ) +{ + // get a graph with a constraint in it + LinearFactorGraph fg = createSimpleConstraintGraph(); + + // eliminate and solve + Ordering ord; + ord += "x", "y"; + VectorConfig actual = fg.optimize(ord); + + // verify + VectorConfig expected = createSimpleConstraintConfig(); + CHECK(assert_equal(actual, expected)); +} + +// These tests require multiple constraints on a single node and will fail ///* ************************************************************************* */ //TEST( LinearFactorGraph, constrained_single ) //{ diff --git a/cpp/testVector.cpp b/cpp/testVector.cpp index a91e25810..f6ac4c01a 100644 --- a/cpp/testVector.cpp +++ b/cpp/testVector.cpp @@ -151,6 +151,44 @@ TEST( TestVector, weightedPseudoinverse ) CHECK(fabs(expPrecision-precision) < 1e-5); } +/* ************************************************************************* */ +TEST( TestVector, weightedPseudoinverse_constraint ) +{ + // column from a matrix + Vector x(2); + x(0) = 1.0; x(1) = 2.0; + + // create sigmas + Vector sigmas(2); + sigmas(0) = 0.0; sigmas(1) = 0.2; + + // perform solve + Vector act; double precision; + boost::tie(act, precision) = weightedPseudoinverse(x, sigmas); + + // construct expected + Vector exp(2); + exp(0) = 1.0; exp(1) = 0.0; + + // verify + CHECK(assert_equal(act, exp)); + CHECK(isinf(precision)); +} + +/* ************************************************************************* */ +TEST( TestVector, weightedPseudoinverse_nan ) +{ + Vector a = Vector_(4, 1., 0., 0., 0.); + Vector sigmas = Vector_(4, 0.1, 0.1, 0., 0.); + Vector pseudo; double precision; + boost::tie(pseudo, precision) = weightedPseudoinverse(a, sigmas); + + Vector exp = Vector_(4, 1., 0., 0.,0.); + CHECK(assert_equal(pseudo, exp)); + DOUBLES_EQUAL(100, precision, 1e-5); +} + + /* ************************************************************************* */ TEST( TestVector, ediv ) {