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

Rationalized some cholesky-related code as I was looking at it.

release/4.3a0
dellaert 2014-05-28 10:36:26 -04:00
parent b7fc6762af
commit 8dba25f532
3 changed files with 952 additions and 979 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1848
.cproject
View File

File diff suppressed because it is too large Load Diff

47
gtsam/base/Matrix.cpp
View File

@ -591,41 +591,17 @@ Matrix3 skewSymmetric(double wx, double wy, double wz)
}
/* ************************************************************************* */
/** Numerical Recipes in C wrappers
* create Numerical Recipes in C structure
* pointers are subtracted by one to provide base 1 access
*/
/* ************************************************************************* */
// FIXME: assumes row major, rather than column major
//double** createNRC(Matrix& A) {
// const size_t m=A.rows();
// double** a = new double* [m];
// for(size_t i = 0; i < m; i++)
// a[i] = &A(i,0)-1;
// return a;
//}
/* ******************************************
*
* Modified from Justin's codebase
*
* Idea came from other public domain code. Takes a S.P.D. matrix
* and computes the LL^t decomposition. returns L, which is lower
* triangular. Note this is the opposite convention from Matlab,
* which calculates Q'Q where Q is upper triangular.
*
* ******************************************/
Matrix LLt(const Matrix& A)
{
Matrix L = zeros(A.rows(), A.rows());
Eigen::LLT<Matrix> llt;
llt.compute(A);
Eigen::LLT<Matrix> llt(A);
return llt.matrixL();
}
/* ************************************************************************* */
Matrix RtR(const Matrix &A)
{
return LLt(A).transpose();
Eigen::LLT<Matrix> llt(A);
return llt.matrixU();
}
/*
@ -633,13 +609,10 @@ Matrix RtR(const Matrix &A)
*/
Matrix cholesky_inverse(const Matrix &A)
{
// FIXME: replace with real algorithm
return A.inverse();
// Matrix L = LLt(A);
// Matrix inv(eye(A.rows()));
// inplace_solve (L, inv, BNU::lower_tag ());
// return BNU::prod(trans(inv), inv);
Eigen::LLT<Matrix> llt(A);
Matrix inv = eye(A.rows());
llt.matrixU().solveInPlace<Eigen::OnTheRight>(inv);
return inv*inv.transpose();
}
/* ************************************************************************* */
@ -648,9 +621,9 @@ Matrix cholesky_inverse(const Matrix &A)
// inv(B) * inv(B)' == A
// inv(B' * B) == A
Matrix inverse_square_root(const Matrix& A) {
Matrix R = RtR(A);
Eigen::LLT<Matrix> llt(A);
Matrix inv = eye(A.rows());
R.triangularView<Eigen::Upper>().solveInPlace<Eigen::OnTheRight>(inv);
llt.matrixU().solveInPlace<Eigen::OnTheRight>(inv);
return inv.transpose();
}

36
gtsam/base/tests/testMatrix.cpp
View File

@ -1022,22 +1022,32 @@ TEST( matrix, inverse_square_root )
/* *********************************************************************** */
// M was generated as the covariance of a set of random numbers. L that
// we are checking against was generated via chol(M)' on octave
namespace cholesky {
Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463,
0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363,
0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463,
0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363,
-0.0021113, 0.0036415, 0.0909464);
Matrix expected = (Matrix(5, 5) <<
0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000,
-0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000,
0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000,
0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000,
0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247);
}
TEST( matrix, LLt )
{
Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463,
0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363,
0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463,
0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363,
-0.0021113, 0.0036415, 0.0909464);
EQUALITY(cholesky::expected, LLt(cholesky::M));
}
TEST( matrix, RtR )
{
EQUALITY(cholesky::expected.transpose(), RtR(cholesky::M));
}
Matrix expected = (Matrix(5, 5) <<
0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000,
-0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000,
0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000,
0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000,
0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247);
EQUALITY(expected, LLt(M));
TEST( matrix, cholesky_inverse )
{
EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M));
}
/* ************************************************************************* */