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 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]; }