diff --git a/.cproject b/.cproject
index ef7985e39..ed2d41ca6 100644
--- a/.cproject
+++ b/.cproject
@@ -5,46 +5,47 @@
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -298,573 +299,589 @@
-
-
-make
--j2
-install
-true
-true
-true
-
-
-make
--j2
-check
-true
-true
-true
-
-
-make
-
-clean
-true
-true
-true
-
-
-make
--k
-check
-true
-false
-true
-
-
-make
--j2
-testSimpleCamera.run
-true
-true
-true
-
-
-make
--f local.mk
-testCal3_S2.run
-true
-true
-true
-
-
-make
--j2
-testVSLAMFactor.run
-true
-true
-true
-
-
-make
--j2
-testCalibratedCamera.run
-true
-true
-true
-
-
-make
--j2
-testGaussianConditional.run
-true
-true
-true
-
-
-make
--j2
-testPose2.run
-true
-true
-true
-
-
-make
--j2
-testRot3.run
-true
-true
-true
-
-
-make
--j2
-testNonlinearOptimizer.run
-true
-true
-true
-
-
-make
--j2
-testGaussianFactor.run
-true
-true
-true
-
-
-make
--j2
-testGaussianFactorGraph.run
-true
-true
-true
-
-
-make
--j2
-testNonlinearFactorGraph.run
-true
-true
-true
-
-
-make
--j2
-testPose3.run
-true
-true
-true
-
-
-make
--j2
-testVectorMap.run
-true
-true
-true
-
-
-make
--j2
-testPoint2.run
-true
-true
-true
-
-
-make
--j2
-testNonlinearFactor.run
-true
-true
-true
-
-
-make
--j2
-timeGaussianFactor.run
-true
-true
-true
-
-
-make
--j2
-timeGaussianFactorGraph.run
-true
-true
-true
-
-
-make
--j2
-testGaussianBayesNet.run
-true
-true
-true
-
-
-make
-testBayesTree.run
-true
-false
-true
-
-
-make
-
-testSymbolicBayesNet.run
-true
-false
-true
-
-
-make
-testSymbolicFactorGraph.run
-true
-false
-true
-
-
-make
--j2
-testVector.run
-true
-true
-true
-
-
-make
--j2
-testMatrix.run
-true
-true
-true
-
-
-make
--j2
-testVSLAMGraph.run
-true
-true
-true
-
-
-make
--j2
-testNonlinearEquality.run
-true
-true
-true
-
-
-make
--j2
-testSQP.run
-true
-true
-true
-
-
-make
--j2
-testNonlinearConstraint.run
-true
-true
-true
-
-
-make
--j2
-testVSLAMConfig.run
-true
-true
-true
-
-
-make
--j2
-testOrdering.run
-true
-true
-true
-
-
-make
--j2
-testRot2.run
-true
-true
-true
-
-
-make
--j2
-testGaussianBayesTree.run
-true
-true
-true
-
-
-make
--j2
-testPose3Config.run
-true
-true
-true
-
-
-make
--j2
-testUrbanMeasurement.run
-true
-true
-true
-
-
-make
--j2
-testUrbanOdometry.run
-true
-true
-true
-
-
-make
--j2
-testIterative.run
-true
-true
-true
-
-
-make
--j2
-testGaussianISAM2.run
-true
-true
-true
-
-
-make
--j2
-testSubgraphPreconditioner.run
-true
-true
-true
-
-
-make
--j2
-testBayesNetPreconditioner.run
-true
-true
-true
-
-
-make
--j2
-testPose2Factor.run
-true
-true
-true
-
-
-make
--j2
-timeRot3.run
-true
-true
-true
-
-
-make
--j2
-testPose2SLAM.run
-true
-true
-true
-
-
-make
--j2
-testPose2Config.run
-true
-true
-true
-
-
-make
--j2
-testLieConfig.run
-true
-true
-true
-
-
-make
--j2
-testPlanarSLAM.run
-true
-true
-true
-
-
-make
-testGraph.run
-true
-false
-true
-
-
-make
--j2
-testPose3SLAM.run
-true
-true
-true
-
-
-make
--j2
-testTupleConfig.run
-true
-true
-true
-
-
-make
--j2
-testPose2Prior.run
-true
-true
-true
-
-
-make
--j2
-testNoiseModel.run
-true
-true
-true
-
-
-make
--j2
-testISAM.run
-true
-true
-true
-
-
-make
--j2
-testGaussianISAM.run
-true
-true
-true
-
-
-make
-
-testSimulated2D.run
-true
-false
-true
-
-
-make
--j2
-timeMatrix.run
-true
-true
-true
-
-
-make
--j2
-testKey.run
-true
-true
-true
-
-
-make
--j2
-timeVectorConfig.run
-true
-true
-true
-
-
-make
--j2
-testHomography2.run
-true
-true
-true
-
-
-make
--j2
-testVectorBTree.run
-true
-true
-true
-
-
-make
-testErrors.run
-true
-false
-true
-
-
-make
-testDSF.run
-true
-true
-true
-
-
-make
--j2
-testFactorGraph.run
-true
-true
-true
-
-
-make
-testConstraintOptimizer.run
-true
-true
-true
-
-
-make
-
-testBTree.run
-true
-true
-true
-
-
-make
-testSimulated2DOriented.run
-true
-false
-true
-
-
-make
-
-testDSFVector.run
-true
-true
-true
-
-
-make
--j2
-install
-true
-true
-true
-
-
-make
--j2
-clean
-true
-true
-true
-
-
-make
--j2
-check
-true
-true
-true
-
-
-
-
+
+
+ make
+ -j2
+ install
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+
+ clean
+ true
+ true
+ true
+
+
+ make
+ -k
+ check
+ true
+ false
+ true
+
+
+ make
+ -j2
+ testSimpleCamera.run
+ true
+ true
+ true
+
+
+ make
+ -f local.mk
+ testCal3_S2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testVSLAMFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testCalibratedCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testGaussianConditional.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testRot3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testNonlinearOptimizer.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testGaussianFactorGraph.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testNonlinearFactorGraph.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testVectorMap.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPoint2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testNonlinearFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ timeGaussianFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ timeGaussianFactorGraph.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testGaussianBayesNet.run
+ true
+ true
+ true
+
+
+ make
+ testBayesTree.run
+ true
+ false
+ true
+
+
+ make
+
+ testSymbolicBayesNet.run
+ true
+ false
+ true
+
+
+ make
+ testSymbolicFactorGraph.run
+ true
+ false
+ true
+
+
+ make
+ -j2
+ testVector.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testMatrix.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testVSLAMGraph.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testNonlinearEquality.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testSQP.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testNonlinearConstraint.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testVSLAMConfig.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testOrdering.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testRot2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testGaussianBayesTree.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose3Config.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testUrbanMeasurement.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testUrbanOdometry.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testIterative.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testGaussianISAM2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testSubgraphPreconditioner.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testBayesNetPreconditioner.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose2Factor.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ timeRot3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose2SLAM.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose2Config.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testLieConfig.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPlanarSLAM.run
+ true
+ true
+ true
+
+
+ make
+ testGraph.run
+ true
+ false
+ true
+
+
+ make
+ -j2
+ testPose3SLAM.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testTupleConfig.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose2Prior.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testNoiseModel.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testISAM.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testGaussianISAM.run
+ true
+ true
+ true
+
+
+ make
+
+ testSimulated2D.run
+ true
+ false
+ true
+
+
+ make
+ -j2
+ timeMatrix.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testKey.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ timeVectorConfig.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testHomography2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testVectorBTree.run
+ true
+ true
+ true
+
+
+ make
+ testErrors.run
+ true
+ false
+ true
+
+
+ make
+ testDSF.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testFactorGraph.run
+ true
+ true
+ true
+
+
+ make
+ testConstraintOptimizer.run
+ true
+ true
+ true
+
+
+ make
+
+ testBTree.run
+ true
+ true
+ true
+
+
+ make
+ testSimulated2DOriented.run
+ true
+ false
+ true
+
+
+ make
+
+ testDSFVector.run
+ true
+ true
+ true
+
+
+ make
+
+ testSPQRUtil.run
+ true
+ true
+ true
+
+
+ make
+
+ testInference.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ install
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+
+
-
-
+
+
diff --git a/Makefile.am b/Makefile.am
index ea66ff12a..cbeb711c3 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -3,7 +3,7 @@ ACLOCAL_AMFLAGS = -I m4
# make automake install some standard but missing files
AUTOMAKE_OPTIONS = foreign
-SUBDIRS = colamd CppUnitLite ldl cpp wrap
+SUBDIRS = colamd CppUnitLite ldl spqr_mini cpp wrap
# install the matlab toolbox
install-exec-hook:
@@ -20,6 +20,10 @@ install-exec-hook:
cp -f ldl/ldl.h $(prefix)/include/ldl/ && \
cp -f ldl/UFconfig.h $(prefix)/include/ldl/ && \
cp -f ldl/libldl.a $(prefix)/lib/
+ install -d $(prefix)/include/spqr_mini && \
+ cp -f spqr_mini/*.h $(prefix)/include/spqr_mini/ && \
+ cp -f spqr_mini/*.hpp $(prefix)/include/spqr_mini/ && \
+ cp -f spqr_mini/libspqr_mini.a $(prefix)/lib/
dist-hook:
mkdir $(distdir)/CppUnitLite
@@ -36,5 +40,10 @@ dist-hook:
cp -p $(srcdir)/ldl/*.h $(distdir)/ldl
cp -p $(srcdir)/ldl/*.c $(distdir)/ldl
cp -p $(srcdir)/ldl/*.cpp $(distdir)/ldl
+ mkdir $(distdir)/spqr_mini
+ cp -p $(srcdir)/spqr_mini/Makefile $(distdir)/spqr_mini
+ cp -p $(srcdir)/spqr_mini/*.h $(distdir)/spqr_mini
+ cp -p $(srcdir)/spqr_mini/*.c $(distdir)/spqr_mini
+ cp -p $(srcdir)/spqr_mini/*.cpp $(distdir)/spqr_mini
mkdir $(distdir)/matlab
cp -p $(srcdir)/matlab/*.m $(distdir)/matlab
diff --git a/configure.ac b/configure.ac
index b5a4f5258..585439326 100644
--- a/configure.ac
+++ b/configure.ac
@@ -4,11 +4,12 @@
AC_PREREQ(2.59)
AC_INIT(gtsam, 0.0.0, dellaert@cc.gatech.edu)
AM_INIT_AUTOMAKE(gtsam, 0.0.0)
-AC_OUTPUT(Makefile cpp/Makefile wrap/Makefile)
+AC_OUTPUT(Makefile spqr_mini/Makefile cpp/Makefile wrap/Makefile)
AC_CONFIG_MACRO_DIR([m4])
AC_CONFIG_SRCDIR([cpp/cal3_S2.cpp])
AC_CONFIG_HEADER([cpp/config.h])
AC_CONFIG_SRCDIR([wrap/wrap.cpp])
+AC_CONFIG_SRCDIR([spqr_mini/spqr_front.cpp])
# Check for OS
AC_CANONICAL_HOST # needs to be called at some point earlier
diff --git a/cpp/GaussianConditional.cpp b/cpp/GaussianConditional.cpp
index 1f929bc27..d9f639f8e 100644
--- a/cpp/GaussianConditional.cpp
+++ b/cpp/GaussianConditional.cpp
@@ -63,28 +63,32 @@ bool GaussianConditional::equals(const Conditional &c, double tol) const {
// check if the size of the parents_ map is the same
if (parents_.size() != p->parents_.size()) return false;
- bool equal_R1 = equal_with_abs_tol(R_, p->R_, tol);
- bool equal_R2 = equal_with_abs_tol(R_*(-1), p->R_, tol);
- bool equal_d1 = ::equal_with_abs_tol(d_, p->d_, tol);
- bool equal_d2 = ::equal_with_abs_tol(d_*(-1), p->d_, tol);
-
// check if R_ and d_ are equal up to a sign
- if (!((equal_R1 && equal_d1) || (equal_R2 && equal_d2))) return false;
+ for (int i=0; iR_, i);
+ if (!((::equal_with_abs_tol(row1, row2, tol) && fabs(d_(i) - p->d_(i)) < tol) ||
+ (::equal_with_abs_tol(row1, row2*(-1), tol) && fabs(d_(i) + p->d_(i)) < tol)))
+ return false;
+
+ float sign = ::equal_with_abs_tol(row1, row2, tol) ? 1 : -1;
+
+ // check if the matrices are the same
+ // iterate over the parents_ map
+ for (it = parents_.begin(); it != parents_.end(); it++) {
+ Parents::const_iterator it2 = p->parents_.find(it->first);
+ if (it2 != p->parents_.end()) {
+ if (!::equal_with_abs_tol(row_(it->second*sign, i), row_(it2->second,i), tol))
+ return false;
+ } else
+ return false;
+ }
+
+ }
// check if sigmas are equal
if (!(::equal_with_abs_tol(sigmas_, p->sigmas_, tol))) return false;
- // check if the matrices are the same
- // iterate over the parents_ map
- for (it = parents_.begin(); it != parents_.end(); it++) {
- Parents::const_iterator it2 = p->parents_.find(it->first);
- if (it2 != p->parents_.end()) {
- if (!(equal_with_abs_tol(it->second, it2->second, tol)) &&
- !(equal_with_abs_tol(it->second*(-1), it2->second, tol)))
- return false;
- } else
- return false;
- }
return true;
}
diff --git a/cpp/GaussianFactor.cpp b/cpp/GaussianFactor.cpp
index f77f93412..b92d45fde 100644
--- a/cpp/GaussianFactor.cpp
+++ b/cpp/GaussianFactor.cpp
@@ -166,16 +166,29 @@ bool GaussianFactor::equals(const Factor& f, double tol) const {
const_iterator it1 = As_.begin(), it2 = lf->As_.begin();
if(As_.size() != lf->As_.size()) return false;
- for(; it1 != As_.end(); it1++, it2++) {
- const Symbol& j1 = it1->first, j2 = it2->first;
- const Matrix A1 = it1->second, A2 = it2->second;
- if (j1 != j2) return false;
- if (!equal_with_abs_tol(A1,A2,tol))
- return false;
- }
+ // check whether each row is up to a sign
+ for (int i=0; i row1;
+ list row2;
+ row1.push_back(Vector_(1, b_(i)));
+ row2.push_back(Vector_(1, lf->b_(i)));
- if( !(::equal_with_abs_tol(b_, (lf->b_),tol)) )
- return false;
+ for(; it1 != As_.end(); it1++, it2++) {
+ const Symbol& j1 = it1->first, j2 = it2->first;
+ const Matrix A1 = it1->second, A2 = it2->second;
+ if (j1 != j2) return false;
+
+ row1.push_back(row_(A1,i));
+ row2.push_back(row_(A2,i));
+ }
+
+ Vector r1 = concatVectors(row1);
+ Vector r2 = concatVectors(row2);
+ if( !::equal_with_abs_tol(r1, r2, tol) &&
+ !::equal_with_abs_tol(r1*(-1), r2, tol)) {
+ return false;
+ }
+ }
return model_->equals(*(lf->model_),tol);
}
diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp
index fb65ac5d3..db5868070 100644
--- a/cpp/GaussianFactorGraph.cpp
+++ b/cpp/GaussianFactorGraph.cpp
@@ -203,7 +203,8 @@ VectorConfig GaussianFactorGraph::optimize(const Ordering& ordering, bool old)
GaussianBayesNet chordalBayesNet = eliminate(ordering, old);
// calculate new configuration (using backsubstitution)
- return ::optimize(chordalBayesNet);
+ VectorConfig delta = ::optimize(chordalBayesNet);
+ return delta;
}
/* ************************************************************************* */
diff --git a/cpp/Makefile.am b/cpp/Makefile.am
index c2383cf19..d051c644e 100644
--- a/cpp/Makefile.am
+++ b/cpp/Makefile.am
@@ -29,12 +29,14 @@ version = $(current):$(revision):$(age)
# we specify the library in levels
# basic Math
-sources = Vector.cpp svdcmp.cpp Matrix.cpp
-check_PROGRAMS = testVector testMatrix
+sources = Vector.cpp svdcmp.cpp Matrix.cpp SPQRUtil.cpp
+check_PROGRAMS = testVector testMatrix testSPQRUtil
testVector_SOURCES = testVector.cpp
testVector_LDADD = libgtsam.la
testMatrix_SOURCES = testMatrix.cpp
testMatrix_LDADD = libgtsam.la
+testSPQRUtil_SOURCES = testSPQRUtil.cpp
+testSPQRUtil_LDADD = libgtsam.la
# GTSAM basics
# The header files will be installed in ~/include/gtsam
@@ -291,7 +293,7 @@ AM_CXXFLAGS = -I$(boost) -fPIC
lib_LTLIBRARIES = libgtsam.la
libgtsam_la_SOURCES = $(sources)
libgtsam_la_CPPFLAGS = $(AM_CXXFLAGS)
-libgtsam_la_LDFLAGS = -version-info $(version) -L../colamd -lcolamd -L../ldl -lldl # ../ldl/libldl.a -lboost_serialization-mt
+libgtsam_la_LDFLAGS = -version-info $(version) -L../colamd -lcolamd -L../ldl -lldl -L../spqr_mini -lspqr_mini # ../ldl/libldl.a -lboost_serialization-mt
# enable debug if --enable-debug is set in configure
if DEBUG
@@ -304,13 +306,6 @@ include_HEADERS = $(headers)
AM_CXXFLAGS += -I..
AM_LDFLAGS = -L../CppUnitLite -lCppUnitLite $(BOOST_LDFLAGS) $(boost_serialization)
-# adding SparseQR support
-if USE_SPQR
-AM_CXXFLAGS += -I$(HOME)/include/SuiteSparse -DUSE_SPQR
-AM_LDFLAGS += -L$(HOME)/lib/SuiteSparse -lufconfig -lamd -lcamd -lcolamd -lbtf -lklu -lldl -lccolamd -lumfpack -lcholmod -lcxsparse -lspqr
-#AM_LDFLAGS += -L$(HOME)/lib/SuiteSparse -lsqpr
-endif
-
# adding cblas implementation - split into a default linux version using the
# autotools script, and a mac version that is hardcoded
# NOTE: the GT_USE_CBLAS is just used as a means of detecting when blas is available
diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp
index 0d0ca56e2..6375a925b 100644
--- a/cpp/Matrix.cpp
+++ b/cpp/Matrix.cpp
@@ -155,6 +155,20 @@ bool assert_equal(const std::list& As, const std::list& Bs, doub
return true;
}
+/* ************************************************************************* */
+bool linear_dependent(const Matrix& A, const Matrix& B, double tol) {
+ size_t n1 = A.size2(), m1 = A.size1();
+ size_t n2 = B.size2(), m2 = B.size1();
+
+ if(m1!=m2 || n1!=n2) return false;
+
+ for(int i=0; i& As, const std::list& Bs, double tol = 1e-9);
+/**
+ * check whether the rows of two matrices are linear indepdent
+ */
+bool linear_dependent(const Matrix& A, const Matrix& B, double tol = 1e-9);
+
/**
* overload * for matrix-vector multiplication (as BOOST does not)
*/
diff --git a/cpp/NoiseModel.cpp b/cpp/NoiseModel.cpp
index 46cdd17d3..a0689ab29 100644
--- a/cpp/NoiseModel.cpp
+++ b/cpp/NoiseModel.cpp
@@ -20,6 +20,7 @@
#include "NoiseModel.h"
#include "SharedDiagonal.h"
+#include "SPQRUtil.h"
namespace ublas = boost::numeric::ublas;
typedef ublas::matrix_column column;
@@ -104,8 +105,6 @@ void Gaussian::WhitenInPlace(Matrix& H) const {
// General QR, see also special version in Constrained
SharedDiagonal Gaussian::QR(Matrix& Ab) const {
- bool verbose = false;
- if (verbose) cout << "\nIn Gaussian::QR" << endl;
// get size(A) and maxRank
// TODO: really no rank problems ?
@@ -113,19 +112,23 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const {
size_t maxRank = min(m,n);
// pre-whiten everything (cheaply if possible)
- if (verbose) gtsam::print(Ab, "Ab before whitening");
WhitenInPlace(Ab);
- if (verbose) gtsam::print(Ab, "Ab after whitening");
// Perform in-place Householder
#ifdef GT_USE_LAPACK
- householder(Ab);
+ long* Stair = MakeStairs(Ab);
+// double t0, t1;
+// t0 = clock();
+// gtsam::print(Ab, "Ab");
+ householder_spqr(Ab, Stair);
+// householder_spqr(Ab);
+// gtsam::print(Ab, "Ab");
+// t1 = clock();
+// printf("QR: %ld %ld %g\n", m, n, (double) (t1 - t0) / CLOCKS_PER_SEC * 1000);
#else
householder(Ab, maxRank);
#endif
- if (verbose) gtsam::print(Ab, "Ab before householder");
-
return Unit::Create(maxRank);
}
diff --git a/cpp/SPQRUtil.cpp b/cpp/SPQRUtil.cpp
new file mode 100644
index 000000000..06f3b2720
--- /dev/null
+++ b/cpp/SPQRUtil.cpp
@@ -0,0 +1,142 @@
+/*
+ * SPQRUtil.cpp
+ *
+ * Created on: Jul 1, 2010
+ * Author: nikai
+ * Description: the utility functions for SPQR
+ */
+
+#include