diff --git a/.cproject b/.cproject index 0a8bd560e..739ad24a7 100644 --- a/.cproject +++ b/.cproject @@ -300,6 +300,7 @@ make + install true true @@ -307,6 +308,7 @@ make + check true true @@ -314,7 +316,6 @@ make - check true true @@ -322,6 +323,7 @@ make + testSimpleCamera.run true true @@ -337,7 +339,6 @@ make - testVSLAMFactor.run true true @@ -345,6 +346,7 @@ make + testCalibratedCamera.run true true @@ -352,7 +354,6 @@ make - testConditionalGaussian.run true true @@ -360,6 +361,7 @@ make + testPose2.run true true @@ -375,6 +377,7 @@ make + testRot3.run true true @@ -382,7 +385,6 @@ make - testNonlinearOptimizer.run true true @@ -390,6 +392,7 @@ make + testLinearFactor.run true true @@ -397,6 +400,7 @@ make + testConstrainedNonlinearFactorGraph.run true true @@ -404,6 +408,7 @@ make + testLinearFactorGraph.run true true @@ -411,7 +416,6 @@ make - testNonlinearFactorGraph.run true true @@ -419,6 +423,7 @@ make + testPose3.run true true @@ -426,7 +431,6 @@ make - testConstrainedLinearFactorGraph.run true true @@ -434,7 +438,6 @@ make - testVectorConfig.run true true @@ -442,7 +445,6 @@ make - testPoint2.run true true @@ -450,6 +452,7 @@ make + testNonlinearFactor.run true true @@ -457,6 +460,7 @@ make + timeLinearFactor.run true true @@ -464,6 +468,7 @@ make + timeLinearFactorGraph.run true true @@ -471,6 +476,7 @@ make + testChordalBayesNet.run true true @@ -478,7 +484,6 @@ make - testBayesTree.run true false @@ -486,7 +491,6 @@ make - testSymbolicBayesChain.run true false @@ -494,15 +498,29 @@ make - testSymbolicFactorGraph.run true false true - + make +testVector.run +true +true +true + + +make + +testMatrix.run +true +true +true + + +make install true true @@ -510,7 +528,6 @@ make - clean true true @@ -518,7 +535,6 @@ make - check true true diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp index 8c31f9c18..7ada97ebb 100644 --- a/cpp/Matrix.cpp +++ b/cpp/Matrix.cpp @@ -270,29 +270,27 @@ void householder_update(Matrix &A, int j, double beta, const Vector& vjm) { } /* ************************************************************************* */ -void whouse_subs(Matrix& A, unsigned int row, const Vector& pseudo, const Vector& x) { +void whouse_subs(Matrix& A, unsigned int row, const Vector& a, const Vector& pseudo) { +// cout << "In matrix::whouse_subs()" << endl; // 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"); - // calculate Hw - Matrix Hw = eye(m,m); - for (int i=0; i house(Vector &x); */ 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); + /** * concatenate Vectors */ diff --git a/cpp/testMatrix.cpp b/cpp/testMatrix.cpp index 93be74c08..abfdef5ba 100644 --- a/cpp/testMatrix.cpp +++ b/cpp/testMatrix.cpp @@ -456,22 +456,22 @@ TEST( matrix, whouse_subs ) { // create the system Matrix A(2,2); - A(0,0) = 1; A(0,1) = 3; - A(1,0) = 2; A(1,1) = 4; + A(0,0) = 1.0; A(0,1) = 3.0; + A(1,0) = 2.0; A(1,1) = 4.0; // Vector to eliminate - Vector x(2); - x(0) = 1.0; x(1) = 2.0; + 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; + tau(0) = 100.; tau(1) = 25.; // find the pseudoinverse - Vector pseudo = whouse_solve(x, tau); + Vector pseudo = whouse_solve(a, tau); // substitute int row = 0; // eliminating the first column - whouse_subs(A, row, pseudo, x); + whouse_subs(A, row, a, pseudo); // create expected value Matrix exp(2,2); @@ -482,8 +482,120 @@ TEST( matrix, whouse_subs ) CHECK(assert_equal(A, exp)); } +/* ************************************************************************* */ +TEST( matrix, whouse_subs2 ) +{ + // create the system + Matrix A(4,2); + A(0,0) = 1.0; A(0,1) = 0.0; + A(1,0) = 0.0; A(1,1) = 1.0; + A(2,0) = 2.0/3.0; A(2,1) = 0.0; + A(3,0) = 0.0; A(3,1) = 0.0; + + // Vector to eliminate + Vector a(3); + a(0) = -0.333333; + a(1) = 0.0; + a(2) = 0.666667; + + // find the pseudoinverse + Vector pseudo(3); + pseudo(0) = -1.; + pseudo(1) = 0.; + pseudo(2) = 1.; + + // substitute + int row = 1; + whouse_subs(A, row, a, pseudo); + + // create expected value + Matrix exp(4,2); + exp(0,0) = 1.0; exp(0,1) = 0.0; + exp(1,0) = 0.0; exp(1,1) = 1.0; + exp(2,0) = 2.0/3.0; exp(2,1) = 0.0; + exp(3,0) = 0.0; exp(3,1) = 2.0/3.0; + + // verify + CHECK(assert_equal(A, exp, 1e-5)); +} + +/* ************************************************************************* */ +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); + Vector sigmas = Vector_(4, sigma1, sigma1, 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.3333, 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.3333, 0., + 0., 0.3333 + ); + CHECK(assert_equal(Al1, Al1_exp, 1e-4)); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/cpp/testVector.cpp b/cpp/testVector.cpp index 2274f260d..488e16595 100644 --- a/cpp/testVector.cpp +++ b/cpp/testVector.cpp @@ -148,6 +148,50 @@ TEST( TestVector, whouse_solve ) 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) = -4.0/3.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.3333, 0., 0.6667, 0.); + CHECK(assert_equal(pseudo1, expected, 1e-4)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */