private bool initICP() { if (TWO_MESHS_sourceObj == null || TWO_MESHS_targetObj == null) { Form_ICPMeshSelection form_Selection = new Form_ICPMeshSelection(geometryRoot); if (form_Selection.ShowDialog() != DialogResult.OK) { return(false); } TWO_MESHS_sourceObj = geometryRoot[form_Selection.source]; TWO_MESHS_targetObj = geometryRoot[form_Selection.target]; SVDLoops = form_Selection.SVDLoops; SourceSamples = form_Selection.SourceSamples; TargetSamples = form_Selection.TargetSamples; } TWO_MESHS_sourceObj.Transform = new Transform(); TWO_MESHS_targetObj.Transform = new Transform(); inputMat_source = TWO_MESHS_sourceObj.ToSampledDataMat(SourceSamples); inputMat_target = TWO_MESHS_targetObj.ToSampledDataMat(TargetSamples); //checkMat_source = TWO_MESHS_sourceObj.ToSampledDataMat(NCheck); //checkMat_target = TWO_MESHS_targetObj.ToSampledDataMat(NCheck); PCATrans_source = Utils_PCA.DoPCA(inputMat_source); PCATrans_target = Utils_PCA.DoPCA(inputMat_target); //PCATrans_source = correctPCATransform(inputMat_source, inputMat_target, PCATrans_source, PCATrans_target); PCA_InvTransMat_source = PCATrans_source.GetMatrix().Inverse() as DenseMatrix; PCA_TransMat_target = PCATrans_target.GetMatrix(); PCA_InvTransMat_target = PCA_TransMat_target.Inverse() as DenseMatrix; inputMat_source = PCA_InvTransMat_source * inputMat_source; inputMat_target = PCA_InvTransMat_target * inputMat_target; errList = new List <double>(); SVD_TransMat = DenseMatrix.CreateIdentity(4); return(true); }
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); }
private void endSA(int iloop) { double[] data = sa_Processor.currentMinData; 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; TWO_MESHS_sourceObj.Transform = new Transform(PCA_TransMat_target * transformMat * PCA_InvTransMat_source); DenseMatrix checkSourceMat = PCA_TransMat_target * transformMat * PCA_InvTransMat_source * TWO_MESHS_sourceObj.ToDataMat(); DenseMatrix checkSourceMat_PCA = PCA_TransMat_target * PCA_InvTransMat_source * TWO_MESHS_sourceObj.ToDataMat(); int timeDelta = EndTime - StartTime; double err_pca = Utils_CheckErr.CheckErr(checkSourceMat_PCA, TWO_MESHS_targetObj.ToDataMat()); double err = Utils_CheckErr.CheckErr(checkSourceMat, TWO_MESHS_targetObj.ToDataMat()); string str = "Time: " + timeDelta.ToString() + "\n"; str += "Err_PCA: " + err_pca.ToString() + "\n"; str += "Err: " + err.ToString() + "\n"; foreach (double rec in sa_Processor.Record) { str += rec.ToString() + "\n"; } StreamWriter writer = new StreamWriter("d:/rec_sa_" + TargetSamples.ToString() + "_" + SourceSamples.ToString() + "_" + iloop.ToString() + ".txt"); writer.Write(str); writer.Close(); }
private bool initSA() { if (TWO_MESHS_sourceObj == null || TWO_MESHS_targetObj == null) { Form_SAMeshSelection form_Selection = new Form_SAMeshSelection(geometryRoot); if (form_Selection.ShowDialog() != DialogResult.OK) { return(false); } TWO_MESHS_sourceObj = geometryRoot[form_Selection.source]; TWO_MESHS_targetObj = geometryRoot[form_Selection.target]; SourceSamples = form_Selection.SourceSamples; TargetSamples = form_Selection.TargetSamples; } TWO_MESHS_sourceObj.Transform = new Transform(); TWO_MESHS_targetObj.Transform = new Transform(); inputMat_source = TWO_MESHS_sourceObj.ToSampledDataMat(SourceSamples); inputMat_target = TWO_MESHS_targetObj.ToSampledDataMat_WithNormal(TargetSamples, out inputNormalMat_target); PCATrans_source = Utils_PCA.DoPCA(inputMat_source); PCATrans_target = Utils_PCA.DoPCA(inputMat_target); //PCATrans_source = correctPCATransform(inputMat_source, inputMat_target, PCATrans_source, PCATrans_target); PCA_InvTransMat_source = PCATrans_source.GetMatrix().Inverse() as DenseMatrix; PCA_TransMat_target = PCATrans_target.GetMatrix(); PCA_InvTransMat_target = PCA_TransMat_target.Inverse() as DenseMatrix; inputMat_source = PCA_InvTransMat_source * inputMat_source; inputMat_target = PCA_InvTransMat_target * inputMat_target; inputNormalMat_target = PCATrans_target.ToRotationMatrix().Inverse() * inputNormalMat_target as DenseMatrix; return(true); }
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); }