public static CellIndex GetCellIndex(DenseMatrix mat_target, int minCellCount)
{
double[] Min = { mat_target[0, 0], mat_target[1, 0], mat_target[2, 0] };
double[] Max = { mat_target[0, 0], mat_target[1, 0], mat_target[2, 0] };
for (int i = 0; i < mat_target.ColumnCount; i++)
{
Min[0] = Math.Min(Min[0], mat_target[0, i]);
Min[1] = Math.Min(Min[1], mat_target[1, i]);
Min[2] = Math.Min(Min[2], mat_target[2, i]);
Max[0] = Math.Max(Max[0], mat_target[0, i]);
Max[1] = Math.Max(Max[1], mat_target[1, i]);
Max[2] = Math.Max(Max[2], mat_target[2, i]);
}
CellIndex cellIndex = new CellIndex(Min, Max, minCellCount);
for (int i = 0; i < mat_target.ColumnCount; i++)
{
double[] p = { mat_target[0, i], mat_target[1, i], mat_target[2, i] };
cellIndex.InsertPoint(p);
}
return(cellIndex);
}
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);
}