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

solve_ldl() now works and is a real function

release/4.3a0
Alex Cunningham 2010-04-22 23:53:36 +00:00
parent 25bd1c840f
commit bb654a73ac
2 changed files with 62 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

94
cpp/Matrix.cpp
View File

@ -895,55 +895,83 @@ Matrix RtR(const Matrix &A)
/* ************************************************************************* */
Vector solve_ldl(const Matrix& M, const Vector& rhs) {
// execute test in here
// create a matrix (from ldlsimple.c example)
const int N = 10, // size of the matrix
ANZ = 19, // # of nonzeros on diagonal and upper triangular part of A
LNZ = 13; // # of nonzeros below the diagonal of L
Matrix A = Matrix_(N, N,
1.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, .13, 0.0,
0.0, 1., 0.0, 0.0, .02, 0.0, 0.0, 0.0, 0.0, .01,
0.0, 0.0, 1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, .02, 0.0, 0.0, 2.6, 0.0, .16, .09, .52, .53,
0.0, 0.0, 0.0, 0.0, 0.0, 1.2, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, .16, 0.0, 1.3, 0.0, 0.0, .56,
0.0, 0.0, 0.0, 0.0, .09, 0.0, 0.0, 1.6, .11, 0.0,
.13, 0.0, 0.0, 0.0, .52, 0.0, 0.0, .11, 1.4, 0.0,
0.0, .01, 0.0, 0.0, .53, 0.0, .56, 0.0, 0.0, 3.1);
int N = M.size1(); // size of the matrix
// convert to the right format
// count the nonzero entries above diagonal
double thresh = 1e-9;
size_t nrANZ = 0; // # of nonzeros on diagonal and upper triangular part of A
for (size_t i=0; i<N; ++i) // rows
for (size_t j=i; j<N; ++j) // columns
if (fabs(M(i,j)) > thresh)
++nrANZ;
/* only the upper triangular part of A is required */
int Ap [N+1] = {0, 1, 2, 3, 4, 6, 7, 9, 11, 15, ANZ},
Ai [ANZ] = {0, 1, 2, 3, 1,4, 5, 4,6, 4,7, 0,4,7,8, 1,4,6,9 } ;
double Ax [ANZ] = {1.7, 1., 1.5, 1.1, .02,2.6, 1.2, .16,1.3, .09,1.6,
.13,.52,.11,1.4, .01,.53,.56,3.1},
b [N] = {.287, .22, .45, .44, 2.486, .72, 1.55, 1.424, 1.621, 3.759};
double Lx [LNZ], D [N], Y [N] ;
int Li [LNZ], Lp [N+1], Parent [N], Lnz [N], Flag [N], Pattern [N], d, i ;
// components to pass in
int * Ap = new int[N+1],
* Ai = new int[nrANZ];
double * Ax = new double[nrANZ],
* b = new double[N];
/* factorize A into LDL' (P and Pinv not used) */
// set ending value for Ap
Ap[N] = nrANZ;
// copy in the full A matrix to compressed column form
size_t t = 0; // count the elements added
for (size_t j=0; j<N; ++j) { // columns
Ap[j] = t; // add to the column indices
for (size_t i=0; i<=j; ++i) { // rows
const double& m = M(i,j);
if (fabs(m) > thresh) {
Ai[t] = i;
Ax[t++] = m;
}
}
}
// copy in RHS
for (size_t i = 0; i < N; ++i)
b[i] = rhs(i);
// workspace variables
double * D = new double[N],
* Y = new double[N];
int * Lp = new int [N+1],
* Parent = new int [N],
* Lnz = new int [N],
* Flag = new int [N],
* Pattern = new int [N];
// factorize A into LDL' (P and Pinv not used)
LDL_symbolic (N, Ap, Ai, Lp, Parent, Lnz, Flag, NULL, NULL);
cout << "Nonzeros in L, excluding diagonal: " << Lp [N] << endl;
d = LDL_numeric (N, Ap, Ai, Ax, Lp, Parent, Lnz, Li, Lx, D, Y, Pattern,
Flag, NULL, NULL) ;
size_t nrLNZ = Lp[N]; // # of nonzeros below the diagonal of L
if (d == N)
{
// after getting size of L, initialize storage arrays
double * Lx = new double[nrLNZ];
int * Li = new int [nrLNZ];
int d = LDL_numeric (N, Ap, Ai, Ax, Lp, Parent, Lnz, Li, Lx, D, Y, Pattern,
Flag, NULL, NULL);
if (d == N) {
/* solve Ax=b, overwriting b with the solution x */
LDL_lsolve (N, b, Lp, Li, Lx) ;
LDL_dsolve (N, b, D) ;
LDL_ltsolve (N, b, Lp, Li, Lx) ;
for (i = 0 ; i < N ; i++)
cout << "x [" << i << "] = " << b[i] << endl;
} else {
throw runtime_error("ldl_numeric failed");
}
// copy solution out
Vector result = Vector_(N, b);
// cleanup
delete[] Ap; delete[] Ai;
delete[] Ax; delete[] b;
delete[] Lx; delete[] D; delete[] Y;
delete[] Li; delete[] Lp; delete[] Parent; delete[] Lnz; delete[] Flag; delete[] Pattern;
return result;
}

1
cpp/testMatrix.cpp
View File

@ -953,6 +953,7 @@ TEST( matrix, LDL_factorization ) {
0.0, 0.0, 0.0, 0.0, .09, 0.0, 0.0, 1.6, .11, 0.0,
.13, 0.0, 0.0, 0.0, .52, 0.0, 0.0, .11, 1.4, 0.0,
0.0, .01, 0.0, 0.0, .53, 0.0, .56, 0.0, 0.0, 3.1);
Vector b = Vector_(10, .287, .22, .45, .44, 2.486, .72, 1.55, 1.424, 1.621, 3.759);
// perform LDL solving