From 2c37c94b5d0f5c357d887e2e37ec0f20770d7133 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 9 Nov 2009 21:34:20 +0000 Subject: [PATCH] Replaced the householder transform with the weighted system Removed constrained components from makefile, they will disappear shortly --- .cproject | 44 +++++------ cpp/LinearFactor.cpp | 32 +++----- cpp/LinearFactor.h | 6 +- cpp/Makefile.am | 13 +--- cpp/Matrix.cpp | 62 +++++++++++----- cpp/Matrix.h | 23 ++++-- cpp/Vector.cpp | 27 ++----- cpp/Vector.h | 15 +--- cpp/smallExample.cpp | 86 ++++++++++++++++++---- cpp/smallExample.h | 17 +++-- cpp/testLinearFactor.cpp | 68 ++++++++++++++++- cpp/testLinearFactorGraph.cpp | 51 +++++++++++++ cpp/testMatrix.cpp | 135 ++++++++++------------------------ cpp/testVector.cpp | 59 +++------------ 14 files changed, 348 insertions(+), 290 deletions(-) diff --git a/.cproject b/.cproject index f47fbdce0..76e521ae2 100644 --- a/.cproject +++ b/.cproject @@ -300,6 +300,7 @@ make + install true true @@ -307,6 +308,7 @@ make + check true true @@ -314,7 +316,7 @@ make - +-k check true false @@ -322,6 +324,7 @@ make + testSimpleCamera.run true true @@ -337,7 +340,6 @@ make - testVSLAMFactor.run true true @@ -345,6 +347,7 @@ make + testCalibratedCamera.run true true @@ -352,7 +355,6 @@ make - testConditionalGaussian.run true true @@ -360,6 +362,7 @@ make + testPose2.run true true @@ -375,6 +378,7 @@ make + testRot3.run true true @@ -382,7 +386,6 @@ make - testNonlinearOptimizer.run true true @@ -390,20 +393,15 @@ make + testLinearFactor.run true true true - -make -testConstrainedNonlinearFactorGraph.run -true -true -true - make + testLinearFactorGraph.run true true @@ -411,7 +409,6 @@ make - testNonlinearFactorGraph.run true true @@ -419,22 +416,14 @@ make -testPose3.run -true -true -true - - -make -testConstrainedLinearFactorGraph.run +testPose3.run true true true make - testVectorConfig.run true true @@ -442,7 +431,6 @@ make - testPoint2.run true true @@ -450,6 +438,7 @@ make + testNonlinearFactor.run true true @@ -457,6 +446,7 @@ make + timeLinearFactor.run true true @@ -464,6 +454,7 @@ make + timeLinearFactorGraph.run true true @@ -471,6 +462,7 @@ make + testGaussianBayesNet.run true true @@ -478,7 +470,6 @@ make - testBayesTree.run true false @@ -486,6 +477,7 @@ make + testSymbolicBayesNet.run true false @@ -493,7 +485,6 @@ make - testSymbolicFactorGraph.run true false @@ -501,6 +492,7 @@ make + testVector.run true true @@ -508,6 +500,7 @@ make + testMatrix.run true true @@ -515,7 +508,6 @@ make - install true true @@ -523,7 +515,6 @@ make - clean true true @@ -531,7 +522,6 @@ make - check true true diff --git a/cpp/LinearFactor.cpp b/cpp/LinearFactor.cpp index 24bc9b1cf..070fa0f50 100644 --- a/cpp/LinearFactor.cpp +++ b/cpp/LinearFactor.cpp @@ -198,10 +198,7 @@ Matrix LinearFactor::matrix_augmented(const Ordering& ordering) const { B_mat(i,0) = b_(i); matrices.push_back(&B_mat); - // divide in sigma so error is indeed 0.5*|Ax-b| - Vector t = ediv(ones(sigmas_.size()),sigmas_); - Matrix A = vector_scale(collect(matrices), t); - return A; + return collect(matrices); } /* ************************************************************************* */ @@ -307,8 +304,8 @@ LinearFactor::eliminate(const string& key) // extract A, b from the combined linear factor (ensure that x is leading) // use an augmented system [A b] to prevent copying - Matrix Rd = matrix_augmented(ordering); - size_t m = Rd.size1(); size_t n = Rd.size2()-1; + Matrix Ab = matrix_augmented(ordering); + size_t m = Ab.size1(); size_t n = Ab.size2()-1; // get dimensions of the eliminated variable size_t n1 = getDim(key); @@ -319,25 +316,18 @@ LinearFactor::eliminate(const string& key) throw(domain_error("LinearFactor::eliminate: fewer constraints than unknowns")); // Do in-place QR to get R, d of the augmented system - if (verbose) ::print(Rd,"Rd before"); - householder(Rd, maxRank); + if (verbose) ::print(Ab,"Rd before"); + Matrix Rd; Vector sigmas; + boost::tie(Rd, sigmas) = weighted_eliminate(Ab, sigmas_); if (verbose) ::print(Rd,"Rd after"); - // R as calculated by householder has inverse sigma on diagonal - // Use them to normalize R to unit-upper-triangular matrix - Vector sigmas(m); // standard deviations - if (verbose) cout << n1 << " " << n << " " << m << endl; - for (int i=0; i=A.size2()) + throw invalid_argument("Column index out of bounds!"); + + size_t m = A.size1(); + Vector a(m); + for (int i=0; i weighted_eliminate(Matrix& A, const Vector& sigmas) { // get sizes - size_t m = A.size1(); size_t n = A.size2(); -// print(A,"Initial A"); -// cout << "Row: " << row << endl; -// print(a,"a"); -// print(pseudo,"pseudo"); + size_t m = A.size1(); + size_t n = A.size2(); + size_t maxRank = min(m,n); - // calculate product = pseudo*A - Vector product = zero(n); - for (int j=0; j qr(const Matrix& A); */ void householder_update(Matrix &A, int j, double beta, const Vector& vjm); -/* - * Imperative version of weighted Householder substitution - * @param A is the matrix to reduce - * @param row is the row to start on (rows above aren't affected) - * @param pseudo is the pseudoinverse of the first column of A - * @param a is the first column of A - * A is updated into non-normalized R of A that has been updated +/** + * Imperative algorithm for in-place full elimination with + * weights and constraint handling + * @param Ab is an augmented system to eliminate + * @param sigmas is a vector of the measurement standard deviation + * @return a pair of R (normalized) and new sigmas */ -void whouse_subs(Matrix& A, unsigned int row, const Vector& a, const Vector& pseudo); +std::pair weighted_eliminate(Matrix& Ab, const Vector& sigmas); /** * Householder tranformation, Householder vectors below diagonal diff --git a/cpp/Vector.cpp b/cpp/Vector.cpp index ef53cd48d..1e7fd3822 100644 --- a/cpp/Vector.cpp +++ b/cpp/Vector.cpp @@ -182,32 +182,19 @@ namespace gtsam { } /* ************************************************************************* */ - Vector whouse_solve(const Vector& v, const Vector& precisions) { - if (v.size() != precisions.size()) + pair weightedPseudoinverse(const Vector& v, const Vector& sigmas) { + if (v.size() != sigmas.size()) throw invalid_argument("V and precisions have different sizes!"); double normV = 0; - for(int i = 0; i house(Vector &x); * Weighted Householder solution vector, * a.k.a., the pseudoinverse of the column * @param v is the first column of the matrix to solve - * @param precisions is a vector of precisions ( sigma^(-2) ) - * @return the pseudoinverse of v + * @param simgas is a vector of standard deviations + * @return a pair of the pseudoinverse of v and the precision */ -Vector whouse_solve(const Vector& v, const Vector& precisions); - -/** - * Weighted Householder solution substitution into a vector - * @param b is vector to update IN PLACE - * @param row is the row being updated (the specified row is not touched) - * @param a is the first column of A - * @param pseudo is the pseudoinverse of a - */ -void whouse_subs(Vector& b, size_t row, const Vector& a, const Vector& pseudo); +std::pair weightedPseudoinverse(const Vector& v, const Vector& sigmas); /** * concatenate Vectors diff --git a/cpp/smallExample.cpp b/cpp/smallExample.cpp index 3a51c11ea..9b3e3ff60 100644 --- a/cpp/smallExample.cpp +++ b/cpp/smallExample.cpp @@ -14,7 +14,6 @@ using namespace std; #include "Ordering.h" #include "Matrix.h" #include "NonlinearFactor.h" -#include "ConstrainedLinearFactorGraph.h" #include "smallExample.h" #include "Point2Prior.h" #include "Simulated2DOdometry.h" @@ -283,7 +282,45 @@ LinearFactorGraph createSmoother(int T) { } /* ************************************************************************* */ -ConstrainedLinearFactorGraph createSingleConstraintGraph() { +LinearFactorGraph createSimpleConstraintGraph() { + // create unary factor + // prior on "x", mean = [1,-1], sigma=0.1 + double sigma = 0.1; + Matrix Ax = eye(2); + Vector b1(2); + b1(0) = 1.0; + b1(1) = -1.0; + LinearFactor::shared_ptr f1(new LinearFactor("x", Ax, b1, sigma)); + + // create binary constraint factor + // between "x" and "y", that is going to be the only factor on "y" + // |1 0||x_1| + |-1 0||y_1| = |0| + // |0 1||x_2| | 0 -1||y_2| |0| + Matrix Ax1 = eye(2); + Matrix Ay1 = eye(2) * -1; + Vector b2 = Vector_(2, 0.0, 0.0); + LinearFactor::shared_ptr f2( + new LinearFactor("x", Ax1, "y", Ay1, b2, 0.0)); + + // construct the graph + LinearFactorGraph fg; + fg.push_back(f1); + fg.push_back(f2); + + return fg; +} + +/* ************************************************************************* */ +VectorConfig createSimpleConstraintConfig() { + VectorConfig config; + Vector v = Vector_(2, 1.0, -1.0); + config.insert("x", v); + config.insert("y", v); + return config; +} + +/* ************************************************************************* */ +LinearFactorGraph createSingleConstraintGraph() { // create unary factor // prior on "x", mean = [1,-1], sigma=0.1 double sigma = 0.1; @@ -302,19 +339,27 @@ ConstrainedLinearFactorGraph createSingleConstraintGraph() { Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0; Matrix Ay1 = eye(2) * 10; Vector b2 = Vector_(2, 1.0, 2.0); - LinearConstraint::shared_ptr f2( - new LinearConstraint("x", Ax1, "y", Ay1, b2)); + LinearFactor::shared_ptr f2( + new LinearFactor("x", Ax1, "y", Ay1, b2, 0.0)); // construct the graph - ConstrainedLinearFactorGraph fg; + LinearFactorGraph fg; fg.push_back(f1); - fg.push_back_constraint(f2); + fg.push_back(f2); return fg; } /* ************************************************************************* */ -ConstrainedLinearFactorGraph createMultiConstraintGraph() { +VectorConfig createSingleConstraintConfig() { + VectorConfig config; + config.insert("x", Vector_(2, 1.0, -1.0)); + config.insert("y", Vector_(2, 0.2, 0.1)); + return config; +} + +/* ************************************************************************* */ +LinearFactorGraph createMultiConstraintGraph() { // unary factor 1 double sigma = 0.1; Matrix A = eye(2); @@ -332,7 +377,7 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() { Vector b1(2); b1(0) = 1.0; b1(1) = 2.0; - LinearConstraint::shared_ptr lc1(new LinearConstraint("x", A11, "y", A12, b1)); + LinearFactor::shared_ptr lc1(new LinearFactor("x", A11, "y", A12, b1, 0.0)); // constraint 2 Matrix A21(2,2); @@ -345,25 +390,34 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() { Vector b2(2); b2(0) = 3.0; b2(1) = 4.0; - LinearConstraint::shared_ptr lc2(new LinearConstraint("x", A21, "z", A22, b2)); + LinearFactor::shared_ptr lc2(new LinearFactor("x", A21, "z", A22, b2, 0.0)); // construct the graph - ConstrainedLinearFactorGraph fg; + LinearFactorGraph fg; fg.push_back(lf1); - fg.push_back_constraint(lc1); - fg.push_back_constraint(lc2); + fg.push_back(lc1); + fg.push_back(lc2); return fg; } /* ************************************************************************* */ -//ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph() +VectorConfig createMultiConstraintConfig() { + VectorConfig config; + config.insert("x", Vector_(2, -2.0, 2.0)); + config.insert("y", Vector_(2, -0.1, 0.4)); + config.insert("z", Vector_(2, -4.0, 5.0)); + return config; +} + +/* ************************************************************************* */ +//LinearFactorGraph createConstrainedLinearFactorGraph() //{ -// ConstrainedLinearFactorGraph graph; +// LinearFactorGraph graph; // // // add an equality factor // Vector v1(2); v1(0)=1.;v1(1)=2.; -// LinearConstraint::shared_ptr f1(new LinearConstraint(v1, "x0")); +// LinearFactor::shared_ptr f1(new LinearFactor(v1, "x0")); // graph.push_back_eq(f1); // // // add a normal linear factor @@ -386,7 +440,7 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() { // VectorConfig c = createConstrainedConfig(); // // // equality constraint for initial pose -// LinearConstraint::shared_ptr f1(new LinearConstraint(c["x0"], "x0")); +// LinearFactor::shared_ptr f1(new LinearFactor(c["x0"], "x0")); // graph.push_back_eq(f1); // // // odometry between x0 and x1 diff --git a/cpp/smallExample.h b/cpp/smallExample.h index ff359a41b..ae7bbb278 100644 --- a/cpp/smallExample.h +++ b/cpp/smallExample.h @@ -15,8 +15,6 @@ namespace gtsam { - class ConstrainedLinearFactorGraph; - typedef NonlinearFactorGraph ExampleNonlinearFactorGraph; /** @@ -75,17 +73,26 @@ namespace gtsam { // Constrained Examples /* ******************************************************* */ + /** + * Creates a simple constrained graph with one linear factor and + * one binary equality constraint that sets x = y + */ + LinearFactorGraph createSimpleConstraintGraph(); + VectorConfig createSimpleConstraintConfig(); + /** * Creates a simple constrained graph with one linear factor and * one binary constraint. */ - ConstrainedLinearFactorGraph createSingleConstraintGraph(); + LinearFactorGraph createSingleConstraintGraph(); + VectorConfig createSingleConstraintConfig(); /** * Creates a constrained graph with a linear factor and two * binary constraints that share a node */ - ConstrainedLinearFactorGraph createMultiConstraintGraph(); + LinearFactorGraph createMultiConstraintGraph(); + VectorConfig createMultiConstraintConfig(); /** * These are the old examples from the EqualityFactor/DeltaFunction @@ -110,7 +117,7 @@ namespace gtsam { /** * Create small example constrained factor graph */ - //ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph(); + //LinearFactorGraph createConstrainedLinearFactorGraph(); /** * Create small example constrained nonlinear factor graph diff --git a/cpp/testLinearFactor.cpp b/cpp/testLinearFactor.cpp index bedb46e42..7e2e55af0 100644 --- a/cpp/testLinearFactor.cpp +++ b/cpp/testLinearFactor.cpp @@ -523,8 +523,8 @@ TEST( LinearFactor, matrix_aug ) Ab = lf->matrix_augmented(ord); Matrix Ab1 = Matrix_(2,5, - -10.0, 0.0, 10.0, 0.0, 2.0, - 000.0,-10.0, 0.0, 10.0, -1.0 ); + -1.0, 0.0, 1.0, 0.0, 0.2, + 00.0,- 1.0, 0.0, 1.0, -0.1 ); EQUALITY(Ab,Ab1); } @@ -635,6 +635,70 @@ TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian ) CHECK(assert_equal(expectedLF,actualLF,1e-5)); } + +/* ************************************************************************* */ +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 +} + +/* ************************************************************************* */ +TEST ( LinearFactor, constraint_eliminate2 ) +{ +// // Construct a linear constraint +// // RHS +// Vector b(2); b(0)=3.0; b(1)=4.0; +// +// // A1 - invertible +// Matrix A1(2,2); +// A1(0,0) = 1.0 ; A1(0,1) = 2.0; +// A1(1,0) = 2.0 ; A1(1,1) = 1.0; +// +// // A2 - not invertible - solve will throw an exception +// Matrix A2(2,2); +// A2(0,0) = 1.0 ; A2(0,1) = 2.0; +// A2(1,0) = 2.0 ; A2(1,1) = 4.0; +// +// LinearFactor lc("x", A1, "y", A2, b, 0.0); +// +// Vector y = Vector_(2, 1.0, 2.0); +// +// VectorConfig fg1; +// fg1.insert("y", y); +// +// 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)); +// +// // eliminate y to test thrown error +// VectorConfig fg2; +// fg2.insert("x", expected); +// actual = lc.eliminate("y"); +// try { +// Vector output = actual->solve(fg2); +// CHECK(false); +// } catch (...) { +// CHECK(true); +// } +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/cpp/testLinearFactorGraph.cpp b/cpp/testLinearFactorGraph.cpp index 92476c31c..88204fbe6 100644 --- a/cpp/testLinearFactorGraph.cpp +++ b/cpp/testLinearFactorGraph.cpp @@ -585,6 +585,57 @@ TEST( LinearFactorGraph, variables ) CHECK(expected==actual); } +/* ************************************************************************* */ +// 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_single ) +//{ +// // get a graph with a constraint in it +// LinearFactorGraph fg = createSingleConstraintGraph(); +// +// // eliminate and solve +// Ordering ord; +// ord += "x", "y"; +// VectorConfig actual = fg.optimize(ord); +// +// // verify +// VectorConfig expected = createSingleConstraintConfig(); +// CHECK(assert_equal(actual, expected)); +//} +// +///* ************************************************************************* */ +//TEST( LinearFactorGraph, constrained_multi ) +//{ +// // get a graph with a constraint in it +// LinearFactorGraph fg = createMultiConstraintGraph(); +// +// // eliminate and solve +// Ordering ord; +// ord += "x", "y", "z"; +// VectorConfig actual = fg.optimize(ord); +// +// // verify +// VectorConfig expected = createMultiConstraintConfig(); +// CHECK(assert_equal(actual, expected)); +//} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/cpp/testMatrix.cpp b/cpp/testMatrix.cpp index eb8dc84c7..35bfda605 100644 --- a/cpp/testMatrix.cpp +++ b/cpp/testMatrix.cpp @@ -102,6 +102,27 @@ TEST( matrix, stack ) EQUALITY(C,AB); } +/* ************************************************************************* */ +TEST( matrix, column ) +{ + Matrix A = Matrix_(4, 7, + -1., 0., 1., 0., 0., 0., -0.2, + 0., -1., 0., 1., 0., 0., 0.3, + 1., 0., 0., 0., -1., 0., 0.2, + 0., 1., 0., 0., 0., -1., -0.1); + Vector a1 = column(A, 0); + Vector exp1 = Vector_(4, -1., 0., 1., 0.); + CHECK(assert_equal(a1, exp1)); + + Vector a2 = column(A, 3); + Vector exp2 = Vector_(4, 0., 1., 0., 0.); + CHECK(assert_equal(a2, exp2)); + + Vector a3 = column(A, 6); + Vector exp3 = Vector_(4, -0.2, 0.3, 0.2, -0.1); + CHECK(assert_equal(a3, exp3)); +} + /* ************************************************************************* */ TEST( matrix, zeros ) { @@ -509,109 +530,29 @@ TEST( matrix, svd ) } /* ************************************************************************* */ -TEST( matrix, whouse_subs ) +TEST( matrix, weighted_elimination ) { - // create the system - Matrix A(2,2); - A(0,0) = 1.0; A(0,1) = 3.0; - A(1,0) = 2.0; A(1,1) = 4.0; + // create a matrix to eliminate - assume augmented + Matrix A = Matrix_(4, 7, + -1., 0., 1., 0., 0., 0., -0.2, + 0., -1., 0., 1., 0., 0., 0.3, + 1., 0., 0., 0., -1., 0., 0.2, + 0., 1., 0., 0., 0., -1., -0.1); + Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1); - // Vector to eliminate - Vector a(2); - a(0) = 1.0; a(1) = 2.0; - - Vector tau(2); //correspond to sigmas = [0.1 0.2] - tau(0) = 100.; tau(1) = 25.; - - // find the pseudoinverse - Vector pseudo = whouse_solve(a, tau); - - // substitute - int row = 0; // eliminating the first column - whouse_subs(A, row, a, pseudo); - - // create expected value - Matrix exp(2,2); - exp(0,0) = 1.0; exp(0,1) = 3.0; - exp(1,0) = 0.0; exp(1,1) = -1.0; + // perform elimination + Matrix actual; Vector newSigmas; + boost::tie(actual, newSigmas) = weighted_eliminate(A, sigmas); // verify - CHECK(assert_equal(A, exp)); + Matrix expected = Matrix_(4, 7, + 1., 0.,-0.2, 0.,-0.8, 0., 0.2, + 0., 1., 0.,-0.2, 0.,-0.8,-0.14, + 0., 0., 1., 0., -1., 0., 0.0, + 0., 0., 0., 1., 0., -1., 0.2); + CHECK(assert_equal(actual,expected)); } -/* ************************************************************************* */ -TEST( matrix, whouse_subs_multistep ) -{ - // update two matrices - double sigma1 = 0.2; double tau1 = 1/(sigma1*sigma1); - double sigma2 = 0.1; double tau2 = 1/(sigma2*sigma2); - Matrix Ax2 = Matrix_(4,2, - // x2 - -1., 0., - +0.,-1., - 1., 0., - +0.,1. - ); - - Matrix Al1 = Matrix_(4,2, - // l1 - 1., 0., - 0., 1., - 0., 0., - 0., 0. - ); - - // Eliminating x2 - step 1 - Vector a1 = Vector_(4, -1., 0., 1., 0.); - Vector tau = Vector_(4, tau1, tau1, tau2, tau2); - Vector pseudo1 = whouse_solve(a1, tau); - - size_t row = 0; - whouse_subs(Ax2, row, a1, pseudo1); - whouse_subs(Al1, row, a1, pseudo1); - - // verify first update - Matrix Ax2_exp = Matrix_(4,2, - -1., 0., - +0.,-1., - +0., 0., - +0.,1. - ); - CHECK(assert_equal(Ax2, Ax2_exp, 1e-4)); - Matrix Al1_exp = Matrix_(4,2, - // l1 - 1., 0., - 0., 1., - 0.2, 0., - 0., 0. - ); - CHECK(assert_equal(Al1, Al1_exp, 1e-4)); - - // Eliminating x2 - step 2 - Vector a2 = Vector_(3, -1., 0., 1.); - Vector tauR2 = sub(tau,1,4); - Vector pseudo2 = whouse_solve(a2, tauR2); - - row = 1; - whouse_subs(Ax2, row, a2, pseudo2); - whouse_subs(Al1, row, a2, pseudo2); - - // verify second update - Ax2_exp = Matrix_(4,2, - -1., 0., - +0.,-1., - +0., 0., - +0., 0. - ); - CHECK(assert_equal(Ax2, Ax2_exp, 1e-4)); - Al1_exp = Matrix_(4,2, - 1., 0., - 0., 1., - 0.2, 0., - 0., 0.2 - ); - CHECK(assert_equal(Al1, Al1_exp, 1e-4)); //fails -} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/cpp/testVector.cpp b/cpp/testVector.cpp index 0226e38a7..a91e25810 100644 --- a/cpp/testVector.cpp +++ b/cpp/testVector.cpp @@ -127,69 +127,28 @@ TEST( TestVector, concatVectors) } /* ************************************************************************* */ -TEST( TestVector, whouse_solve ) +TEST( TestVector, weightedPseudoinverse ) { // column from a matrix Vector x(2); x(0) = 1.0; x(1) = 2.0; - // create precisions - correspond to sigmas = [0.1 0.2] - Vector tau(2); - tau(0) = 100; tau(1) = 25; + // create sigmas + Vector sigmas(2); + sigmas(0) = 0.1; sigmas(1) = 0.2; // perform solve - Vector act = whouse_solve(x, tau); + Vector act; double precision; + boost::tie(act, precision) = weightedPseudoinverse(x, sigmas); // construct expected Vector exp(2); exp(0) = 0.5; exp(1) = 0.25; + double expPrecision = 200.0; // verify - CHECK(assert_equal(act, exp)); -} - -/* ************************************************************************* */ -TEST( TestVector, whouse_subs_vector ) -{ - // vector to update - Vector b(2); - b(0) = 5; b(1) = 6; - - // Vector to eliminate - Vector a(2); - a(0) = 1.0; a(1) = 2.0; - - Vector tau(2); //correspond to sigmas = [0.1 0.2] - tau(0) = 100; tau(1) = 25; - - // find the pseudoinverse - Vector pseudo = whouse_solve(a, tau); - - // substitute - int row = 0; // eliminating the first column - whouse_subs(b, row, a, pseudo); - - // create expected value - Vector exp(2); - exp(0) = 5; exp(1) = -2.0; - - // verify - CHECK(assert_equal(b, exp, 1e-5)); -} - -/* ************************************************************************* */ -TEST( TestVector, whouse_subs_vector2 ) -{ - double sigma1 = 0.2; double tau1 = 1/(sigma1*sigma1); - double sigma2 = 0.1; double tau2 = 1/(sigma2*sigma2); - Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2); - - Vector a1 = Vector_(4, -1., 0., 1., 0.); - Vector tau = Vector_(4, tau1, tau1, tau2, tau2); - Vector pseudo1 = whouse_solve(a1, tau); - - Vector expected = Vector_(4,-0.2, 0., 0.8, 0.); - CHECK(assert_equal(pseudo1, expected, 1e-4)); + CHECK(assert_equal(act, exp)); + CHECK(fabs(expPrecision-precision) < 1e-5); } /* ************************************************************************* */