denseQR
Kai Ni
parent
8ff5bf5c7c
commit
b6c625a63f
|
|
@ -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
|
||||
}
|
||||
|
|
@ -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 <gtsam/base/timing.h>
|
||||
#include <gtsam/base/DenseQRUtil.h>
|
||||
#include <boost/numeric/ublas/matrix.hpp>
|
||||
#include <boost/numeric/ublas/matrix_proxy.hpp>
|
||||
#include <boost/numeric/ublas/triangular.hpp>
|
||||
|
||||
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<int> remained;
|
||||
a[0] = A.data().begin();
|
||||
for(int i=0; i<m-1; i++) {
|
||||
a[i+1] = a[i] + n;
|
||||
remained.push_back(i);
|
||||
}
|
||||
remained.push_back(m-1);
|
||||
|
||||
// reorder the rows
|
||||
int j;
|
||||
vector<int> sorted;
|
||||
list<int>::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<n; j++)
|
||||
Stair[j] = m;
|
||||
|
||||
// copy the new row
|
||||
Matrix A_new = zeros(m,n);
|
||||
int offset[m];
|
||||
offset[0] = 0;
|
||||
for(int i=1; i<m; i++)
|
||||
offset[i] = offset[i-1] +n;
|
||||
vector<int>::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; i<m; i++) {
|
||||
for(int j=0; j<n; j++)
|
||||
cout << a[j*m+i] << "\t";
|
||||
cout << endl;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void householder_denseqr(Matrix &A, int* Stair) {
|
||||
|
||||
tic("householder_denseqr");
|
||||
|
||||
int m = A.size1();
|
||||
int n = A.size2();
|
||||
|
||||
bool allocedStair = false;
|
||||
if (Stair == NULL) {
|
||||
allocedStair = true;
|
||||
Stair = new int[n];
|
||||
for(int j=0; j<n; j++)
|
||||
Stair[j] = m;
|
||||
}
|
||||
|
||||
tic("householder_denseqr: row->col");
|
||||
// convert from row major to column major
|
||||
ublas::matrix<double, ublas::column_major> 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; j<n; j++, k0+=m) {
|
||||
k = k0;
|
||||
j0 = min(j+1,m);
|
||||
for(int i=0; i<j0; i++, k++)
|
||||
A(i,j) = a[k];
|
||||
}
|
||||
|
||||
toc("householder_denseqr: col->row");
|
||||
|
||||
|
||||
if(allocedStair) delete[] Stair;
|
||||
|
||||
toc("householder_denseqr");
|
||||
}
|
||||
|
||||
void householder_denseqr_colmajor(ublas::matrix<double, ublas::column_major>& 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
|
||||
|
|
@ -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 <gtsam/base/Matrix.h>
|
||||
|
||||
#ifdef GT_USE_LAPACK
|
||||
#include <gtsam/base/DenseQR.h>
|
||||
|
||||
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<double, boost::numeric::ublas::column_major>& A, int *Stair);
|
||||
}
|
||||
#endif
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 <iostream>
|
||||
#include <gtsam/CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/DenseQRUtil.h>
|
||||
|
||||
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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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<long> firstZeroRows(Ab_.size2());
|
||||
vector<int> firstZeroRows(Ab_.size2());
|
||||
{
|
||||
size_t lastNonzeroRow = 0;
|
||||
vector<long>::iterator firstZeroRowsIt = firstZeroRows.begin();
|
||||
vector<int>::iterator firstZeroRowsIt = firstZeroRows.begin();
|
||||
for(size_t var=0; var<keys().size(); ++var) {
|
||||
while(lastNonzeroRow < this->numberOfRows() && 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<long> firstZeroRows(Ab_.size2());
|
||||
vector<int> firstZeroRows(Ab_.size2());
|
||||
{
|
||||
size_t lastNonzeroRow = 0;
|
||||
vector<long>::iterator firstZeroRowsIt = firstZeroRows.begin();
|
||||
vector<int>::iterator firstZeroRowsIt = firstZeroRows.begin();
|
||||
for(size_t var=0; var<keys().size(); ++var) {
|
||||
while(lastNonzeroRow < this->numberOfRows() && firstNonzeroBlocks_[lastNonzeroRow] <= var)
|
||||
++ lastNonzeroRow;
|
||||
|
|
|
|||
|
|
@ -31,6 +31,7 @@
|
|||
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/linear/SharedDiagonal.h>
|
||||
#include <gtsam/base/DenseQRUtil.h>
|
||||
|
||||
namespace ublas = boost::numeric::ublas;
|
||||
typedef ublas::matrix_column<Matrix> 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<vector<long>&> firstZeroRows) const {
|
||||
SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<vector<int>&> 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<std::vector<long>&> firstZeroRows) const {
|
||||
/*SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<std::vector<long>&> firstZeroRows) const {
|
||||
|
||||
WhitenInPlace(Ab);
|
||||
// get size(A) and maxRank
|
||||
|
|
@ -214,31 +215,31 @@ SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<std::vector<long>&> firs
|
|||
}
|
||||
|
||||
return Unit::Create(maxRank);
|
||||
}
|
||||
}*/
|
||||
|
||||
|
||||
// General QR, see also special version in Constrained
|
||||
SharedDiagonal Gaussian::QRColumnWise(ublas::matrix<double, ublas::column_major>& Ab, vector<long>& 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<double, ublas::column_major>& Ab, vector<int>& 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<std::vector<long>&> firstZeroRows) const {
|
||||
SharedDiagonal Constrained::QR(Matrix& Ab, boost::optional<std::vector<int>&> firstZeroRows) const {
|
||||
bool verbose = false;
|
||||
if (verbose) cout << "\nStarting Constrained::QR" << endl;
|
||||
|
||||
|
|
@ -401,7 +402,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab, boost::optional<std::vector<long>&> f
|
|||
return mixed ? Constrained::MixedPrecisions(precisions) : Diagonal::Precisions(precisions);
|
||||
}
|
||||
|
||||
SharedDiagonal Constrained::QRColumnWise(ublas::matrix<double, ublas::column_major>& Ab, vector<long>& firstZeroRows) const {
|
||||
SharedDiagonal Constrained::QRColumnWise(ublas::matrix<double, ublas::column_major>& Ab, vector<int>& firstZeroRows) const {
|
||||
Matrix AbRowWise(Ab);
|
||||
SharedDiagonal result = this->QR(AbRowWise, firstZeroRows);
|
||||
Ab = AbRowWise;
|
||||
|
|
|
|||
|
|
@ -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<std::vector<long>&> firstZeroRows = boost::none) const;
|
||||
virtual SharedDiagonal QR(Matrix& Ab, boost::optional<std::vector<int>&> firstZeroRows = boost::none) const;
|
||||
|
||||
/**
|
||||
* Version for column-wise matrices
|
||||
*/
|
||||
virtual SharedDiagonal QRColumnWise(boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major>& Ab,
|
||||
std::vector<long>& firstZeroRows) const;
|
||||
std::vector<int>& 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<std::vector<long>&> firstZeroRows = boost::none) const;
|
||||
virtual SharedDiagonal QR(Matrix& Ab, boost::optional<std::vector<int>&> firstZeroRows = boost::none) const;
|
||||
|
||||
/**
|
||||
* Version for column-wise matrices
|
||||
*/
|
||||
virtual SharedDiagonal QRColumnWise(boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major>& Ab,
|
||||
std::vector<long>& firstZeroRows) const;
|
||||
std::vector<int>& firstZeroRows) const;
|
||||
|
||||
/**
|
||||
* Check constrained is always true
|
||||
|
|
|
|||
Loading…
Reference in New Issue