private void button4_Click(object sender, EventArgs e)
{
///definitions
var m = Matrix <double> .Build;
int max_itr = 200;
double grid = 0.1;
double err = 0;
int Np = 500;
double threshold = 0.0001;
Matrix <double> R = m.Dense(3, 3);
Matrix <double> t = m.Dense(3, 1);
double offset_x = 0, offset_y = 120; ///translation respect to origin
double alfa = 0; ///rotation respect to origin
offset_x = Convert.ToDouble(textBox1.Text);
offset_y = Convert.ToDouble(textBox2.Text);
alfa = Convert.ToDouble(textBox3.Text);
///Create C shaped Data cloud
Matrix <double> M_points = CreateCircle(grid, Np, 10, 350);
///transform this data cloud for specified parameters as Model cloud and copy them to another cloud as sample
Matrix <double> P_points = addMotion(M_points, offset_x, offset_y, alfa);
///Add random noise to Sample Data cloud
///Create and draw data images
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);
///delay 1500 ms for user to see data clouds
System.Threading.Thread.Sleep(1500);
Stopwatch watch = new Stopwatch();
watch.Restart();
///Apply ICP to Point clouds
P_points = ICP.ICP_run(M_points, P_points, threshold, max_itr, false);
watch.Stop();
Debug.WriteLine("ICP time = " + watch.ElapsedMilliseconds + " ms");
///Draw Results
g.Clear(Color.Black);
pictureBox1.Image = flag;
DrawPoint2(P_points, Brushes.Red, pictureBox1, g);
DrawPoint2(M_points, Brushes.Green, pictureBox1, g);
}
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");
}
