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

A little better debugging message when backSubstituting a singular matrix

release/4.3a0
Richard Roberts 2010-07-16 17:11:40 +00:00
parent f0b424a3d1
commit ec016e7668
1 changed files with 18 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
base/Matrix.cpp
View File

@ -722,8 +722,12 @@ Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit) {
for (size_t j = i+1; j <= n; j++)
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");
if(!unit && fabs(U(i-1,i-1)) <= numeric_limits<double>::epsilon()) {
stringstream ss;
ss << "backSubstituteUpper: U is singular,\n";
print(U, "U: ", ss);
throw invalid_argument(ss.str());
}
#endif
if (!unit) zi /= U(i-1,i-1);
result(i-1) = zi;
@ -747,8 +751,12 @@ Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit) {
for (size_t j = 1; j < i; j++)
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");
if(!unit && fabs(U(i-1,i-1)) <= numeric_limits<double>::epsilon()) {
stringstream ss;
ss << "backSubstituteUpper: U is singular,\n";
print(U, "U: ", ss);
throw invalid_argument(ss.str());
}
#endif
if (!unit) zi /= U(i-1,i-1);
result(i-1) = zi;
@ -772,8 +780,12 @@ Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit) {
for (size_t j = 1; j < i; j++)
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");
if(!unit && fabs(L(i-1,i-1)) <= numeric_limits<double>::epsilon()) {
stringstream ss;
ss << "backSubstituteUpper: L is singular,\n";
print(L, "L: ", ss);
throw invalid_argument(ss.str());
}
#endif
if (!unit) zi /= L(i-1,i-1);
result(i-1) = zi;