private void button1_Click(object sender, EventArgs e)
{
var m = Matrix <double> .Build;
int max_itr = 5000;
double grid = 0.1;
double err = 0;
int Np = 500;
double threshold = 0.00001;
Matrix <double> R = m.Dense(3, 3);
Matrix <double> t = m.Dense(3, 1);
double offset_x = 0, offset_y = 120;
double alfa = 0;
offset_x = Convert.ToDouble(textBox1.Text);
offset_y = Convert.ToDouble(textBox2.Text);
alfa = Convert.ToDouble(textBox3.Text);
Matrix <double> M_points = CreateCircle(grid, Np, 10, 350);
Matrix <double> P_points = addMotion(M_points, offset_x, offset_y, alfa);
Random r = new Random();
for (int i = 0; i < Np; i++)
{
P_points[0, i] += (r.NextDouble() - 0.5) * 5;
P_points[1, i] += (r.NextDouble() - 0.5) * 5;
}
Bitmap flag = new Bitmap(1000, 1000);
Graphics g = Graphics.FromImage(flag);
g.Clear(Color.Black);
pictureBox1.Image = flag;
DrawPoint2(P_points, Brushes.Red, pictureBox1, g);
DrawPoint2(M_points, Brushes.Green, pictureBox1, g);
System.Threading.Thread.Sleep(1500);
Tuple <Matrix <double>, Matrix <double>, double> ret;
Stopwatch watch = new Stopwatch();
Stopwatch watch2 = new Stopwatch();
double time = 0;
for (int itr = 1; itr <= max_itr; itr++)
{
watch.Restart();
ret = ICP.ICP_run(M_points, P_points, false);
R = ret.Item1;
t = ret.Item2;
err = ret.Item3;
Matrix <double> Third_raw = m.Dense(1, Np, 0);
Matrix <double> Px = m.Dense(1, Np);
Matrix <double> Py = m.Dense(1, Np);
Matrix <double> P_points2;
P_points2 = R.Multiply(P_points.Stack(Third_raw));
Px.SetRow(0, P_points2.Row(0));
Py.SetRow(0, P_points2.Row(1));
Px = Px + t[0, 0];
Py = Py + t[1, 0];
P_points.SetRow(0, Px.Row(0));
P_points.SetRow(1, Py.Row(0));
watch.Stop();
time += watch.ElapsedMilliseconds;
g.Clear(Color.Black);
pictureBox1.Image = flag;
DrawPoint2(P_points, Brushes.Red, pictureBox1, g);
DrawPoint2(M_points, Brushes.Green, pictureBox1, g);
if (err < threshold || Math.Abs(previous_error - err) < delta_error_thresh)
{
Debug.WriteLine("error: " + err);
Debug.WriteLine("iteration= " + itr);
break;
}
previous_error = err;
}
Debug.WriteLine("Time=" + time + "ms");
}
/// <summary>
/// This Function Calculates Final P_points under the conditions of either threshold or maximum iterations that given as input.
/// Returns matched P_points.
/// </summary>
/// <param name="M_points"></param>
/// <param name="P_points"></param>
/// <param name="threshold"></param>
/// <param name="max_iterations"></param>
/// <param name="_3D"></param>
/// <returns></returns>
public static Matrix <double> ICP_run(Matrix <double> M_points, Matrix <double> P_points, double threshold, int max_iterations, bool _3D)
{
#region "Definitions"
var m = Matrix <double> .Build;
int Np = P_points.ColumnCount;
Matrix <double> dummy_p1 = m.Dense(1, Np);
Matrix <double> dummy_p2 = m.Dense(1, Np);
Matrix <double> dummy_p3 = m.Dense(1, Np, 0);
Matrix <double> dummy_y1 = m.Dense(1, Np);
Matrix <double> dummy_y2 = m.Dense(1, Np);
Matrix <double> dummy_y3 = m.Dense(1, Np, 0);
Matrix <double> Px = m.Dense(1, Np);
Matrix <double> Py = m.Dense(1, Np);
Matrix <double> Pz = m.Dense(1, Np, 0);
Matrix <double> Yx = m.Dense(1, Np);
Matrix <double> Yy = m.Dense(1, Np);
Matrix <double> Yz = m.Dense(1, Np, 0);
Matrix <double> dummy_Row = m.Dense(1, Np, 0);
Matrix <double> Third_raw = m.Dense(1, Np, 0); ///dummy row to be used in calculations
Matrix <double> Px2 = m.Dense(1, Np);
Matrix <double> Py2 = m.Dense(1, Np);
Matrix <double> Pz2 = m.Dense(1, Np);
Matrix <double> P_points2;
double s = 1;
double delta_error_thresh = 0.00001;
double previous_error = 0;
Matrix <double> R;
Matrix <double> t;
double err = 0;
Matrix <double> Y;
Vector <double> d;
#endregion
for (int itr = 1; itr <= max_iterations; itr++)
{
Y = KD_tree(M_points, P_points);
///Calculate Centroid for both point clouds
///P ve Y matrislerinin agirlik merkezi hesaplaniyor
Matrix <double> Mu_p = FindCentroid(P_points);
Matrix <double> Mu_y = FindCentroid(Y);
///P matrisinin X ve Y koordinatlarini iceren satirlari farkli matrislere aliniyor
dummy_p1.SetRow(0, P_points.Row(0));
dummy_p2.SetRow(0, P_points.Row(1));
if (_3D)
{
dummy_p3.SetRow(0, P_points.Row(2));
}
///P_points is moved to origin by subtructing centroid from every element.
/// P deki her bir noktadan p nin agirlik merkezinin koordinatlari cikartiliyor(ZERO MEAN) yeni bir matris icerisine kaydediliyor.
Matrix <double> P_prime = (dummy_p1 - Mu_p[0, 0]).Stack((dummy_p2 - Mu_p[1, 0]).Stack(dummy_p3 - Mu_p[2, 0]));
///Calculate Centroid for both point clouds
///Y matrisinin X ve Y koordinatlarini iceren satirlari farkli matrislere aliniyor
dummy_y1.SetRow(0, Y.Row(0));
dummy_y2.SetRow(0, Y.Row(1));
if (_3D)
{
dummy_y3.SetRow(0, Y.Row(2));
}
///M_points is moved to origin by subtructing centroid from every element.
/// P deki her bir noktadan p nin agirlik merkezinin koordinatlari cikartiliyor(ZERO MEAN) yeni bir matris icerisine kaydediliyor.
Matrix <double> Y_prime = (dummy_y1 - Mu_y[0, 0]).Stack((dummy_y2 - Mu_y[1, 0]).Stack(dummy_y3 - Mu_y[2, 0]));
/// -X -Y -Z koordinat matrisleri aliniyor.
Px.SetRow(0, P_prime.Row(0));
Py.SetRow(0, P_prime.Row(1));
if (_3D)
{
Pz.SetRow(0, P_prime.Row(2));
Yz.SetRow(0, Y_prime.Row(2));
}
Yx.SetRow(0, Y_prime.Row(0));
Yy.SetRow(0, Y_prime.Row(1));
var Sxx = Px * Yx.Transpose();
var Sxy = Px * Yy.Transpose();
var Sxz = Px * Yz.Transpose();
var Syx = Py * Yx.Transpose();
var Syy = Py * Yy.Transpose();
var Syz = Py * Yz.Transpose();
var Szx = Pz * Yx.Transpose();
var Szy = Pz * Yy.Transpose();
var Szz = Pz * Yz.Transpose();
Matrix <double> Nmatrix = m.DenseOfArray(new[, ] {
{ Sxx[0, 0] + Syy[0, 0] + Szz[0, 0], Syz[0, 0] - Szy[0, 0], -Sxz[0, 0] + Szx[0, 0], Sxy[0, 0] - Syx[0, 0] },
{ -Szy[0, 0] + Syz[0, 0], Sxx[0, 0] - Syy[0, 0] - Szz[0, 0], Sxy[0, 0] + Syx[0, 0], Sxz[0, 0] + Szx[0, 0] },
{ Szx[0, 0] - Sxz[0, 0], Syx[0, 0] + Sxy[0, 0], -Sxx[0, 0] + Syy[0, 0] - Szz[0, 0], Syz[0, 0] + Szy[0, 0] },
{ -Syx[0, 0] + Sxy[0, 0], Szx[0, 0] + Sxz[0, 0], Szy[0, 0] + Syz[0, 0], -Sxx[0, 0] + Szz[0, 0] - Syy[0, 0] }
});
var evd = Nmatrix.Evd();
Matrix <double> eigenvectors = evd.EigenVectors;
var q = eigenvectors.Column(3);
var q0 = q[0]; var q1 = q[1]; var q2 = q[2]; var q3 = q[3];
///Quernion matrix is calculated
///Quernion matrislerinin bulunmasi
var Qbar = m.DenseOfArray(new[, ] {
{ q0, -q1, -q2, -q3 },
{ q1, q0, q3, -q2 },
{ q2, -q3, q0, q1 },
{ q3, q2, -q1, q0 }
});
var Q = m.DenseOfArray(new[, ] {
{ q0, -q1, -q2, -q3 },
{ q1, q0, -q3, q2 },
{ q2, q3, q0, -q1 },
{ q3, -q2, q1, q0 }
});
///Calculating Rotation matrix
///Rotasyon matrisi hesabi
R = (Qbar.Transpose()).Multiply(Q);
R = (R.RemoveColumn(0)).RemoveRow(0);
///Translation hesabi
t = Mu_y - s * R * Mu_p;
///SCALE Factor hesabi
//
///
///Transformation is applied to P_points
///Hesaplanan değerler P_points matrisine uygulanıyor
if (!_3D)
{
P_points2 = R.Multiply(P_points.Stack(Third_raw));
Px2.SetRow(0, P_points2.Row(0));
Py2.SetRow(0, P_points2.Row(1));
//Pz2.SetRow(0, P_points2.Row(2));
Px2 = Px2 + t[0, 0];
Py2 = Py2 + t[1, 0];
//Pz2=Pz2 + t[2, 0];
P_points.SetRow(0, Px2.Row(0));
P_points.SetRow(1, Py2.Row(0));
//P_points.SetRow(2, Pz2.Row(0));
Matrix <double> pp = P_points.Stack(Third_raw); ///For 3-D clouds, this line should be commentted out and replace the pp with P_points in following lines
///error calculation
///hata hesabi
for (int i = 0; i < Np; i++)
{
d = Y.Column(i).Subtract(pp.Column(i));
err += d[0] * d[0] + d[1] * d[1] + d[2] * d[2];////
}
}
else
{
P_points2 = R.Multiply(P_points);
Px2.SetRow(0, P_points2.Row(0));
Py2.SetRow(0, P_points2.Row(1));
Pz2.SetRow(0, P_points2.Row(2));
Px2 = Px2 + t[0, 0];
Py2 = Py2 + t[1, 0];
Pz2 = Pz2 + t[2, 0];
P_points.SetRow(0, Px2.Row(0));
P_points.SetRow(1, Py2.Row(0));
P_points.SetRow(2, Pz2.Row(0));
for (int i = 0; i < Np; i++)
{
d = Y.Column(i).Subtract(P_points.Column(i));
err += d[0] * d[0] + d[1] * d[1] + d[2] * d[2];////
}
}
err = err / Np;
///checking the conditions of convergence
///koşullar kontrol ediliyor.
if (err < threshold || Math.Abs(previous_error - err) < delta_error_thresh)
{
Debug.WriteLine("iteration= " + itr);
Debug.WriteLine("error= " + err);
break;
}
previous_error = err;
err = 0;
}
return(P_points);
}