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

Fixed flaw in pseudoinverse calculation, and updated tests for Matrix and Vector

release/4.3a0
Alex Cunningham 2009-10-29 20:12:07 +00:00
parent 80b162a412
commit 65b949c008
3 changed files with 10 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cpp/Vector.cpp
View File

@ -188,17 +188,12 @@ namespace gtsam {
Vector whouse_solve(const Vector& v, const Vector& precisions) {
if (v.size() != precisions.size())
throw invalid_argument("V and precisions have different sizes!");
// get the square root of the precisions
//FIXME: this probably means that storing precisions is a bad idea
Vector D(precisions.size());
for(int i=0;i<D.size();i++)
D(i)=sqrt(precisions[i]);
double normV = 0;
for(int i = 0; i<v.size(); i++)
normV += v[i]*v[i]*D[i];
normV += v[i]*v[i]*precisions[i];
Vector sol(v.size());
for(int i = 0; i<v.size(); i++)
sol[i] = D[i]*v[i];
sol[i] = precisions[i]*v[i];
return sol/normV;
}

51
cpp/testMatrix.cpp
View File

@ -476,56 +476,17 @@ TEST( matrix, whouse_subs )
// 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) = -2.0/3.0;
exp(1,0) = 0.0; exp(1,1) = -1.0;
// verify
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
@ -564,7 +525,7 @@ TEST( matrix, whouse_subs_multistep )
// l1
1., 0.,
0., 1.,
0.3333, 0.,
0.2, 0.,
0., 0.
);
CHECK(assert_equal(Al1, Al1_exp, 1e-4));
@ -589,12 +550,10 @@ TEST( matrix, whouse_subs_multistep )
Al1_exp = Matrix_(4,2,
1., 0.,
0., 1.,
0.3333, 0.,
0., 0.3333
0.2, 0.,
0., 0.2
);
CHECK(assert_equal(Al1, Al1_exp, 1e-4));
CHECK(assert_equal(Al1, Al1_exp, 1e-4)); //fails
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

6
cpp/testVector.cpp
View File

@ -142,7 +142,7 @@ TEST( TestVector, whouse_solve )
// construct expected
Vector exp(2);
exp(0) = 1.0/3.0; exp(1) = 1.0/3.0;
exp(0) = 0.5; exp(1) = 0.25;
// verify
CHECK(assert_equal(act, exp));
@ -171,7 +171,7 @@ TEST( TestVector, whouse_subs_vector )
// create expected value
Vector exp(2);
exp(0) = 5; exp(1) = -4.0/3.0;
exp(0) = 5; exp(1) = -2.0;
// verify
CHECK(assert_equal(b, exp, 1e-5));
@ -188,7 +188,7 @@ TEST( TestVector, whouse_subs_vector2 )
Vector tau = Vector_(4, tau1, tau1, tau2, tau2);
Vector pseudo1 = whouse_solve(a1, tau);
Vector expected = Vector_(4,-0.3333, 0., 0.6667, 0.);
Vector expected = Vector_(4,-0.2, 0., 0.8, 0.);
CHECK(assert_equal(pseudo1, expected, 1e-4));
}