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

transpose backsubstitute on upper-triangular matrix

release/4.3a0
Frank Dellaert 2009-12-30 13:20:16 +00:00
parent 42fca8c399
commit 15bb00683a
3 changed files with 52 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
cpp/Matrix.cpp
View File

@ -414,11 +414,31 @@ Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit) {
Vector result(n); Vector result(n);
for (size_t i = n; i > 0; i--) { for (size_t i = n; i > 0; i--) {
double tmp = b(i-1); double zi = b(i-1);
for (size_t j = i+1; j <= n; j++) for (size_t j = i+1; j <= n; j++)
tmp -= U(i-1,j-1) * result(j-1); zi -= U(i-1,j-1) * result(j-1);
if (!unit) tmp /= U(i-1,i-1); if (!unit) zi /= U(i-1,i-1);
result(i-1) = tmp; result(i-1) = zi;
}
return result;
}
/* ************************************************************************* */
Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit) {
size_t m = U.size1(), n = U.size2();
#ifndef NDEBUG
if (m!=n)
throw invalid_argument("backSubstituteUpper: U must be square");
#endif
Vector result(n);
for (size_t i = 1; i <= n; i++) {
double zi = b(i-1);
for (size_t j = 1; j < i; j++)
zi -= U(j-1,i-1) * result(j-1);
if (!unit) zi /= U(i-1,i-1);
result(i-1) = zi;
} }
return result; return result;
@ -434,11 +454,11 @@ Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit) {
Vector result(n); Vector result(n);
for (size_t i = 1; i <= n; i++) { for (size_t i = 1; i <= n; i++) {
double tmp = b(i-1); double zi = b(i-1);
for (size_t j = 1; j < i; j++) for (size_t j = 1; j < i; j++)
tmp -= L(i-1,j-1) * result(j-1); zi -= L(i-1,j-1) * result(j-1);
if (!unit) tmp /= L(i-1,i-1); if (!unit) zi /= L(i-1,i-1);
result(i-1) = tmp; result(i-1) = zi;
} }
return result; return result;

18
cpp/Matrix.h
View File

@ -209,18 +209,28 @@ void householder(Matrix& A, size_t k);
/** /**
* backSubstitute U*x=b * backSubstitute U*x=b
* @param U an upper triangular matrix * @param U an upper triangular matrix
* @param b a RHS vector * @param b an RHS vector
* @param unit, set tru if unit triangular * @param unit, set true if unit triangular
* @return the solution x of U*x=b * @return the solution x of U*x=b
* TODO: use boost * TODO: use boost
*/ */
Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit=false); Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit=false);
/**
* backSubstitute x'*U=b'
* @param U an upper triangular matrix
* @param b an RHS vector
* @param unit, set true if unit triangular
* @return the solution x of x'*U=b'
* TODO: use boost
*/
Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit=false);
/** /**
* backSubstitute L*x=b * backSubstitute L*x=b
* @param L an lower triangular matrix * @param L an lower triangular matrix
* @param b a RHS vector * @param b an RHS vector
* @param unit, set tru if unit triangular * @param unit, set true if unit triangular
* @return the solution x of L*x=b * @return the solution x of L*x=b
* TODO: use boost * TODO: use boost
*/ */

20
cpp/testMatrix.cpp
View File

@ -391,29 +391,29 @@ TEST( matrix, backsubtitution )
{ {
// TEST ONE 2x2 matrix U1*x=b1 // TEST ONE 2x2 matrix U1*x=b1
Vector expected1 = Vector_(2, 3.6250, -0.75); Vector expected1 = Vector_(2, 3.6250, -0.75);
Matrix U1 = Matrix_(2, 2, Matrix U22 = Matrix_(2, 2,
2., 3., 2., 3.,
0., 4.); 0., 4.);
Vector b1 = U1*expected1; Vector b1 = U22*expected1;
CHECK( assert_equal(expected1 , backSubstituteUpper(U1, b1), 0.000001)); CHECK( assert_equal(expected1 , backSubstituteUpper(U22, b1), 0.000001));
// TEST TWO 3x3 matrix U2*x=b2 // TEST TWO 3x3 matrix U2*x=b2
Vector expected2 = Vector_(3, 5.5, -8.5, 5.); Vector expected2 = Vector_(3, 5.5, -8.5, 5.);
Matrix U2 = Matrix_(3, 3, Matrix U33 = Matrix_(3, 3,
3., 5., 6., 3., 5., 6.,
0., 2., 3., 0., 2., 3.,
0., 0., 1.); 0., 0., 1.);
Vector b2 = U2*expected2; Vector b2 = U33*expected2;
CHECK( assert_equal(expected2 , backSubstituteUpper(U2, b2), 0.000001)); CHECK( assert_equal(expected2 , backSubstituteUpper(U33, b2), 0.000001));
// TEST THREE Lower triangular 3x3 matrix L3*x=b3 // TEST THREE Lower triangular 3x3 matrix L3*x=b3
Vector expected3 = Vector_(3, 1., 1., 1.); Vector expected3 = Vector_(3, 1., 1., 1.);
Matrix L3 = Matrix_(3, 3, Matrix L3 = trans(U33);
3., 0., 0.,
5., 2., 0.,
6., 3., 1.);
Vector b3 = L3*expected3; Vector b3 = L3*expected3;
CHECK( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001)); CHECK( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001));
// TEST FOUR Try the above with transpose backSubstituteUpper
CHECK( assert_equal(expected3 , backSubstituteUpper(b3,U33), 0.000001));
} }
/* ************************************************************************* */ /* ************************************************************************* */