Skip to content

Instantly share code, notes, and snippets.

@Abdo1994
Created April 26, 2016 07:50
Show Gist options
  • Save Abdo1994/8edf97c0c3997bbff0f496b92b8a93e3 to your computer and use it in GitHub Desktop.
Save Abdo1994/8edf97c0c3997bbff0f496b92b8a93e3 to your computer and use it in GitHub Desktop.
using System;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
namespace HelloWorld
{
class Hello
{
public static void Main()
{
Console.WriteLine("Hi");
Matrix<double> B = DenseMatrix.OfArray(new double[4, 3]{
{-1.37285008021333,-2.39065444083597,-0.272229060927326},
{-1.49265215830869,1.48381481188292,0.689109865473517},
{2.69081228630851,-2.14018577957389,-0.326988358173307},
{2.41142646810131,1.78497640136291,0.805298962324319},});
Matrix<double> A = DenseMatrix.OfArray(new double[4, 3] {
{-1.93017948067421,-2.05741846282552,0.460712729349417},
{-2.11572517274675,2.01370167038341,0.420172921759525},
{2.08424703950644,-2.15648187564387,-0.485352019388814},
{1.95017255998246,2.22306603146633,-0.272585072006361}});
int N = B.RowCount; //Total number of points
Vector<double> X = A.ColumnSums() / N; // Mean(A)
Vector<double> Y = B.ColumnSums() / N; // Mean(B)
Matrix<double> centroid_A = DenseMatrix.OfArray(new double[N, 3]);
for (int i = 0; i < N; i++)
{
centroid_A = centroid_A.InsertRow(i, X);
}
centroid_A = centroid_A.SubMatrix(0, N, 0, 3); //centroid_A Nx3 of mean A values
Matrix<double> centroid_B = DenseMatrix.OfArray(new double[N, 3]);
for (int i = 0; i < N; i++)
{
centroid_B = centroid_B.InsertRow(i, Y);
}
centroid_B = centroid_B.SubMatrix(0, N, 0, 3); //centroid_B Nx3 of mean B values
Matrix<double> x1 = DenseMatrix.OfArray(new double[N, 3]);
x1 = A - centroid_A;
Matrix<double> x2 = DenseMatrix.OfArray(new double[N, 3]);
x2 = B - centroid_B;
Matrix<double> H;
H = x1.Transpose() * x2; // Computing H
var svd = H.Svd(); // Singular Value Decomposition
var S = svd.S;
var U = svd.U;
var V = svd.VT;
Matrix<double> R;
R = V.Transpose() * U.Transpose(); //Rotation Matrix
Matrix<double> t;
t = -1 * R * centroid_A.Transpose() + centroid_B.Transpose();
Vector<double> t1;
t1 = t.Column(0); // Translation Vector
Matrix<double> final_matrix = DenseMatrix.OfArray(new double[4, 4]);
final_matrix = R;
final_matrix = final_matrix.InsertColumn(3, t1);
double[] end = new double[4] { 0, 0, 0, 1 };
Vector<double> end1 = DenseVector.OfArray(end);
final_matrix = final_matrix.InsertRow(3, end1); // 4x4 Transformation matrix
Console.WriteLine(final_matrix.ToString()); // Print the Matrix
Console.WriteLine("Press enter to close...");
Console.ReadLine();
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment