static void Main(string[] args)
{
List<double[]> head_pts = PointCloudStorage.deserialize("model_pts");
List<double[]> scene_pts = PointCloudStorage.deserialize("scene_pts");
double[,] M = new double[head_pts.Count, 3];
for (int i = 0; i < head_pts.Count; i++)
{
for (int j = 0; j < 3; j++)
{
M[i, j] = head_pts[i][j];
}
}
double[,] T = new double[scene_pts.Count, 3];
for (int i = 0; i < scene_pts.Count; i++)
{
for (int j = 0; j < 3; j++)
{
T[i, j] = scene_pts[i][j];
}
}
ManagedICP icp = new ManagedICP(M, head_pts.Count, 3);
double[,] Rot = new double[3, 3];
/*Rot[0, 0] = 0.707;
Rot[1, 1] = 0.707;
Rot[0, 1] = -0.707;
Rot[1, 0] = 0.707;
Rot[2, 2] = 1.0;
* */
Rot[0, 0] = 0;
Rot[1, 1] = 0;
Rot[0, 1] = -1;
Rot[1, 0] = 1;
Rot[2, 2] = 1.0;
double[] trans = new double[3];
icp.fit(T, scene_pts.Count, Rot, trans, -1);
Console.WriteLine("Rotation:");
for (int i = 0; i < 3; i++)
{
Console.WriteLine(Rot[i, 0] + " " + Rot[i, 1] + " " + Rot[i, 2]);
}
int b = 5;
}
static void Main(string[] args)
{
int dim = 3;
int M_num = 10000;
int T_num = 10000;
double[,] M = new double[M_num, 3];
double[,] T = new double[T_num, 3];
int k = 0;
for (double x = -2; x < 2; x += 0.04)
{
for (double y = -2; y < 2; y += 0.04)
{
double z = 5 * x * Math.Exp(-x * x - y * y);
M[k, 0] = x;
M[k, 1] = y;
M[k, 2] = z;
T[k, 0] = x - 1;
T[k, 1] = y - 1;
T[k, 2] = z - 1;
k++;
}
}
icp_net.ManagedICP m = new ManagedICP(M, M_num, dim);
double[,] R = new double[3, 3];
R[0, 0] = 1.0;
R[1, 1] = 1.0;
R[2, 2] = 1.0;
double[] t = new double[3];
m.fit(T, T_num, R, t, -1);
Console.WriteLine("Transformation Results:");
Console.WriteLine("R:" + R);
Console.WriteLine("T:" + t[0] + " " + t[1] + " " + t[2]);
Console.Read();
}
static void Main(string[] args)
{
int dim = 3;
int M_num = 10000;
int T_num = 10000;
double[,] M = new double[M_num, 3];
double[,] T = new double[T_num, 3];
int k = 0;
for (double x = -2; x < 2; x += 0.04)
{
for (double y = -2; y < 2; y += 0.04)
{
double z = 5 * x * Math.Exp(-x * x - y * y);
M[k, 0] = x;
M[k, 1] = y;
M[k, 2] = z;
T[k, 0] = x - 1;
T[k, 1] = y - 1;
T[k, 2] = z - 1;
k++;
}
}
icp_net.ManagedICP m = new ManagedICP(M, M_num, dim);
double[,] R = new double[3, 3];
R[0, 0] = 1.0;
R[1, 1] = 1.0;
R[2, 2] = 1.0;
double[] t = new double[3];
m.fit(T, T_num, R, t, -1);
Console.WriteLine("Transformation Results:");
Console.WriteLine("R:" + R);
Console.WriteLine("T:" + t[0] + " " + t[1] + " " + t[2]);
Console.Read();
}
/// <summary>
/// aligns two point clouds using a transformation matrix. Additionally an ICP implementation can be used
/// </summary>
/// <param name="pReferencePointCloud">the reference cloud to align the adding cloud to</param>
/// <param name="pAddingPointCloud">the cloud that is supposed to be aligned</param>
/// <param name="pTransMat">the transformation matrix: 4x4 with [column,row] where [0,0] to [2,2] is the rotation and [3,0] to [3,2] the translation. The other fields are unused</param>
/// <param name="pUseICP">boolean to check if ICP is supposed to be used</param>
/// <param name="pIndis">inlier distance for ICP</param>
/// <returns>a modified point cloud with the valies [i,{x,y,z}]</returns>
public static double[,] alignPointClouds(PointCloud pReferencePointCloud, PointCloud pAddingPointCloud, double[,] pTransMat, bool pUseICP, int pIndis)
{
//log messages
Log.LogManager.updateAlgorithmStatus("Point Cloud Alignment");
Log.LogManager.writeLogDebug("[PointCloudAlignment] Use ICP: " + pUseICP + ", (ICP) In distance inliers : " + pIndis + ", Transformation Matrix: " +
pTransMat[0, 0] + "," + pTransMat[1, 0] + "," + pTransMat[2, 0] + "," + pTransMat[3, 0] + ";" +
pTransMat[0, 1] + "," + pTransMat[1, 1] + "," + pTransMat[2, 1] + "," + pTransMat[3, 1] + ";" +
pTransMat[0, 2] + "," + pTransMat[1, 2] + "," + pTransMat[2, 2] + "," + pTransMat[3, 2] + ";" +
pTransMat[0, 3] + "," + pTransMat[1, 3] + "," + pTransMat[2, 3] + "," + pTransMat[3, 3]);
//transform point clouds into format
double[,] referenceCloudDouble = new double[pReferencePointCloud.count, 3];
double[,] addingCloudDouble = new double[pAddingPointCloud.count, 3];
Task t1 = Task.Factory.StartNew(() =>
{
int counterRef = 0;
foreach (PointCloud.Point p in pReferencePointCloud.pointcloud_hs)
{
referenceCloudDouble[counterRef, 0] = p.point.X;
referenceCloudDouble[counterRef, 1] = p.point.Y;
referenceCloudDouble[counterRef, 2] = p.point.Z;
counterRef++;
}
});
Task t2 = Task.Factory.StartNew(() =>
{
int counterRef = 0;
foreach (PointCloud.Point p in pAddingPointCloud.pointcloud_hs)
{
addingCloudDouble[counterRef, 0] = p.point.X;
addingCloudDouble[counterRef, 1] = p.point.Y;
addingCloudDouble[counterRef, 2] = p.point.Z;
counterRef++;
}
});
t1.Wait(); t2.Wait();
//perform translation
Parallel.For(0, addingCloudDouble.GetLength(0), i =>
{
//fetch point
double[] point = new double[3];
for (int a = 0; a < 3; a++)
{
point[a] = addingCloudDouble[i, a];
}
//transform with matrix
double[] pointNEW = transformPointWithMatrix(point, pTransMat);
//write point
for (int a = 0; a < 3; a++)
{
addingCloudDouble[i, a] = pointNEW[a];
}
});
///*****************************
///***ITERATIVE CLOSEST POINT***
///*****************************
if (pUseICP)
{
//perform icp
//create reference icp object
ManagedICP icpManager = new ManagedICP(referenceCloudDouble, pReferencePointCloud.count, 3);
//fit
double[,] R = new double[3, 3]; //rotation matrix
R[0, 0] = 1.0;
R[1, 1] = 1.0;
R[2, 2] = 1.0;
double[] t = new double[3]; //translation vector
icpManager.fit(addingCloudDouble, pAddingPointCloud.count, R, t, pIndis);
//add to matrix4
double[,] rem = new double[4, 4];
rem[0, 0] = 1; rem[1, 1] = 2; rem[2, 2] = 1; rem[3, 3] = 1;
//add transformation matrix;
rem[0, 0] = (float)R[0, 0]; rem[0, 1] = (float)R[0, 1]; rem[0, 2] = (float)R[0, 2];
rem[1, 0] = (float)R[1, 0]; rem[1, 1] = (float)R[1, 1]; rem[1, 2] = (float)R[1, 2];
rem[2, 0] = (float)R[2, 0]; rem[2, 1] = (float)R[2, 1]; rem[2, 2] = (float)R[2, 2];
//add translation vector
rem[3, 0] = (float)t[0]; rem[3, 1] = (float)t[1]; rem[3, 2] = (float)t[2];
//set remaining fields
rem[0, 3] = 0; rem[1, 3] = 0; rem[2, 3] = 0;
//perform translation
Parallel.For(0, addingCloudDouble.GetLength(0), i =>
{
//fetch point
double[] point = new double[3];
for (int a = 0; a < 3; a++)
{
point[a] = addingCloudDouble[i, a];
}
//transform with matrix
double[] pointNEW = transformPointWithMatrix(point, rem);
//write point
for (int a = 0; a < 3; a++)
{
addingCloudDouble[i, a] = pointNEW[a];
}
});
}
///******************************
//updates the status
Log.LogManager.updateAlgorithmStatus("Done");
return(addingCloudDouble);
}
