private static Matrix4d PutTheMatrix4together(Vector3d T, Matrix3d Rotation)
{
//put the 4d matrix together
Matrix3d r3D = MatrixUtilsOpenTK.Matrix3dfromMatrix3d(Rotation);
Matrix4d myMatrix = new Matrix4d(r3D);
myMatrix[0, 3] = T.X;
myMatrix[1, 3] = T.Y;
myMatrix[2, 3] = T.Z;
myMatrix[3, 3] = 1D;
//myMatrix[0, 3] = T.X;
//myMatrix[3, 1] = T.Y;
//myMatrix[3, 2] = T.Z;
//myMatrix[3, 3] = 1D;
return(myMatrix);
}
private static double CalculateScale_Zinsser(List <Vector3d> pointsSourceShift, List <Vector3d> pointsTargetShift, ref Matrix3d R)
{
double sum1 = 0;
double sum2 = 0;
Matrix3d RT = Matrix3d.Transpose(R);
Matrix3d checkT = Matrix3d.Mult(RT, R);
Matrix4d R4D = PutTheMatrix4together(Vector3d.Zero, R);
//Matrix4d R4DT = Matrix4d.Transpose(R4D);
//double check = 0;
//Matrix4d checkIdentity = Matrix4d.Mult(R4DT, R4D);
//checkIdentity = Matrix4d.Mult(R4DT, R4D);
for (int i = 0; i < pointsSourceShift.Count; i++)
{
//Vector3d v = Vector3d.TransformNormalInverse(pointsSourceShift[i], R4D);
Vector3d v = Vector3d.TransformNormalInverse(pointsSourceShift[i], R4D);
//v = Matrix3d.Tr Vector3d.TransformNormal(pointsSourceShift[i], R4D);
v = R.TransformVector(pointsSourceShift[i]);
//v = Vector3d.TransformVector(pointsSourceShift[i], R4D);
//v = Vector3d.TransformNormalInverse(pointsSourceShift[i], R4D);
sum1 += Vector3d.Dot(v, pointsTargetShift[i]);
sum2 += Vector3d.Dot(pointsSourceShift[i], pointsSourceShift[i]);
}
double c = sum1 / sum2;
R = MatrixUtilsOpenTK.MultiplyScalar3D(R, c);
return(c);
}
public void GetOrientationWXYZ(double[] wxyz)
{
int i;
Matrix3d ortho = new Matrix3d();
for (i = 0; i < 3; i++)
{
ortho[0, i] = Matrix[0, i];
ortho[1, i] = Matrix[1, i];
ortho[2, i] = Matrix[2, i];
}
if (ortho.Determinant < 0)
{
ortho[0, i] = -ortho[0, i];
ortho[1, i] = -ortho[1, i];
ortho[2, i] = -ortho[2, i];
}
double[,] orthoArray = MatrixUtilsOpenTK.MatrixToDoubleArray(ortho);
MathUtils.Matrix3x3ToQuaternion(orthoArray, wxyz);
// calc the return value wxyz
double mag = Math.Sqrt(wxyz[1] * wxyz[1] + wxyz[2] * wxyz[2] + wxyz[3] * wxyz[3]);
if ((int)mag != 0)
{
wxyz[0] = 2.0 * Math.Acos(wxyz[0]) / MathBase.DegreesToRadians;
wxyz[1] /= mag;
wxyz[2] /= mag;
wxyz[3] /= mag;
}
else
{
wxyz[0] = 0.0;
wxyz[1] = 0.0;
wxyz[2] = 0.0;
wxyz[3] = 1.0;
}
}
private static double CalculateScale_Umeyama(List <Vector3d> pointsSourceShift, double[] eigenvalues, Matrix3d K2)
{
double sigmaSquared = 0f;
for (int i = 0; i < pointsSourceShift.Count; i++)
{
sigmaSquared += MatrixUtilsOpenTK.Norm(pointsSourceShift[i]);
}
sigmaSquared /= pointsSourceShift.Count;
double c = 0.0F;
for (int i = 0; i < 3; i++)
{
c += eigenvalues[i];
}
if (K2[2, 2] < 0)
{
c -= 2 * eigenvalues[2];
}
c = c / sigmaSquared;
return(c);
}
//other methods for SVD, for possible later usage
//MathUtils.SingularValueDecomposition3x3(Harray, Uarray, warray, VTarray);
//MathNet.Numerics.Providers.LinearAlgebra.Mkl.MklLinearAlgebraProvider svd = new MathNet.Numerics.Providers.LinearAlgebra.Mkl.MklLinearAlgebraProvider();
//double[] a = MatrixUtilsNumerics.doubleFromArrayDouble(Harray);
//double[] u = MatrixUtilsNumerics.doubleFromArrayDouble(Uarray);
//double[] vt = MatrixUtilsNumerics.doubleFromArrayDouble(Uarray);
//double[] s = new double[3];
//svd.SingularValueDecomposition(true, a, 3, 3, s, u, vt);
private static Matrix3d CalculateRotationBySingularValueDecomposition(Matrix3d H, List <Vector3d> pointsSourceShift, ICP_VersionUsed icpVersionUsed)
{
double[,] Harray = TransformPointsUtils.DoubleArrayFromMatrix(H);
double[,] Uarray = new double[3, 3];
double[,] VTarray = new double[3, 3];
double[] eigenvalues = new double[3];
//trial 3:
alglib.svd.rmatrixsvd(Harray, 3, 3, 2, 2, 2, ref eigenvalues, ref Uarray, ref VTarray);
Matrix3d U = MatrixUtilsOpenTK.DoubleArrayToMatrix3d(Uarray);
Matrix3d VT = MatrixUtilsOpenTK.DoubleArrayToMatrix3d(VTarray);
Matrix3d R = Matrix3d.Mult(U, VT);
Matrix3d UT = Matrix3d.Transpose(U);
Matrix3d V = Matrix3d.Transpose(VT);
Matrix3d Rtest = Matrix3d.Mult(UT, V);
//R = Rtest;
Matrix3d checkShouldGiveI = Matrix3d.Mult(UT, U);
checkShouldGiveI = Matrix3d.Mult(VT, V);
Matrix3d RT = Matrix3d.Transpose(R);
checkShouldGiveI = Matrix3d.Mult(RT, R);
//see article by Umeyama
//if (H.Determinant < 0)
//{
// R[2, 2] = -R[2, 2];
// //S[2, 2] = -1;
//}
//calculate the sign matrix for using the scale factor
Matrix3d K2 = Matrix3d.Identity;
double check = U.Determinant * VT.Determinant;
//if (check < 0 && Math.Abs(check) > 1E-3)
//{
// K2[2, 2] = -1;
//}
RT = Matrix3d.Transpose(R);
checkShouldGiveI = Matrix3d.Mult(RT, R);
double scale = CalculateScale_Umeyama(pointsSourceShift, eigenvalues, K2);
R = Matrix3d.Mult(R, K2);
if (icpVersionUsed == ICP_VersionUsed.Scaling_Umeyama)
{
//R = Matrix3d.Mult(R, K2);
R = MatrixUtilsOpenTK.MultiplyScalar3D(R, scale);
}
////check eigenvectors
//Matrix3d Snew = Matrix3d.Mult(U, MatrixUtilsOpenTK.Matrix3FromMatrix3d(H));
//Snew = Matrix3d.Mult(Snew, VT);
//Matrix3d si = S.Inverted();
//Matrix3d Rnew = Matrix3d.Mult(VT, si);
//Rnew = Matrix3d.Mult(Rnew, U);
//Rnew = Matrix3d.Mult(VT, S);
//Rnew = Matrix3d.Mult(Rnew, U);
return(R);
}
public void GetOrientation(double[] orientation, Matrix4 amatrix)
{
int i;
// convenient access to matrix
//double[] matrixElement = amatrix;
//float[,] ortho = new float[3, 3];
Matrix3d ortho = new Matrix3d();
for (i = 0; i < 3; i++)
{
ortho[0, i] = amatrix[0, i];
ortho[1, i] = amatrix[1, i];
ortho[2, i] = amatrix[2, i];
}
if (ortho.Determinant < 0)
{
ortho[0, 2] = -ortho[0, 2];
ortho[1, 2] = -ortho[1, 2];
ortho[2, 2] = -ortho[2, 2];
}
double[,] orthoArray = MatrixUtilsOpenTK.MatrixToDoubleArray(ortho);
MathUtils.Orthogonalize3x3(orthoArray, orthoArray);
// first rotate about y axis
double x2 = ortho[2, 0];
double y2 = ortho[2, 1];
double z2 = ortho[2, 2];
double x3 = ortho[1, 0];
double y3 = ortho[1, 1];
double z3 = ortho[1, 2];
double d1 = Math.Sqrt(x2 * x2 + z2 * z2);
double cosTheta;
double sinTheta;
if (d1 < AXIS_EPSILON)
{
cosTheta = 1.0;
sinTheta = 0.0;
}
else
{
cosTheta = z2 / d1;
sinTheta = x2 / d1;
}
double theta = Math.Atan2(sinTheta, cosTheta);
orientation[1] = -theta / MathBase.DegreesToRadians;
// now rotate about x axis
double d = Math.Sqrt(x2 * x2 + y2 * y2 + z2 * z2);
double sinPhi;
double cosPhi;
if (d < AXIS_EPSILON)
{
sinPhi = 0.0;
cosPhi = 1.0;
}
else if (d1 < AXIS_EPSILON)
{
sinPhi = y2 / d;
cosPhi = z2 / d;
}
else
{
sinPhi = y2 / d;
cosPhi = (x2 * x2 + z2 * z2) / (d1 * d);
}
double phi = Math.Atan2(sinPhi, cosPhi);
orientation[0] = phi / MathBase.DegreesToRadians;
// finally, rotate about z
double x3p = x3 * cosTheta - z3 * sinTheta;
double y3p = -sinPhi * sinTheta * x3 + cosPhi * y3 - sinPhi * cosTheta * z3;
double d2 = Math.Sqrt(x3p * x3p + y3p * y3p);
double cosAlpha;
double sinAlpha;
if (d2 < AXIS_EPSILON)
{
cosAlpha = 1.0;
sinAlpha = 0.0;
}
else
{
cosAlpha = y3p / d2;
sinAlpha = x3p / d2;
}
double alpha = Math.Atan2(sinAlpha, cosAlpha);
orientation[2] = alpha / MathBase.DegreesToRadians;
}
