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