From b6c625a63fa47c32584e70cba97278d9a1bf9f57 Mon Sep 17 00:00:00 2001 From: Kai Ni Date: Fri, 22 Oct 2010 22:32:24 +0000 Subject: [PATCH] denseQR --- base/DenseQR.h | 14 +++ base/DenseQRUtil.cpp | 168 ++++++++++++++++++++++++++ base/DenseQRUtil.h | 37 ++++++ base/Makefile.am | 5 + base/tests/testDenseQRUtil.cpp | 207 +++++++++++++++++++++++++++++++++ linear/GaussianFactor.cpp | 8 +- linear/NoiseModel.cpp | 63 +++++----- linear/NoiseModel.h | 8 +- 8 files changed, 471 insertions(+), 39 deletions(-) create mode 100644 base/DenseQR.h create mode 100644 base/DenseQRUtil.cpp create mode 100644 base/DenseQRUtil.h create mode 100644 base/tests/testDenseQRUtil.cpp diff --git a/base/DenseQR.h b/base/DenseQR.h new file mode 100644 index 000000000..8bd09fa37 --- /dev/null +++ b/base/DenseQR.h @@ -0,0 +1,14 @@ +/* + * DenseQR.h + * + * Created on: Oct 19, 2010 + * Author: nikai + * Description: Dense QR, inspired by Tim Davis's dense solver + */ + +#pragma once + +namespace gtsam { + void DenseQR( int m, int n, int numPivotColumns, // inputs + double *F, int *Stair, double *W); // outputs +} diff --git a/base/DenseQRUtil.cpp b/base/DenseQRUtil.cpp new file mode 100644 index 000000000..2afeed165 --- /dev/null +++ b/base/DenseQRUtil.cpp @@ -0,0 +1,168 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * DenseQRUtil.cpp + * + * Created on: Jul 1, 2010 + * Author: nikai + * Description: the utility functions for DenseQR + */ + +#include +#include +#include +#include +#include + +using namespace std; +namespace ublas = boost::numeric::ublas; + +#ifdef GT_USE_LAPACK +namespace gtsam { + + /* ************************************************************************* */ + int* MakeStairs(Matrix& A) { + + const int m = A.size1(); + const int n = A.size2(); + int* Stair = new int[n]; + + // record the starting pointer of each row + double* a[m]; + list remained; + a[0] = A.data().begin(); + for(int i=0; i sorted; + list::iterator itRemained; + for(j = 0; j < n; ) { + // remove the non-zero rows in the current column + for(itRemained = remained.begin(); itRemained!=remained.end(); ) { + if (*(a[*itRemained]) != 0) { + sorted.push_back(*itRemained); + itRemained = remained.erase(itRemained); + } else { + a[*itRemained]++; + itRemained ++; + } + } + + // record the stair number + Stair[j++] = m - remained.size(); + + if(remained.empty()) break; + } + + // all the remained columns have maximum stair + for (; j::const_iterator itSorted; + double* ptr1 = A.data().begin(); + double* ptr2 = A_new.data().begin(); + int row = 0, sizeOfRow = n * sizeof(double); + for(itSorted=sorted.begin(); itSorted!=sorted.end(); itSorted++, row++) + memcpy(ptr2+offset[row], ptr1+offset[*itSorted], sizeOfRow); + + A = A_new; + + return Stair; + } + + void printColumnMajor(const double* a, const int m, const int n) { + for(int i=0; icol"); + // convert from row major to column major + ublas::matrix Acolwise(A); + double *a = Acolwise.data().begin(); + toc("householder_denseqr: row->col"); + + tic("householder_denseqr: denseqr_front"); + int npiv = min(m,n); + int b = min(min(m,n),32); + double W[b*(n+b)]; + DenseQR(m, n, npiv, a, Stair, W); + toc("householder_denseqr: denseqr_front"); + + tic("householder_denseqr: col->row"); + int k0 = 0; + int j0; + int k; + memset(A.data().begin(), 0, m*n*sizeof(double)); + for(int j=0; jrow"); + + + if(allocedStair) delete[] Stair; + + toc("householder_denseqr"); + } + + void householder_denseqr_colmajor(ublas::matrix& A, int *Stair) { + tic("householder_denseqr"); + + int m = A.size1(); + int n = A.size2(); + + assert(Stair != NULL); + + tic("householder_denseqr: denseqr_front"); + int npiv = min(m,n); + int b = min(min(m,n),32); + double W[b*(n+b)]; + DenseQR(m, n, npiv, A.data().begin(), Stair, W); + toc("householder_denseqr: denseqr_front"); + + } + +} // namespace gtsam +#endif diff --git a/base/DenseQRUtil.h b/base/DenseQRUtil.h new file mode 100644 index 000000000..8f29012b7 --- /dev/null +++ b/base/DenseQRUtil.h @@ -0,0 +1,37 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * DenseQRUtil.h + * + * Created on: Jul 1, 2010 + * Author: nikai + * Description: the utility functions for DenseQR + */ + +#pragma once + +#include + +#ifdef GT_USE_LAPACK +#include + +namespace gtsam { + + /** make stairs and speed up householder_denseqr. Stair is defined as the row index of where zero entries start in each column */ + int* MakeStairs(Matrix &A); + + /** Householder tranformation, zeros below diagonal */ + void householder_denseqr(Matrix &A, int* Stair = NULL); + + void householder_denseqr_colmajor(boost::numeric::ublas::matrix& A, int *Stair); +} +#endif diff --git a/base/Makefile.am b/base/Makefile.am index 923a63518..4086a62af 100644 --- a/base/Makefile.am +++ b/base/Makefile.am @@ -17,6 +17,11 @@ headers += FixedVector.h types.h blockMatrices.h sources += Vector.cpp svdcmp.cpp Matrix.cpp check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix +if USE_LAPACK +sources += DenseQR.cpp DenseQRUtil.cpp +check_PROGRAMS += tests/testDenseQRUtil +endif + # Testing headers += Testable.h TestableAssertions.h numericalDerivative.h sources += timing.cpp diff --git a/base/tests/testDenseQRUtil.cpp b/base/tests/testDenseQRUtil.cpp new file mode 100644 index 000000000..755be6e51 --- /dev/null +++ b/base/tests/testDenseQRUtil.cpp @@ -0,0 +1,207 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSPQRUtil.cpp + * @brief Unit test for SPQR utility functions + * @author Kai Ni + **/ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +#ifdef GT_USE_LAPACK +/* ************************************************************************* */ +TEST(SPQRUtil, MakeStair) +{ + double data[] = { -5, 0, 5, 0, 0, 0, -1, + 00,-5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10,0, 2, + 00, 10,0, 0, 0, -10, -1 }; + Matrix A = Matrix_(4, 7, data); + + int* Stair = MakeStairs(A); + + double data2[] = { -5, 0, 5, 0, 0, 0, -1, + 10, 0, 0, 0,-10,0, 2, + 00,-5, 0, 5, 0, 0, 1.5, + 00, 10,0, 0, 0, -10, -1 }; + Matrix A_expected = Matrix_(4, 7, data2); + CHECK(assert_equal(A_expected, A, 1e-10)); + + int Stair_expected[] = {2, 4, 4, 4, 4, 4, 4}; + for (int i=0; i<7; i++) + DOUBLES_EQUAL(Stair_expected[i], Stair[i], 1e-7); + delete []Stair; +} + +/* ************************************************************************* */ +TEST(SPQRUtil, MakeStair2) +{ + double data[] = { 0.1, 0, 0, 0, + 0, 0.3, 0, 0, + 0, 0, 0.3, 0, + 1.6,-0.2, -2.5, 0.2, + 0, 1.6, 0.7, 0.1, + 0, 0, -7.8, 0.7 }; + Matrix A = Matrix_(6, 4, data); + + int* Stair = MakeStairs(A); + + double data2[] = { 0.1, 0, 0, 0, + 1.6,-0.2, -2.5, 0.2, + 0, 0.3, 0, 0, + 0, 1.6, 0.7, 0.1, + 0, 0, 0.3, 0, + 0, 0, -7.8, 0.7 + }; + Matrix A_expected = Matrix_(6, 4, data2); + CHECK(assert_equal(A_expected, A, 1e-10)); + + int Stair_expected[] = {2, 4, 6, 6}; + for (int i=0; i<4; i++) + DOUBLES_EQUAL(Stair_expected[i], Stair[i], 1e-7); + delete []Stair; +} + +/* ************************************************************************* */ +TEST(SPQRUtil, houseHolder_denseqr) +{ + double data[] = { -5, 0, 5, 0, 0, 0, -1, + 00,-5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10,0, 2, + 00, 10,0, 0, 0, -10, -1 }; + + // check in-place householder, with v vectors below diagonal + double data1[] = { 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, + 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, + 0, 0, 4.4721, 0, -4.4721, 0, 0, + 0, 0, 0, 4.4721, 0, -4.4721, 0.894 }; + Matrix expected1 = Matrix_(4, 7, data1); + Matrix A1 = Matrix_(4, 7, data); + householder_denseqr(A1); + CHECK(assert_equal(expected1, A1, 1e-3)); +} + +/* ************************************************************************* */ +TEST(SPQRUtil, houseHolder_denseqr2) +{ + double data[] = { -5, 0, 5, 0, 0, 0, -1, + 00,-5, 0, 5, 0, 0, 1.5, + 10, 0, 0, 0,-10,0, 2, + 00, 10,0, 0, 0, -10, -1 }; + + // check in-place householder, with v vectors below diagonal + double data1[] = { 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, + 0, -11.1803, 0, 2.2361, 0, 8.9443, 1.565, + 0, 0, -4.4721, 0, 4.4721, 0, 0, + 0, 0, 0, 4.4721, 0, -4.4721, 0.894 }; + Matrix expected1 = Matrix_(4, 7, data1); + Matrix A1 = Matrix_(4, 7, data); + int* Stair = MakeStairs(A1); + householder_denseqr(A1, Stair); + delete[] Stair; + CHECK(assert_equal(expected1, A1, 1e-3)); +} + +/* ************************************************************************* */ +TEST(SPQRUtil, houseHolder_denseqr3) +{ + double data[] = { 1, 1, 9, + 1, 0, 5}; + + // check in-place householder, with v vectors below diagonal + double data1[] = {-sqrt(2), -1/sqrt(2), -7*sqrt(2), + 0, -1/sqrt(2), -4/sqrt(2)}; + Matrix expected1 = Matrix_(2, 3, data1); + Matrix A1 = Matrix_(2, 3, data); + householder_denseqr(A1); + CHECK(assert_equal(expected1, A1, 1e-3)); +} +/* ************************************************************************* */ +TEST(SPQRUtil, houseHolder_denseqr4) +{ + Matrix A = Matrix_(15, 34, + -5.48351, 23.2337, -45.2073, 6.33455,-0.342553,-0.897005, 7.91126, 3.20237, -2.49219, -2.44189,-0.977376, -1.61127, -3.68421,-1.28059, -2.83303, 2.45949, 0.218835, -0.71239,-0.169314,-0.131355, 2.04233, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.0782689, + -15.8515, 10.2731, 23.2069, 0.0, 2.0891, 9.02822, 2.18705, 6.1083, -10.5157,-0.690031,-0.638592, -2.47301, -1.16601,-1.35043, -5.39516, 0.69744, -1.22492, -1.86158, -5.12896, -7.27133, -18.7928, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.360876, + 13.5817, 17.9017, 50.0056, 0.0, 0.0, -1.10618, 6.61197, -6.7864, 8.87588, -2.01464, 1.49684, 3.39016, -2.92526, 2.16326, 6.17234, 1.98881, 0.309325, 1.86023, -4.94073, 2.7695, 7.85143, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,0.0619088, + 12.797, 4.79759, -15.866, -1.94292, 0.436084, 1.43799, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -8.05369,-0.492175, -7.84104, 1.53703,-0.807928, 0.144945,-0.341446, -2.06456, 2.259, 0.101199, 0.161626,-0.184581, -11.7786, -1.58626, 4.21069,-0.179109, + -9.32565, 6.08188, 4.92746, 0.0,-0.0726655, -0.280519, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.5653, 10.1011, 19.53,-0.948225, -16.6125, -4.1864, 4.82523, 5.36202, 12.5239, 0.410163, 0.493983,-0.122754, -1.62954, -5.42323, 24.9016, 0.868911, + -8.2164, -7.81388, -9.60582, 0.0, 0.0,-0.0461159, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -16.7678, 5.91287, 12.5045, 6.54954, -15.7228, -10.519, -7.66513, 0.958071,0.462615, 0.744471,-0.334807, -1.22164, -3.33139, 9.43355, -14.4208,-0.831651, + -1.1057, 1.99291, 3.61474, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 6.56605, 4.08432, 7.3415, -4.07027, -25.9866, -19.3138, -2.7792, -3.83993,-3.53144, 0.603377,-0.588407,-0.296625, -17.2456, -9.02528, -51.079, -1.49078, + 0.0, 1.5495, 2.63487, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.63944, 4.62248, 4.4997, -29.3868, -6.10506, 20.9431, 10.654, -11.8748,-0.904113, -1.14369, 2.23985, 3.24988, 10.8288, 32.2749, 0.665805,-0.840659, + 0.0, 0.0,-0.576351, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.60377, 5.9181, 5.34736, 18.6972, -1.10715, 39.0521, -5.25853, -14.9718, 4.29131, 0.480123, 3.42935, -1.58455, 24.3192, -17.98, 4.7336, 0.939854, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -6.19444,-0.142454, 0.586582, 29.6886, -4.73646, -5.11474, 39.6994, -1.12835,-3.69964, -3.04975,0.0965116, 8.58342, -3.79485, 19.0323, -5.69059, -1.11543, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.44195, -4.96618, -1.12282, -1.01802, -46.1653, 0.864773, -18.0404, 24.8898, 1.64442, 5.72634,-0.948517, 17.2222, 0.916729, -2.30198, 1.17404, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.33992, 1.16655, -18.9535, -24.3327, -8.3228, -25.6997,-42.6673, -3.59911,0.0388951, 1.07185, 7.14524, -5.94804, 28.0376,-0.364333, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.06503, -4.87522, 3.87236, -11.3562, 3.23001, 33.5579, -13.8812, 7.18718, 1.71541,-0.495603,-0.826235, -6.04699, -1.9388, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.60639, -2.2334, 9.04169, 13.1342,-7.14457, 5.82756, 14.771, -49.7693, -4.22287, 2.58753, 1.22899,-0.752895, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.86199, 8.53755, -19.6873, 20.8838, 6.09985, -12.3691, -28.4341, 7.05672, 3.02888, 0.594889,-0.214789); + + Matrix expectedR = Matrix_(15, 34, + 28.0225, 0.698206, 13.7437, -2.12682,-0.891379, -4.70403, 0.419395, -7.37109, 10.738,-0.108275, 1.27796, 3.35731,-0.037285, 2.06295, 6.5978,0.0881205, 0.799999, 2.09404, 0.0, 0.0, 0.0,-0.742297, 10.7949, 5.30571, 0.59541, -2.85665,-3.13251, -0.332376,0.0308008, 0.326458, -3.17932, -1.32946,-0.120428, 0.181167, + 0.0, 33.0569, 3.44432, 4.21511, 0.477218, 1.84355, 9.81181, 0.629595, -0.43972, -3.01942,-0.101787,-0.135997, -4.53513,-0.191803, -0.46459, 3.02053,-0.0762487, -0.116055, 0.0, 0.0, 0.0, -3.10672, -1.53799, 1.44251, 2.96932,-0.267359, 2.33355, -0.0960386, 0.262347, 0.366951, -1.68673, -2.46105, 5.55563,0.0637128, + 0.0, 0.0, -72.5995, 3.31723, -0.92697, -3.15842, 0.217843, 3.35038, -2.29212,-0.0760686, -1.19838, -1.9188,-0.128748,-1.47433, -3.06396,0.0986865, 0.462591,-0.738925, 0.0, 0.0, 0.0, 2.39657, 2.3479, 0.508439, -1.45276,-0.738523,-0.534709, 0.04058,-0.0489968, -0.230194, -2.92748,0.0364329, 0.119466,-0.0346618, + 0.0, 0.0, 0.0, 3.25682, -1.182, -4.84344, 2.74062, -2.81233, 5.06869,-0.834871, 0.28589, 1.18891, -1.18948,0.606334, 2.52042, 0.831464, 0.575576, 0.884713, 4.47525,0.0381282, 8.65006, 0.178133, 7.13055, 0.99353, -1.7708, 0.464406,-5.86884, -0.194461,-0.365941,0.0828452, 10.1153, 3.22621, -9.90261,0.0257706, + 0.0, 0.0, 0.0, 0.0, 1.18256, 5.14989, 0.838654, 3.86831, -6.31407,-0.268897,-0.494264, -1.63226,-0.480456,-0.931946, -3.4322, 0.275576,-0.655629, -1.15196, -7.78905, -13.5715, -29.2364, 3.37589, 18.448, 5.11948, -4.08059, -3.2509,-9.52461, -0.362224,-0.457579,-0.0601673, 1.85657, 2.99257, -12.1144,-0.624855, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.450726, -2.52457, 2.18784, -3.04253, 0.770326,-0.443888, -1.07379, 1.12148,-0.662977, -1.98947,-0.762824,-0.151537,-0.615483, 19.9937, -5.17055, -11.2101, -10.0805, 10.6168, 9.36274, 8.17588, -1.78258,-0.858858, -0.75124, 0.443364, 1.48335, 4.46589, -5.72814, 8.27179, 0.551859, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,0.0812029,-0.0832869, 0.0674935,-0.0247479, 0.0283366,0.0480445,-0.036613,0.0351956, 0.0768672,0.0242134,-0.00953749,0.0194382, -5.93603, -5.0025, -6.38014, 18.5158, 22.668, 1.61251, -1.86948, 11.5217, 5.39137, 0.160562,-0.866767, -1.46548, 6.35692, -13.7392, 45.5091, 1.89557, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.0237536, 0.0206287,6.48832e-05,0.00746251, 0.0129591,0.000272306,0.00955241, 0.0216226,-0.000202751, -0.00189634,0.00560094, -5.77613, 5.44125, 4.94628, 21.3185,-0.976758, 36.3015, -6.24453, -13.7772, 4.2347, 0.597408, 3.16863, -1.89053, 22.7518, -21.5891, 3.36502, 0.993638, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.00807513,-1.07967e-06, 0.00194212, 0.00125444, -0.000133756, 0.00168498, 0.000217439,-4.13036e-05, -0.00259827,-0.000661005, 1.19994, 2.30552, 0.746869, -18.6973, 9.7233, 31.6093, 9.52016, -8.27898, 2.32924, -1.18233, 2.47028, 2.54466, 21.2909, 29.1971, 32.4215, 0.342241, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -6.19444,-0.142454, 0.586582, 29.6886, -4.73646, -5.11474, 39.6994, -1.12835,-3.69964, -3.04975,0.0965116, 8.58342, -3.79485, 19.0323, -5.69059, -1.11543, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.44195, -4.96618, -1.12282, -1.01802, -46.1653, 0.864773, -18.0404, 24.8898, 1.64442, 5.72634,-0.948517, 17.2222, 0.916729, -2.30198, 1.17404, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.33992, 1.16655, -18.9535, -24.3327, -8.3228, -25.6997,-42.6673, -3.59911,0.0388951, 1.07185, 7.14524, -5.94804, 28.0376,-0.364333, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.06503, -4.87522, 3.87236, -11.3562, 3.23001, 33.5579, -13.8812, 7.18718, 1.71541,-0.495603,-0.826235, -6.04699, -1.9388, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.60639, -2.2334, 9.04169, 13.1342,-7.14457, 5.82756, 14.771, -49.7693, -4.22287, 2.58753, 1.22899,-0.752895, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.86199, 8.53755, -19.6873, 20.8838, 6.09985, -12.3691, -28.4341, 7.05672, 3.02888, 0.594889,-0.214789); + + Matrix expectedA = Matrix_(15, 34, + -5.48351, 23.2337, -45.2073, 6.33455,-0.342553,-0.897005, 7.91126, 3.20237, -2.49219, -2.44189,-0.977376, -1.61127, -3.68421,-1.28059, -2.83303, 2.45949, 0.218835, -0.71239,-0.169314,-0.131355, 2.04233, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.0782689, + -15.8515, 10.2731, 23.2069, 0.0, 2.0891, 9.02822, 2.18705, 6.1083, -10.5157,-0.690031,-0.638592, -2.47301, -1.16601,-1.35043, -5.39516, 0.69744, -1.22492, -1.86158, -5.12896, -7.27133, -18.7928, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.360876, + 13.5817, 17.9017, 50.0056, 0.0, 0.0, -1.10618, 6.61197, -6.7864, 8.87588, -2.01464, 1.49684, 3.39016, -2.92526, 2.16326, 6.17234, 1.98881, 0.309325, 1.86023, -4.94073, 2.7695, 7.85143, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,0.0619088, + 12.797, 4.79759, -15.866, -1.94292, 0.436084, 1.43799, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -8.05369,-0.492175, -7.84104, 1.53703,-0.807928, 0.144945,-0.341446, -2.06456, 2.259, 0.101199, 0.161626,-0.184581, -11.7786, -1.58626, 4.21069,-0.179109, + -9.32565, 6.08188, 4.92746, 0.0,-0.0726655, -0.280519, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.5653, 10.1011, 19.53,-0.948225, -16.6125, -4.1864, 4.82523, 5.36202, 12.5239, 0.410163, 0.493983,-0.122754, -1.62954, -5.42323, 24.9016, 0.868911, + -8.2164, -7.81388, -9.60582, 0.0, 0.0,-0.0461159, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -16.7678, 5.91287, 12.5045, 6.54954, -15.7228, -10.519, -7.66513, 0.958071,0.462615, 0.744471,-0.334807, -1.22164, -3.33139, 9.43355, -14.4208,-0.831651, + -1.1057, 1.99291, 3.61474, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 6.56605, 4.08432, 7.3415, -4.07027, -25.9866, -19.3138, -2.7792, -3.83993,-3.53144, 0.603377,-0.588407,-0.296625, -17.2456, -9.02528, -51.079, -1.49078, + 0.0, 1.5495, 2.63487, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.63944, 4.62248, 4.4997, -29.3868, -6.10506, 20.9431, 10.654, -11.8748,-0.904113, -1.14369, 2.23985, 3.24988, 10.8288, 32.2749, 0.665805,-0.840659, + 0.0, 0.0,-0.576351, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.60377, 5.9181, 5.34736, 18.6972, -1.10715, 39.0521, -5.25853, -14.9718, 4.29131, 0.480123, 3.42935, -1.58455, 24.3192, -17.98, 4.7336, 0.939854, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -6.19444,-0.142454, 0.586582, 29.6886, -4.73646, -5.11474, 39.6994, -1.12835,-3.69964, -3.04975,0.0965116, 8.58342, -3.79485, 19.0323, -5.69059, -1.11543, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.44195, -4.96618, -1.12282, -1.01802, -46.1653, 0.864773, -18.0404, 24.8898, 1.64442, 5.72634,-0.948517, 17.2222, 0.916729, -2.30198, 1.17404, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.33992, 1.16655, -18.9535, -24.3327, -8.3228, -25.6997,-42.6673, -3.59911,0.0388951, 1.07185, 7.14524, -5.94804, 28.0376,-0.364333, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.06503, -4.87522, 3.87236, -11.3562, 3.23001, 33.5579, -13.8812, 7.18718, 1.71541,-0.495603,-0.826235, -6.04699, -1.9388, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.60639, -2.2334, 9.04169, 13.1342,-7.14457, 5.82756, 14.771, -49.7693, -4.22287, 2.58753, 1.22899,-0.752895, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.86199, 8.53755, -19.6873, 20.8838, 6.09985, -12.3691, -28.4341, 7.05672, 3.02888, 0.594889,-0.214789); + + Matrix actualR = A; + householder_denseqr(actualR); + + int* Stair = MakeStairs(A); + CHECK(assert_equal(expectedA, A)); + + Matrix actualRstair = A; + householder_denseqr(actualRstair, Stair); + delete[] Stair; + + CHECK(assert_equal(expectedR, actualR, 1e-3)); + CHECK(assert_equal(expectedR, actualRstair, 1e-3)); +} +#endif + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/linear/GaussianFactor.cpp b/linear/GaussianFactor.cpp index 2e3855c2e..9007ca300 100644 --- a/linear/GaussianFactor.cpp +++ b/linear/GaussianFactor.cpp @@ -328,10 +328,10 @@ GaussianConditional::shared_ptr GaussianFactor::eliminateFirst() { tic("eliminateFirst: stairs"); // Translate the left-most nonzero column indices into top-most zero row indices - vector firstZeroRows(Ab_.size2()); + vector firstZeroRows(Ab_.size2()); { size_t lastNonzeroRow = 0; - vector::iterator firstZeroRowsIt = firstZeroRows.begin(); + vector::iterator firstZeroRowsIt = firstZeroRows.begin(); for(size_t var=0; varnumberOfRows() && firstNonzeroBlocks_[lastNonzeroRow] <= var) ++ lastNonzeroRow; @@ -441,10 +441,10 @@ GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nrFrontals) { tic("eliminate: stairs"); // Translate the left-most nonzero column indices into top-most zero row indices - vector firstZeroRows(Ab_.size2()); + vector firstZeroRows(Ab_.size2()); { size_t lastNonzeroRow = 0; - vector::iterator firstZeroRowsIt = firstZeroRows.begin(); + vector::iterator firstZeroRowsIt = firstZeroRows.begin(); for(size_t var=0; varnumberOfRows() && firstNonzeroBlocks_[lastNonzeroRow] <= var) ++ lastNonzeroRow; diff --git a/linear/NoiseModel.cpp b/linear/NoiseModel.cpp index bbfbd36d1..bd65cf57e 100644 --- a/linear/NoiseModel.cpp +++ b/linear/NoiseModel.cpp @@ -31,6 +31,7 @@ #include #include +#include namespace ublas = boost::numeric::ublas; typedef ublas::matrix_column column; @@ -119,7 +120,7 @@ void Gaussian::WhitenInPlace(MatrixColMajor& H) const { } // General QR, see also special version in Constrained -/*SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional&> firstZeroRows) const { +SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional&> firstZeroRows) const { // get size(A) and maxRank // TODO: really no rank problems ? @@ -132,21 +133,21 @@ void Gaussian::WhitenInPlace(MatrixColMajor& H) const { // Perform in-place Householder #ifdef GT_USE_LAPACK if(firstZeroRows) - householder_denseqr(Ab, &(*firstZeroRows)[0]); + householder_denseqr(Ab, &(*firstZeroRows)[0]); else - householder_denseqr(Ab); + householder_denseqr(Ab); #else -householder(Ab, maxRank); + householder(Ab, maxRank); #endif -return Unit::Create(maxRank); -}*/ + return Unit::Create(maxRank); +} // Special version of QR for Constrained calls slower but smarter code // that deals with possibly zero sigmas // It is Gram-Schmidt orthogonalization rather than Householder // Previously Diagonal::QR -SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional&> firstZeroRows) const { +/*SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional&> firstZeroRows) const { WhitenInPlace(Ab); // get size(A) and maxRank @@ -214,31 +215,31 @@ SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional&> firs } return Unit::Create(maxRank); -} +}*/ // General QR, see also special version in Constrained -SharedDiagonal Gaussian::QRColumnWise(ublas::matrix& Ab, vector& firstZeroRows) const { - WhitenInPlace(Ab); - householderColMajor(Ab); - size_t maxRank = min(Ab.size1(), Ab.size2()-1); - return Unit::Create(maxRank); -// // get size(A) and maxRank -// // TODO: really no rank problems ? -// size_t m = Ab.size1(), n = Ab.size2()-1; -// size_t maxRank = min(m,n); -// -// // pre-whiten everything (cheaply if possible) -// WhitenInPlace(Ab); -// -// // Perform in-place Householder -//#ifdef GT_USE_LAPACK -// householder_denseqr_colmajor(Ab, &firstZeroRows[0]); -//#else -// householder(Ab, maxRank); -//#endif -// -// return Unit::Create(maxRank); +SharedDiagonal Gaussian::QRColumnWise(ublas::matrix& Ab, vector& firstZeroRows) const { +// WhitenInPlace(Ab); +// householderColMajor(Ab); +// size_t maxRank = min(Ab.size1(), Ab.size2()-1); +// return Unit::Create(maxRank); + // get size(A) and maxRank + // TODO: really no rank problems ? + size_t m = Ab.size1(), n = Ab.size2()-1; + size_t maxRank = min(m,n); + + // pre-whiten everything (cheaply if possible) + WhitenInPlace(Ab); + + // Perform in-place Householder +#ifdef GT_USE_LAPACK + householder_denseqr_colmajor(Ab, &firstZeroRows[0]); +#else + householder(Ab, maxRank); +#endif + + return Unit::Create(maxRank); } /* ************************************************************************* */ @@ -330,7 +331,7 @@ void Constrained::WhitenInPlace(MatrixColMajor& H) const { // that deals with possibly zero sigmas // It is Gram-Schmidt orthogonalization rather than Householder // Previously Diagonal::QR -SharedDiagonal Constrained::QR(Matrix& Ab, boost::optional&> firstZeroRows) const { +SharedDiagonal Constrained::QR(Matrix& Ab, boost::optional&> firstZeroRows) const { bool verbose = false; if (verbose) cout << "\nStarting Constrained::QR" << endl; @@ -401,7 +402,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab, boost::optional&> f return mixed ? Constrained::MixedPrecisions(precisions) : Diagonal::Precisions(precisions); } -SharedDiagonal Constrained::QRColumnWise(ublas::matrix& Ab, vector& firstZeroRows) const { +SharedDiagonal Constrained::QRColumnWise(ublas::matrix& Ab, vector& firstZeroRows) const { Matrix AbRowWise(Ab); SharedDiagonal result = this->QR(AbRowWise, firstZeroRows); Ab = AbRowWise; diff --git a/linear/NoiseModel.h b/linear/NoiseModel.h index 7033713e2..fd07f8a07 100644 --- a/linear/NoiseModel.h +++ b/linear/NoiseModel.h @@ -169,13 +169,13 @@ namespace gtsam { * @param Ab is the m*(n+1) augmented system matrix [A b] * @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!! */ - virtual SharedDiagonal QR(Matrix& Ab, boost::optional&> firstZeroRows = boost::none) const; + virtual SharedDiagonal QR(Matrix& Ab, boost::optional&> firstZeroRows = boost::none) const; /** * Version for column-wise matrices */ virtual SharedDiagonal QRColumnWise(boost::numeric::ublas::matrix& Ab, - std::vector& firstZeroRows) const; + std::vector& firstZeroRows) const; /** * Return R itself, but note that Whiten(H) is cheaper than R*H @@ -321,13 +321,13 @@ namespace gtsam { /** * Apply QR factorization to the system [A b], taking into account constraints */ - virtual SharedDiagonal QR(Matrix& Ab, boost::optional&> firstZeroRows = boost::none) const; + virtual SharedDiagonal QR(Matrix& Ab, boost::optional&> firstZeroRows = boost::none) const; /** * Version for column-wise matrices */ virtual SharedDiagonal QRColumnWise(boost::numeric::ublas::matrix& Ab, - std::vector& firstZeroRows) const; + std::vector& firstZeroRows) const; /** * Check constrained is always true