public double SA_CalculateErr(double[] data) { DenseMatrix mirrorMat = Utils_PCA.getMirrorTransMat(Convert.ToInt32(data[7])); Transform transform = new Transform(); transform.DoMove(data[0], data[1], data[2]); transform.DoRotateX(data[3]); transform.DoRotateY(data[4]); transform.DoRotateZ(data[5]); transform.DoScale(data[6]); DenseMatrix transformMat = transform.GetMatrix() * mirrorMat; DenseMatrix matP_init = new DenseMatrix(4, inputMat_source.ColumnCount); inputMat_source.CopyTo(matP_init); matP_init = transformMat * matP_init; DenseMatrix matP = matP_init.SubMatrix(0, 3, 0, matP_init.ColumnCount) as DenseMatrix; if (cellIndex == null) { DenseMatrix matQ_init = inputMat_target.SubMatrix(0, 3, 0, inputMat_target.ColumnCount) as DenseMatrix; cellIndex = CellIndex.GetCellIndex(matQ_init, CellsCount); } DenseMatrix matQ = cellIndex.DoPointMatch(matP); DenseMatrix matErr = matQ - matP; double Err = 0; for (int i = 0; i < matErr.ColumnCount; i++) { Err += Math.Sqrt(matErr[0, i] * matErr[0, i] + matErr[1, i] * matErr[1, i] + matErr[2, i] * matErr[2, i]); } double absScale = Math.Abs(data[6]); Err = Err / absScale; return(Err); }
public static double CheckErr(DenseMatrix mat_source, DenseMatrix mat_target) { //4 * NSamples DenseMatrix matP = mat_source.SubMatrix(0, 3, 0, mat_source.ColumnCount) as DenseMatrix; DenseMatrix matQ_init = mat_target.SubMatrix(0, 3, 0, mat_target.ColumnCount) as DenseMatrix; CellIndex cellIndex = CellIndex.GetCellIndex(matQ_init, 4); DenseMatrix matQ = cellIndex.DoPointMatch(matP); DenseMatrix matErr = matQ - matP; double Err = 0; for (int i = 0; i < matErr.ColumnCount; i++) { Err += Math.Sqrt(matErr[0, i] * matErr[0, i] + matErr[1, i] * matErr[1, i] + matErr[2, i] * matErr[2, i]); } return(Err); }
public static DenseMatrix SVDGetTransMat(DenseMatrix mat_source, DenseMatrix mat_target, ref CellIndex cellIndex, out double ErrValue) { //4 * NSamples int CellsCount = 8; DenseMatrix matP = mat_source.SubMatrix(0, 3, 0, mat_source.ColumnCount) as DenseMatrix; if (cellIndex == null) { DenseMatrix matQ_init = mat_target.SubMatrix(0, 3, 0, mat_target.ColumnCount) as DenseMatrix; cellIndex = CellIndex.GetCellIndex(matQ_init, CellsCount); } DenseMatrix matQ = cellIndex.DoPointMatch(matP); DenseMatrix matErr = matQ - matP; ErrValue = 0; for (int i = 0; i < matErr.ColumnCount; i++) { ErrValue += Math.Sqrt(matErr[0, i] * matErr[0, i] + matErr[1, i] * matErr[1, i] + matErr[2, i] * matErr[2, i]); } DenseMatrix matP_Mean = Utils_PCA.getMeanMat(matP); DenseMatrix matQ_Mean = Utils_PCA.getMeanMat(matQ); DenseMatrix matT_MoveP = DenseMatrix.CreateIdentity(4); DenseMatrix matT_MoveQ = DenseMatrix.CreateIdentity(4); for (int i = 0; i < 3; i++) { matT_MoveP[i, 3] = matP_Mean[i, 0]; matT_MoveQ[i, 3] = matQ_Mean[i, 0]; } matP = matP - matP_Mean; matQ = matQ - matQ_Mean; DenseMatrix matM = matP * matQ.Transpose() as DenseMatrix; //matM.SplitUV(matU, matV, 0.01); MathNet.Numerics.LinearAlgebra.Factorization.Svd <double> svd = matM.Svd(true); DenseMatrix matU = svd.U as DenseMatrix; DenseMatrix matVT = svd.VT as DenseMatrix; DenseMatrix matR = (matU * matVT).Transpose() as DenseMatrix; DenseMatrix matT = DenseMatrix.CreateIdentity(4); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { matT[i, j] = matR[i, j]; } } matT = matT_MoveP.Inverse() * matT * matT_MoveQ as DenseMatrix; return(matT); }