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

fix pending HH problem

release/4.3a0
Kai Ni 2010-10-31 19:56:50 +00:00
parent 69760f7a4c
commit 0e022b49b4
1 changed files with 31 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
gtsam/base/DenseQR.cpp
View File

@ -45,6 +45,17 @@ namespace gtsam {
} }
} }
/* ************************************************************************* */
// check Inf in the input matrix
void CheckInf(int m, int n, double *A, const char* msg) {
bool hasNaN = false;
for(int i=0; i<m; i++) {
for(int j=0; j<n; j++)
if (isinf(A[j*m+i]))
throw std::invalid_argument(msg);
}
}
/* ************************************************************************* */ /* ************************************************************************* */
// remove NaN in the input matrix // remove NaN in the input matrix
void RemoveNaN(int m, int n, double *A) { void RemoveNaN(int m, int n, double *A) {
@ -68,6 +79,18 @@ namespace gtsam {
return false; return false;
} }
/* ************************************************************************* */
// check Inf in the input matrix
bool HasInf(int m, int n, double *A) {
bool hasNaN = false;
for(int i=0; i<m; i++) {
for(int j=0; j<n; j++)
if (isinf(A[j*m+i]))
return true;
}
return false;
}
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* the wrapper for LAPACK dlarft_ and dlarfb_ function * the wrapper for LAPACK dlarft_ and dlarfb_ function
@ -113,6 +136,7 @@ namespace gtsam {
// WriteInput(m, n, numPivotColumns, A, stairs); // WriteInput(m, n, numPivotColumns, A, stairs);
// CheckNaN(m, n, A, "DenseQR: the input matrix has NaN"); // CheckNaN(m, n, A, "DenseQR: the input matrix has NaN");
// CheckInf(m, n, A, "DenseQR: the input matrix has Inf");
if (A == NULL) throw std::invalid_argument("DenseQR: A == NULL!"); if (A == NULL) throw std::invalid_argument("DenseQR: A == NULL!");
if (stairs == NULL) throw std::invalid_argument("DenseQR: stairs == NULL!"); if (stairs == NULL) throw std::invalid_argument("DenseQR: stairs == NULL!");
@ -141,7 +165,8 @@ namespace gtsam {
// if there is a enough large staircase change, apply Householder // if there is a enough large staircase change, apply Householder
stairStartLast = stairStart; stairStartLast = stairStart;
stairs[col] = stairStart = max(numGoodHHs + 1, stairs[col]); stairStart = max(numGoodHHs + 1, stairs[col]);
stairs[col] = stairStart;
numZeros += numPendingHHs * (stairStart - stairStartLast); numZeros += numPendingHHs * (stairStart - stairStartLast);
if (numPendingHHs >= minSizeBlock && colPendingHHEnd < n && if (numPendingHHs >= minSizeBlock && colPendingHHEnd < n &&
numZeros > max(16, ((numPendingHHs * (numPendingHHs + 1)) / 2 + numPendingHHs * (stairStart - row1stHH - numPendingHHs)) / 2)) { numZeros > max(16, ((numPendingHHs * (numPendingHHs + 1)) / 2 + numPendingHHs * (stairStart - row1stHH - numPendingHHs)) / 2)) {
@ -155,12 +180,12 @@ namespace gtsam {
// compute Householder for the current column // compute Householder for the current column
int n_ = stairStart - numGoodHHs; int n_ = stairStart - numGoodHHs;
double *X = &A[numGoodHHs+col*m]; double *X = A + numGoodHHs + col*m;
dlarfg_(&n_, X, X + 1, &one, &tau); dlarfg_(&n_, X, X + 1, &one, &tau);
Tau[col] = tau; Tau[col] = tau;
if (!numPendingHHs) { if (!numPendingHHs) {
row1stHH = numGoodHHs; row1stHH = numGoodHHs;
vectorHH = &A[row1stHH+col*m]; vectorHH = A + row1stHH + col*m;
colPendingHHStart = col; colPendingHHStart = col;
#ifdef DEBUG_MEMORY #ifdef DEBUG_MEMORY
if (row1stHH+col*m >= m*n) throw std::runtime_error("DenseQR: row1stHH+col*m >= m*n"); if (row1stHH+col*m >= m*n) throw std::runtime_error("DenseQR: row1stHH+col*m >= m*n");
@ -177,8 +202,8 @@ namespace gtsam {
*A1 = 1 ; *A1 = 1 ;
dlarf_ (&left, &m1, &n1, A1, &one, &tau, C1, &m, workspace) ; dlarf_ (&left, &m1, &n1, A1, &one, &tau, C1, &m, workspace) ;
*A1 = v0; *A1 = v0;
numGoodHHs++;
} }
numGoodHHs++;
if ((numGoodHHs == m || col + 1 == colPendingHHEnd) && colPendingHHEnd < n) { if ((numGoodHHs == m || col + 1 == colPendingHHEnd) && colPendingHHEnd < n) {
#ifdef DEBUG_MEMORY #ifdef DEBUG_MEMORY
@ -191,6 +216,7 @@ namespace gtsam {
} }
// CheckNaN(m, n, A, "DenseQR: the output matrix has NaN"); // CheckNaN(m, n, A, "DenseQR: the output matrix has NaN");
} // CheckInf(m, n, A, "DenseQR: the input matrix has Inf");
}
} // namespace gtsam } // namespace gtsam