void precon_lusgs(SpMatrix* A, const Matrix<double>& r, Matrix<double>& z)
// Multiplies r by the LU-SGS preconditioner matrix of A, and stores the result in z
{
SpMatrix L;
SpMatrix U;
Matrix<double> D(A->rows(), 1);
Matrix<double> Dinv(A->rows(), 1);
Matrix<double> z_initial(z.rows(), 1);
for(int i = 0; i < z.rows(); i++)
z_initial(i) = z.get(i);
A->get_diagonal(&D);
for(int i = 0; i < A->rows(); i++)
Dinv(i) = 1.0/D(i);
A->get_lower_triangle(L);
A->get_upper_triangle(U);
double temp = 0;
Matrix<double> zold(A->rows(),1);
// solve (D+L)*zold = r by forward substitution
}
void cisstAlgorithmICP_IMLP::ComputeCovDecomposition_NonIter(const vct3x3 &M, vct3x3 &Minv, vct3x3 &N, vct3x3 &Ninv, double &det_M)
{
// Compute eigen decomposition of M
// M = V*diag(S)*V'
vct3 eigenValues;
vct3x3 eigenVectors;
ComputeCovEigenDecomposition_NonIter(M, eigenValues, eigenVectors);
// Compute Minv
// Minv = V*diag(1/S)*V'
static vctFixedSizeMatrix<double, 3, 3, VCT_COL_MAJOR> V_Sinv;
static vct3 Sinv;
Sinv[0] = 1.0 / eigenValues[0];
Sinv[1] = 1.0 / eigenValues[1];
Sinv[2] = 1.0 / eigenValues[2];
V_Sinv.Column(0) = eigenVectors.Column(0)*Sinv[0];
V_Sinv.Column(1) = eigenVectors.Column(1)*Sinv[1];
V_Sinv.Column(2) = eigenVectors.Column(2)*Sinv[2];
Minv.Assign(V_Sinv * eigenVectors.TransposeRef());
// Compute Decomposition of Minv = N'*N
// Minv = R*D^2*R' = N'*N M = R*Dinv^2*R' => R' = V', Dinv = sqrt(S)
// N = D*R' Ninv = R*inv(D)
vct3 Dinv(
sqrt(eigenValues[0]),
sqrt(eigenValues[1]),
sqrt(eigenValues[2])
);
N.Row(0) = eigenVectors.Column(0) / Dinv[0];
N.Row(1) = eigenVectors.Column(1) / Dinv[1];
N.Row(2) = eigenVectors.Column(2) / Dinv[2];
Ninv.Column(0) = eigenVectors.Column(0)*Dinv[0];
Ninv.Column(1) = eigenVectors.Column(1)*Dinv[1];
Ninv.Column(2) = eigenVectors.Column(2)*Dinv[2];
// Compute determinant of M
det_M = eigenValues.ProductOfElements();
}
// Note: this function depends on the SamplePreMatch() function
// to set the noise model of the current transformed sample
// point before this function is called
void cisstAlgorithmICP_IMLP::ComputeNodeMatchCov( cisstCovTreeNode *node )
{
// Note: This function is called when searching a node that is using
// its own noise model rather than that of its parent node
// Compute the effective noise model for this node, assuming the noise
// model of the transformed sample point has already been computed
// noise model of transformed sample
M = sample_RMxRt_sigma2;
// add the effective My for this node
M.Element(0,0) += node->EigMax;
M.Element(1,1) += node->EigMax;
M.Element(2,2) += node->EigMax;
// TODO: can this be done using only the eigen decomposition
// of RMxRt
// Compute Decomposition of M
// M = V*S*V'
vct3 eigenValues;
vct3x3 eigenVectors;
ComputeCovEigenDecomposition_NonIter(M, eigenValues, eigenVectors);
// Compute Decomposition of Minv = N'*N
// Minv = R*D^2*R' = N'*N M = R*Dinv^2*R' => R' = V', Dinv = sqrt(S)
// N = D*R' Ninv = R*inv(D)
vct3 Dinv(
sqrt(eigenValues[0]),
sqrt(eigenValues[1]),
sqrt(eigenValues[2])
);
N.Row(0) = eigenVectors.Column(0) / Dinv[0];
N.Row(1) = eigenVectors.Column(1) / Dinv[1];
N.Row(2) = eigenVectors.Column(2) / Dinv[2];
//N.Row(0) = eigenVectors.TransposeRef().Row(0) / Dinv[0];
//N.Row(1) = eigenVectors.TransposeRef().Row(1) / Dinv[1];
//N.Row(2) = eigenVectors.TransposeRef().Row(2) / Dinv[2];
Dmin = 1.0/Dinv[0]; // eigenvalues are automatically arranged in order of decreasing magnitude
//// Compute Decomposition of M
//static vctFixedSizeMatrix<double, 3, 3, VCT_COL_MAJOR> A;
//static vctFixedSizeMatrix<double, 3, 3, VCT_COL_MAJOR> U;
//static vctFixedSizeMatrix<double, 3, 3, VCT_COL_MAJOR> Vt;
//static vct3 S;
//static nmrSVDFixedSizeData<3, 3, VCT_COL_MAJOR>::VectorTypeWorkspace workspace;
//try
//{
// A.Assign(M);
// nmrSVD(A, U, S, Vt, workspace);
//}
//catch (...)
//{
// assert(0);
//}
//// Compute Decomposition of Minv = N'*N
//// Minv = R*D^2*R' = N'*N M = R*Dinv^2*R' => R' = V', Dinv = sqrt(S)
//// N = D*R'
//// Ninv = R*inv(D)
//static vct3 Dinv;
//Dinv[0] = sqrt(S[0]);
//Dinv[1] = sqrt(S[1]);
//Dinv[2] = sqrt(S[2]);
//N.Row(0) = Vt.Row(0) / Dinv[0];
//N.Row(1) = Vt.Row(1) / Dinv[1];
//N.Row(2) = Vt.Row(2) / Dinv[2];
//Dmin = 1.0 / Dinv[0]; // eigenvalues are automatically arranged in order of decreasing magnitude
}
