/// <summary>
/// each colomn is one vertex - CORRECTED
/// </summary>
/// <param name="PointsModel1"></param>
/// <param name="PointsModel2"></param>
/// <param name="p_mRotationMatrix"></param>
/// <param name="p_mTranslationMatrix"></param>
public static void CalculateRotationAndTranslation(Matrix PointsModel1, Matrix PointsModel2, out Iridium.Numerics.LinearAlgebra.Matrix p_mRotationMatrix, out Iridium.Numerics.LinearAlgebra.Matrix p_mTranslationMatrix)
{
if (PointsModel1.ColumnCount != PointsModel2.ColumnCount || PointsModel1.RowCount != PointsModel2.RowCount)
throw new Exception("Two Matrixes do not have the same count");
Matrix MeanValModel1 = new Matrix(PointsModel1.RowCount, 1);
Matrix MeanValModel2 = new Matrix(PointsModel1.RowCount, 1);
for (int j = 0; j < PointsModel1.RowCount; j++)
{
for (int i = 0; i < PointsModel1.ColumnCount; i++)
{
MeanValModel1[j, 0] += PointsModel1[j, i];
MeanValModel2[j, 0] += PointsModel2[j, i];
}
}
MeanValModel1 *= (1.0f / PointsModel1.ColumnCount);
MeanValModel2 *= (1.0f / PointsModel1.ColumnCount);
Matrix CenteredModel1 = new Matrix(PointsModel1.RowCount, PointsModel1.ColumnCount);
Matrix CenteredModel2 = new Matrix(PointsModel1.RowCount, PointsModel1.ColumnCount);
for (int i = 0; i < PointsModel1.RowCount; i++)
{
for (int j = 0; j < PointsModel1.ColumnCount; j++)
{
CenteredModel1[i, j] = PointsModel1[i, j] - MeanValModel1[i, 0];
CenteredModel2[i, j] = PointsModel2[i, j] - MeanValModel2[i, 0];
}
}
CenteredModel2.Transpose();
Iridium.Numerics.LinearAlgebra.Matrix Covariance = CenteredModel1 * CenteredModel2;
Iridium.Numerics.LinearAlgebra.SingularValueDecomposition SVD = new Iridium.Numerics.LinearAlgebra.SingularValueDecomposition(Covariance);
Iridium.Numerics.LinearAlgebra.Matrix U = SVD.LeftSingularVectors;
Iridium.Numerics.LinearAlgebra.Matrix V = SVD.RightSingularVectors;
Iridium.Numerics.LinearAlgebra.Matrix s = new Iridium.Numerics.LinearAlgebra.Matrix(PointsModel1.RowCount, 1.0);
if (Covariance.Rank() < 2)
throw new Exception("Cannot allign generic model (cov rank is less than 2)");
if (Covariance.Rank() == 2) // m-1 where m is dimension space (3D)
{
double detU = Math.Round(U.Determinant());
double detV = Math.Round(V.Determinant());
double detC = Covariance.Determinant();
if ((int)detU * (int)detV == 1)
s[PointsModel1.RowCount - 1, PointsModel1.RowCount - 1] = 1;
else if ((int)detU * (int)detV == -1)
s[PointsModel1.RowCount - 1, PointsModel1.RowCount - 1] = -1;
else
throw new Exception("Determinant of U and V are not in conditions");
}
else
{
if (Covariance.Determinant() < 0)
s[PointsModel1.RowCount - 1, PointsModel1.RowCount - 1] = -1;
}
V.Transpose();
Iridium.Numerics.LinearAlgebra.Matrix Rotation = U * s * V;
Iridium.Numerics.LinearAlgebra.Matrix Translation = MeanValModel1 - Rotation * MeanValModel2;
p_mRotationMatrix = Rotation;
p_mTranslationMatrix = Translation;
}
public static void CalculateRotationAndTranslation(List<Cl3DModel.Cl3DModelPointIterator> p_ListOfPointsFrom1Model, List<Cl3DModel.Cl3DModelPointIterator> p_ListOfPointsFrom2Model, out Iridium.Numerics.LinearAlgebra.Matrix p_mRotationMatrix, out Iridium.Numerics.LinearAlgebra.Matrix p_mTranslationMatrix)
{
if (p_ListOfPointsFrom1Model.Count != p_ListOfPointsFrom2Model.Count)
throw new Exception("Both models should have the sime number of points");
Matrix Model1 = new Matrix(3, p_ListOfPointsFrom1Model.Count);
Matrix Model2 = new Matrix(3, p_ListOfPointsFrom2Model.Count);
for (int i = 0; i < p_ListOfPointsFrom1Model.Count; i++)
{
Model1[0, i] = p_ListOfPointsFrom1Model[i].X;
Model1[1, i] = p_ListOfPointsFrom1Model[i].Y;
Model1[2, i] = p_ListOfPointsFrom1Model[i].Z;
Model2[0, i] = p_ListOfPointsFrom2Model[i].X;
Model2[1, i] = p_ListOfPointsFrom2Model[i].Y;
Model2[2, i] = p_ListOfPointsFrom2Model[i].Z;
}
CalculateRotationAndTranslation(Model1, Model2, out p_mRotationMatrix, out p_mTranslationMatrix);
}
