void cisstAlgorithmICP_IMLP::ComputeCovDecomposition_SVD( const vct3x3 &M, vct3x3 &Minv, double &det_M )
{
// Compute SVD 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 Minv
// M = U*diag(S)*V' where U = V
// Minv = V*diag(1/S)*U' = U*diag(1/S)*V'
static vctFixedSizeMatrix<double,3,3,VCT_COL_MAJOR> Sinv_Ut;
static vct3 Sinv;
Sinv[0] = 1/S[0];
Sinv[1] = 1/S[1];
Sinv[2] = 1/S[2];
Sinv_Ut.Row(0) = Sinv[0]*Vt.Row(0);
Sinv_Ut.Row(1) = Sinv[1]*Vt.Row(1);
Sinv_Ut.Row(2) = Sinv[2]*Vt.Row(2);
Minv.Assign(U*Sinv_Ut);
// Compute determinant of M
det_M = S[0]*S[1]*S[2];
}
void vctDeterminantTest::TestDeterminant2x2ByInverse(const vctFixedSizeMatrix<ElementType, 2, 2> & inputMatrix)
{
const ElementType determinant = vctDeterminant<2>::Compute(inputMatrix);
const ElementType tolerance = cmnTypeTraits<ElementType>::Tolerance();
if (fabs(determinant) < tolerance) {
vctFixedSizeVector<ElementType, 2> rowRatio;
rowRatio.ElementwiseRatioOf( inputMatrix.Row(0), inputMatrix.Row(1) );
CPPUNIT_ASSERT_DOUBLES_EQUAL( rowRatio[0], rowRatio[1], tolerance );
vctFixedSizeVector<ElementType, 2> columnRatio;
columnRatio.ElementwiseRatioOf( inputMatrix.Column(0), inputMatrix.Column(1) );
CPPUNIT_ASSERT_DOUBLES_EQUAL( columnRatio[0], columnRatio[1], tolerance );
return;
}
const vctFixedSizeMatrix<ElementType, 2, 2> inverse(
inputMatrix[1][1] / determinant, -inputMatrix[0][1] / determinant,
-inputMatrix[1][0] / determinant, inputMatrix[0][0] / determinant
);
/* gcc (Ubuntu 4.4.3-4ubuntu5) 4.4.3 crashes with internal error
on ubuntu 64 - Ubuntu 10.04.1 LTS with const */
/* const */ vctFixedSizeMatrix<ElementType, 2, 2> product( inputMatrix * inverse );
const vctFixedSizeMatrix<ElementType, 2, 2> identity( ElementType(1), ElementType(0), ElementType(0), ElementType(1) );
const vctFixedSizeMatrix<ElementType, 2, 2> difference = product - identity;
ElementType diffNorm = difference.Norm();
CPPUNIT_ASSERT_DOUBLES_EQUAL( 0, diffNorm, tolerance );
}
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();
}
void cisstAlgorithmICP_IMLP::ComputeCovDecomposition_NonIter(const vct3x3 &M, vct3x3 &Minv, 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 determinant of M
det_M = eigenValues.ProductOfElements();
}
void cisstAlgorithmICP_IMLP::ComputeCovDecomposition_SVD( const vct3x3 &M, vct3x3 &Minv,
vct3x3 &N, vct3x3 &Ninv, double &det_M )
{
// Compute SVD 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 Minv
// M = U*diag(S)*V' where U = V
// Minv = V*diag(1/S)*U' = U*diag(1/S)*V'
static vctFixedSizeMatrix<double,3,3,VCT_COL_MAJOR> Sinv_Ut;
static vct3 Sinv;
Sinv[0] = 1/S[0];
Sinv[1] = 1/S[1];
Sinv[2] = 1/S[2];
Sinv_Ut.Row(0) = Sinv[0]*Vt.Row(0);
Sinv_Ut.Row(1) = Sinv[1]*Vt.Row(1);
Sinv_Ut.Row(2) = Sinv[2]*Vt.Row(2);
Minv.Assign(U*Sinv_Ut);
// Compute Decomposition of Minv = N'*N
// Minv = R*D^2*R' = N'*N
// N = D*R'
// Ninv = R*inv(D)
static vct3 Dinv; //,D;
Dinv[0] = sqrt(S[0]);
Dinv[1] = sqrt(S[1]);
Dinv[2] = sqrt(S[2]);
//D[0] = 1/Dinv[0];
//D[1] = 1/Dinv[1];
//D[2] = 1/Dinv[2];
//N.Row(0) = D[0]*Vt.Row(0);
//N.Row(1) = D[1]*Vt.Row(1);
//N.Row(2) = D[2]*Vt.Row(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];
Ninv.Column(0) = U.Column(0)*Dinv[0];
Ninv.Column(1) = U.Column(1)*Dinv[1];
Ninv.Column(2) = U.Column(2)*Dinv[2];
// Compute determinant of M
det_M = S[0]*S[1]*S[2];
}
