diff --git a/.cproject b/.cproject index 88600176d..75845dc2c 100644 --- a/.cproject +++ b/.cproject @@ -322,6 +322,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -348,7 +356,6 @@ make - tests/testBayesTree.run true false @@ -356,7 +363,6 @@ make - testBinaryBayesNet.run true false @@ -404,7 +410,6 @@ make - testSymbolicBayesNet.run true false @@ -412,7 +417,6 @@ make - tests/testSymbolicFactor.run true false @@ -420,7 +424,6 @@ make - testSymbolicFactorGraph.run true false @@ -436,20 +439,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -484,6 +478,7 @@ make + testGraph.run true false @@ -579,6 +574,7 @@ make + testInference.run true false @@ -586,6 +582,7 @@ make + testGaussianBayesNet.run true false @@ -593,6 +590,7 @@ make + testGaussianFactor.run true false @@ -600,6 +598,7 @@ make + testJunctionTree.run true false @@ -607,6 +606,7 @@ make + testSymbolicBayesNet.run true false @@ -614,6 +614,7 @@ make + testSymbolicFactorGraph.run true false @@ -829,7 +830,6 @@ make - testGaussianISAM.run true true @@ -837,6 +837,7 @@ make + -j2 testSymbolicFactorGraph.run true true @@ -940,7 +941,6 @@ make - testErrors.run true false @@ -978,6 +978,14 @@ true true + + make + -j2 + tests/timeSLAMlike.run + true + true + true + make -j2 @@ -1092,6 +1100,7 @@ make + -j2 tests/testEliminationTree.run true true @@ -1115,6 +1124,7 @@ make + -j2 tests/testConditional.run true true @@ -1122,7 +1132,6 @@ make - tests/testSymbolicFactorGraph.run true true @@ -1290,6 +1299,7 @@ make + testSimulated2DOriented.run true false @@ -1329,6 +1339,7 @@ make + testSimulated2D.run true false @@ -1336,6 +1347,7 @@ make + testSimulated3D.run true false @@ -1391,6 +1403,7 @@ make + tests/testGaussianISAM2 true false @@ -1412,86 +1425,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - inference/tests/testEliminationTree - true - true - true - - - make - -j2 - slam/tests/testGaussianISAM2 - true - true - true - - - make - -j2 - inference/tests/testVariableIndex - true - true - true - - - make - -j2 - inference/tests/testJunctionTree - true - true - true - - - make - -j2 - linear/tests/testGaussianJunctionTree - true - true - true - make -j2 @@ -1588,6 +1521,94 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + inference/tests/testEliminationTree + true + true + true + + + make + -j2 + slam/tests/testGaussianISAM2 + true + true + true + + + make + -j2 + inference/tests/testVariableIndex + true + true + true + + + make + -j2 + inference/tests/testJunctionTree + true + true + true + + + make + -j2 + linear/tests/testGaussianJunctionTree + true + true + true + + + make + -j2 + check + true + true + true + make -j2 @@ -1620,13 +1641,6 @@ true true - - make - check - true - true - true - @@ -1949,6 +1963,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1975,7 +1997,6 @@ make - tests/testBayesTree.run true false @@ -1983,7 +2004,6 @@ make - testBinaryBayesNet.run true false @@ -2031,7 +2051,6 @@ make - testSymbolicBayesNet.run true false @@ -2039,7 +2058,6 @@ make - tests/testSymbolicFactor.run true false @@ -2047,7 +2065,6 @@ make - testSymbolicFactorGraph.run true false @@ -2063,20 +2080,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -2111,6 +2119,7 @@ make + testGraph.run true false @@ -2206,6 +2215,7 @@ make + testInference.run true false @@ -2213,6 +2223,7 @@ make + testGaussianBayesNet.run true false @@ -2220,6 +2231,7 @@ make + testGaussianFactor.run true false @@ -2227,6 +2239,7 @@ make + testJunctionTree.run true false @@ -2234,6 +2247,7 @@ make + testSymbolicBayesNet.run true false @@ -2241,6 +2255,7 @@ make + testSymbolicFactorGraph.run true false @@ -2456,7 +2471,6 @@ make - testGaussianISAM.run true true @@ -2464,6 +2478,7 @@ make + -j2 testSymbolicFactorGraph.run true true @@ -2567,7 +2582,6 @@ make - testErrors.run true false @@ -2605,6 +2619,14 @@ true true + + make + -j2 + tests/timeSLAMlike.run + true + true + true + make -j2 @@ -2719,6 +2741,7 @@ make + -j2 tests/testEliminationTree.run true true @@ -2742,6 +2765,7 @@ make + -j2 tests/testConditional.run true true @@ -2749,7 +2773,6 @@ make - tests/testSymbolicFactorGraph.run true true @@ -2917,6 +2940,7 @@ make + testSimulated2DOriented.run true false @@ -2956,6 +2980,7 @@ make + testSimulated2D.run true false @@ -2963,6 +2988,7 @@ make + testSimulated3D.run true false @@ -3018,6 +3044,7 @@ make + tests/testGaussianISAM2 true false @@ -3039,86 +3066,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - inference/tests/testEliminationTree - true - true - true - - - make - -j2 - slam/tests/testGaussianISAM2 - true - true - true - - - make - -j2 - inference/tests/testVariableIndex - true - true - true - - - make - -j2 - inference/tests/testJunctionTree - true - true - true - - - make - -j2 - linear/tests/testGaussianJunctionTree - true - true - true - make -j2 @@ -3215,6 +3162,94 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + inference/tests/testEliminationTree + true + true + true + + + make + -j2 + slam/tests/testGaussianISAM2 + true + true + true + + + make + -j2 + inference/tests/testVariableIndex + true + true + true + + + make + -j2 + inference/tests/testJunctionTree + true + true + true + + + make + -j2 + linear/tests/testGaussianJunctionTree + true + true + true + + + make + -j2 + check + true + true + true + make -j2 @@ -3247,13 +3282,6 @@ true true - - make - check - true - true - true - diff --git a/base/DenseQR.cpp b/base/DenseQR.cpp index 166d36685..475083979 100644 --- a/base/DenseQR.cpp +++ b/base/DenseQR.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include "DenseQR.h" @@ -34,13 +35,15 @@ namespace gtsam { /** * the wrapper for LAPACK dlarft_ and dlarfb_ function */ - inline void dlarftb_wrap(int m, int n, int k, int ldv, double *V, double *Tau, double *C, double *W, + inline void dlarftb_wrap(int m, int n, int k, int ldc, int ldv, double *V, double *Tau, double *C, double *W, int* numPendingHHs, int* numZeros) { + if (m <= 0 || n <= 0 || k <= 0) return ; + if (m < k) throw std::invalid_argument("dlarftb_wrap: m < k!"); dlarft_(&direct, &storev, &m, &k, V, &ldv, Tau, W, &k) ; dlarfb_(&left, &trans, &direct, &storev, &m, &n, &k, V, &ldv, - W, &k, C, &ldv, W + k*k, &n); + W, &k, C, &ldc, W + k*k, &n); *numPendingHHs = 0; *numZeros = 0; } @@ -56,16 +59,15 @@ namespace gtsam { /* ************************************************************************* */ void DenseQR(int m, int n, int numPivotColumns, double *A, int *stairs, double *workspace) { - if (A == NULL) throw std::invalid_argument("DenseQR: A == NULL!"); if (stairs == NULL) throw std::invalid_argument("DenseQR: stairs == NULL!"); if (workspace == NULL) throw std::invalid_argument("DenseQR: W == NULL!"); if (m < 0) throw std::invalid_argument("DenseQR: m < 0"); if (n < 0) throw std::invalid_argument("DenseQR: n < 0"); - if (numPivotColumns < 0l || numPivotColumns > n) + if (numPivotColumns < 0 || numPivotColumns > n) throw std::invalid_argument("DenseQR: numPivotColumns < 0l || numPivotColumns > n"); - double tau; // the scalar in Householder + double tau[n]; // the scalar in Householder int row1stHH = 0, numGoodHHs = 0, numPendingHHs = 0; int colPendingHHEnd = 0; double *vectorHH = A; @@ -88,13 +90,13 @@ namespace gtsam { numZeros += numPendingHHs * (stairStart - stairStartLast); if (numPendingHHs >= minSizeBlock && numZeros > max(16, ((numPendingHHs * (numPendingHHs + 1)) / 2 + numPendingHHs * (stairStart - row1stHH - numPendingHHs)) / 2)) - dlarftb_wrap(stairStartLast - row1stHH, n - colPendingHHEnd, numPendingHHs, m, - vectorHH, &tau, &A[row1stHH+colPendingHHEnd*m], workspace, &numPendingHHs, &numZeros); + dlarftb_wrap(stairStartLast - row1stHH, n - colPendingHHEnd, numPendingHHs, m, m, + vectorHH, tau, &A[row1stHH+colPendingHHEnd*m], workspace, &numPendingHHs, &numZeros); // compute Householder for the current column int n_ = stairStart - numGoodHHs; double *X = &A[numGoodHHs+col*m]; - dlarfg_(&n_, X, X + 1, &one, &tau); + dlarfg_(&n_, X, X + 1, &one, tau); if (!numPendingHHs) { row1stHH = numGoodHHs; vectorHH = &A[row1stHH+col*m]; @@ -103,16 +105,18 @@ namespace gtsam { numPendingHHs++; // apply Householder reflector - double *A1 = &A[numGoodHHs+col*m], *C1 = A1 + m, v0 = *A1; - *A1 = 1 ; m1 = stairStart - numGoodHHs; n1 = colPendingHHEnd - col - 1; - dlarf_ (&left, &m1, &n1, A1, &one, &tau, C1, &m, workspace) ; - *A1 = v0; - numGoodHHs++; + if (m1 > 0 && n1 > 0) { + double *A1 = &A[numGoodHHs+col*m], *C1 = A1 + m, v0 = *A1; + *A1 = 1 ; + dlarf_ (&left, &m1, &n1, A1, &one, tau, C1, &m, workspace) ; + *A1 = v0; + numGoodHHs++; + } if (numGoodHHs == m || col + 1 == colPendingHHEnd) - dlarftb_wrap(stairStart - row1stHH, n - colPendingHHEnd, numPendingHHs, m, vectorHH, &tau, &A[row1stHH+(colPendingHHEnd*m)], workspace, + dlarftb_wrap(stairStart - row1stHH, n - colPendingHHEnd, numPendingHHs, m, m, vectorHH, tau, &A[row1stHH+colPendingHHEnd*m], workspace, &numPendingHHs, &numZeros); } } diff --git a/base/DenseQRUtil.cpp b/base/DenseQRUtil.cpp index d6634cee3..249f4dfe0 100644 --- a/base/DenseQRUtil.cpp +++ b/base/DenseQRUtil.cpp @@ -163,7 +163,6 @@ namespace gtsam { double W[b*(n+b)]; DenseQR(m, n, npiv, A.data().begin(), Stair, W); toc("householder_denseqr: denseqr_front"); - } } // namespace gtsam diff --git a/linear/tests/timeSLAMlike.cpp b/linear/tests/timeSLAMlike.cpp index 4fd31b3f2..54cef472b 100644 --- a/linear/tests/timeSLAMlike.cpp +++ b/linear/tests/timeSLAMlike.cpp @@ -18,8 +18,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -119,8 +119,7 @@ int main(int argc, char *argv[]) { timer.restart(); for(size_t trial=0; trialeliminate()); - VectorValues soln(optimize(*gbn)); + VectorValues soln(*GaussianMultifrontalSolver(blockGfgs[trial]).optimize()); } blocksolve = timer.elapsed(); cout << blocksolve << " s" << endl;