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

Added debugging check for singular matrices in backSubstituteUpper and backSubstituteLower

release/4.3a0
Richard Roberts 2010-07-12 20:19:52 +00:00
parent 9cfe5d780b
commit 3d9ac5094b
1 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
base/Matrix.cpp
View File

@ -9,6 +9,7 @@
#include <iomanip> #include <iomanip>
#include <list> #include <list>
#include <fstream> #include <fstream>
#include <limits>
#ifdef GT_USE_CBLAS #ifdef GT_USE_CBLAS
extern "C" { extern "C" {
@ -720,6 +721,10 @@ Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit) {
double zi = 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++)
zi -= U(i-1,j-1) * result(j-1); zi -= U(i-1,j-1) * result(j-1);
#ifndef NDEBUG
if(!unit && fabs(U(i-1,i-1)) <= numeric_limits<double>::epsilon())
throw invalid_argument("backSubstituteUpper: U is singular");
#endif
if (!unit) zi /= U(i-1,i-1); if (!unit) zi /= U(i-1,i-1);
result(i-1) = zi; result(i-1) = zi;
} }
@ -741,6 +746,10 @@ Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit) {
double zi = b(i-1); double zi = b(i-1);
for (size_t j = 1; j < i; j++) for (size_t j = 1; j < i; j++)
zi -= U(j-1,i-1) * result(j-1); zi -= U(j-1,i-1) * result(j-1);
#ifndef NDEBUG
if(!unit && fabs(U(i-1,i-1)) <= numeric_limits<double>::epsilon())
throw invalid_argument("backSubstituteUpper: U is singular");
#endif
if (!unit) zi /= U(i-1,i-1); if (!unit) zi /= U(i-1,i-1);
result(i-1) = zi; result(i-1) = zi;
} }
@ -762,6 +771,10 @@ Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit) {
double zi = b(i-1); double zi = b(i-1);
for (size_t j = 1; j < i; j++) for (size_t j = 1; j < i; j++)
zi -= L(i-1,j-1) * result(j-1); zi -= L(i-1,j-1) * result(j-1);
#ifndef NDEBUG
if(!unit && fabs(L(i-1,i-1)) <= numeric_limits<double>::epsilon())
throw invalid_argument("backSubstituteLower: L is singular");
#endif
if (!unit) zi /= L(i-1,i-1); if (!unit) zi /= L(i-1,i-1);
result(i-1) = zi; result(i-1) = zi;
} }