Vector and Matrix updated with improved weighted householder operations and more tests.
Alex Cunningham
parent
a513ae0287
commit
4c48bb08e1
44
.cproject
44
.cproject
|
|
@ -300,6 +300,7 @@
|
|||
<buildTargets>
|
||||
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -307,6 +308,7 @@
|
|||
</target>
|
||||
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -314,7 +316,6 @@
|
|||
</target>
|
||||
<target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -322,6 +323,7 @@
|
|||
</target>
|
||||
<target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimpleCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -337,7 +339,6 @@
|
|||
</target>
|
||||
<target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVSLAMFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -345,6 +346,7 @@
|
|||
</target>
|
||||
<target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testCalibratedCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -352,7 +354,6 @@
|
|||
</target>
|
||||
<target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testConditionalGaussian.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -360,6 +361,7 @@
|
|||
</target>
|
||||
<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -375,6 +377,7 @@
|
|||
</target>
|
||||
<target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -382,7 +385,6 @@
|
|||
</target>
|
||||
<target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearOptimizer.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -390,6 +392,7 @@
|
|||
</target>
|
||||
<target name="testLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testLinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -397,6 +400,7 @@
|
|||
</target>
|
||||
<target name="testConstrainedNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testConstrainedNonlinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -404,6 +408,7 @@
|
|||
</target>
|
||||
<target name="testLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testLinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -411,7 +416,6 @@
|
|||
</target>
|
||||
<target name="testNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -419,6 +423,7 @@
|
|||
</target>
|
||||
<target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -426,7 +431,6 @@
|
|||
</target>
|
||||
<target name="testConstrainedLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testConstrainedLinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -434,7 +438,6 @@
|
|||
</target>
|
||||
<target name="testVectorConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVectorConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -442,7 +445,6 @@
|
|||
</target>
|
||||
<target name="testPoint2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPoint2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -450,6 +452,7 @@
|
|||
</target>
|
||||
<target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -457,6 +460,7 @@
|
|||
</target>
|
||||
<target name="timeLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>timeLinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -464,6 +468,7 @@
|
|||
</target>
|
||||
<target name="timeLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>timeLinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -471,6 +476,7 @@
|
|||
</target>
|
||||
<target name="testChordalBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testChordalBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -478,7 +484,6 @@
|
|||
</target>
|
||||
<target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testBayesTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -486,7 +491,6 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesChain.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesChain.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -494,15 +498,29 @@
|
|||
</target>
|
||||
<target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVector.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testMatrix.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testMatrix.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -510,7 +528,6 @@
|
|||
</target>
|
||||
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -518,7 +535,6 @@
|
|||
</target>
|
||||
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
|
|||
|
|
@ -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<m; ++i)
|
||||
for (int j=0; j<m; ++j)
|
||||
Hw(i,j) -= x(i)*pseudo(j);
|
||||
// calculate product = pseudo*A
|
||||
Vector product = zero(n);
|
||||
for (int j=0; j<n; ++j) // for each column in A
|
||||
for (int i=row;i<m;++i) {// only the relevant section of A
|
||||
product(j) += A(i,j)*pseudo(i-row);
|
||||
}
|
||||
|
||||
// zero the selected column below the diagonal
|
||||
for (int i=row+1; i<m; ++i)
|
||||
A(i,row) = 0.0;
|
||||
|
||||
// update As
|
||||
//FIXME: this uselessly updates the top rows as well, and should be fixed
|
||||
Matrix As = sub(A,0,m,row+1,n);
|
||||
Matrix As_new = Hw*As;
|
||||
|
||||
// copy in updated As
|
||||
for (int i=1; i<m; ++i) //index through rows of A
|
||||
for (int j=0; j<As_new.size2(); ++j) //index through columns of As_new
|
||||
A(i,j+row+1) = As_new(i,j);
|
||||
// calculate A -= a*product
|
||||
for (int i=row+1;i<m;++i) // limit only to rows of A being updated
|
||||
for (int j=0;j<n;++j) { // update all columns
|
||||
A(i,j) -= product(j)*a(i-row);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -142,10 +142,10 @@ void householder_update(Matrix &A, int j, double beta, const Vector& vjm);
|
|||
* @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 x is the first column of A
|
||||
* A is updated into non-normalied R of A that has been updated
|
||||
* @param a is the first column of A
|
||||
* A is updated into non-normalized R of A that has been updated
|
||||
*/
|
||||
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);
|
||||
|
||||
/**
|
||||
* Householder tranformation, Householder vectors below diagonal
|
||||
|
|
|
|||
|
|
@ -202,6 +202,22 @@ namespace gtsam {
|
|||
return sol/normV;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void whouse_subs(Vector& b, size_t row, const Vector& a, const Vector& pseudo) {
|
||||
// get sizes
|
||||
size_t m = b.size();
|
||||
|
||||
// calculate product = pseudo*b
|
||||
double product = 0;
|
||||
for (int i=row;i<m;++i) {// only the relevant section of b
|
||||
product += b(i)*pseudo(i-row);
|
||||
}
|
||||
|
||||
// calculate b -= a*product
|
||||
for (int i=row+1;i<m;++i)
|
||||
b(i) -= a(i-row)*product;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector concatVectors(size_t nrVectors, ...)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -112,6 +112,15 @@ std::pair<double,Vector> 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
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue