//get transformation matrix from vector more then current matrice
void addNewMatrix(object sender, RoutedEventArgs e)
{
if (pointsToCalibrate == null)
{
MessageBox.Show("Avaliable points not enough. Please Get more points!!");
return;
}
if (pointsToCalibrate[0] == null)
{
MessageBox.Show("Avaliable points not enough. Please Get more points!!");
return;
}
//get current matrices
//Get transformation matrix from matrices.txt
string text = System.IO.File.ReadAllText("matrices.txt");
string[] matricesData = text.Split(' ');
for (int i = 0; i < matricesData.Length; i++)
{
matricesData[i].TrimEnd('\n');
}
int step = 16; // a matrix
int oldKinectNum = matricesData.Length / step;
List <DenseMatrix> transfMatrix = new List <DenseMatrix>();
MatrixProcessor mp = new MatrixProcessor();
for (int i = 0; i < oldKinectNum; ++i)
{
DenseMatrix m = new DenseMatrix(4);
for (int row = 0; row < 4; ++row)
{
for (int col = 0; col < 4; ++col)
{
m[row, col] = Convert.ToDouble(matricesData[step * i + row * 4 + col]);
}
}
transfMatrix.Add(m);
}
MessageBox.Show("Existing matrix num = " + transfMatrix.Count);
//check whether the point is enough for each kinect without matrix
for (int kinectIndex = oldKinectNum; kinectIndex < pointsToCalibrate.Length; ++kinectIndex)
{
int pointNum = 0;
for (int pointIndex = 0; pointIndex < pointsToCalibrate[kinectIndex].Count; ++pointIndex)
{
if (pointsToCalibrate[kinectIndex][pointIndex][2, 0] > 0.5)
{
bool hasKnownPoint = false;
//one of the previous kinect has value
for (int i = 0; i < oldKinectNum; ++i)
{
if (pointsToCalibrate[i][pointIndex][2, 0] > 0.5)
{
hasKnownPoint = true;
break;
}
}
if (hasKnownPoint)
{
pointNum++;
}
}
}
if (pointNum < 4)
{
MessageBox.Show("Get " + (4 - pointNum) + " more points for kinect " + kinectIndex);
return;
}
}
//using existing information to calibrate kinects without matrix yet
string textToPrint = "";
for (int kinectIndex = oldKinectNum; kinectIndex < pointsToCalibrate.Length; ++kinectIndex)
{
//int pointNum = 0;
List <DenseMatrix> pointsToCalibrate0 = new List <DenseMatrix>();
List <DenseMatrix> pointsToCalibrate1 = new List <DenseMatrix>();
for (int pointIndex = 0; pointIndex < pointsToCalibrate[kinectIndex].Count; ++pointIndex)
{
if (pointsToCalibrate[kinectIndex][pointIndex][2, 0] > 0.5)
{
// bool hasKnownPoint = false;
//one of the existing kinects has value
for (int i = 0; i < oldKinectNum; ++i)
{
if (pointsToCalibrate[i][pointIndex][2, 0] > 0.5)
{
pointsToCalibrate0.Add(transfMatrix[i] * pointsToCalibrate[i][pointIndex]);
pointsToCalibrate1.Add(pointsToCalibrate[kinectIndex][pointIndex]);
break;
}
}
}
}
transfMatrix.Add(mp.getTransformationMatrixFromPoint(pointsToCalibrate0, pointsToCalibrate1));
double errorSum = 0;
for (int i = 0; i < pointsToCalibrate0.Count; ++i)
{
DenseMatrix dm0 = (DenseMatrix)pointsToCalibrate0[i].Clone();
DenseMatrix dm1 = (DenseMatrix)pointsToCalibrate1[i].Clone();
dm1 = transfMatrix[kinectIndex] * dm1;
errorSum += Math.Sqrt((dm0[0, 0] - dm1[0, 0]) * (dm0[0, 0] - dm1[0, 0]) + (dm0[1, 0] - dm1[1, 0]) * (dm0[1, 0] - dm1[1, 0])
+ (dm0[2, 0] - dm1[2, 0]) * (dm0[2, 0] - dm1[2, 0]));
}
textToPrint += "[KINECT " + kinectIndex.ToString() + "] error sum = " + errorSum +
Environment.NewLine + mp.printMatrix(transfMatrix[kinectIndex]);
}
MessageBox.Show("calibration result = " + Environment.NewLine + textToPrint);
//store previous result
prevMatrices = text;
TextWriter tw1 = new StreamWriter("matrices.txt");
for (int j = 0; j < pointsToCalibrate.Length; ++j)
{
for (int row = 0; row < 4; ++row)
{
for (int col = 0; col < 4; ++col)
{
tw1.Write(transfMatrix[j][row, col].ToString() + " ");
}
}
}
tw1.Close();
GUIComponents.fc.fp.getTransformationMatrix();
}
//get transformation matrix from kinect 0
void getMatrixFromAll(object sender, RoutedEventArgs e)
{
//check for points avaliable
if (pointsToCalibrate == null)
{
MessageBox.Show("Avaliable points not enough. Please Get more points!!");
return;
}
if (pointsToCalibrate[0] == null)
{
MessageBox.Show("Avaliable points not enough. Please Get more points!!");
return;
}
//remove failed points
for (int i = 0; i < pointsToCalibrate[0].Count; ++i)
{
bool removeThisPoint = false;
for (int j = 0; j < pointsToCalibrate.Length; ++j)
{
if (pointsToCalibrate[j][i][2, 0] < 0.5 || pointsToCalibrate[j][i][2, 0] > 2.5)
{
removeThisPoint = true;
}
}
if (removeThisPoint)
{
for (int j = 0; j < pointsToCalibrate.Length; ++j)
{
pointsToCalibrate[j].RemoveAt(i);
}
i--;
}
}
//return if there are not enough avaliable points
if (pointsToCalibrate[0].Count < 4)
{
int n = 4 - pointsToCalibrate[0].Count;
MessageBox.Show("Avaliable points not enough. Please Get " + n.ToString() + " more points!!");
return;
}
List <DenseMatrix> transfMatrixList = new List <DenseMatrix>();
MatrixProcessor mp = new MatrixProcessor();
transfMatrixList.Add(DenseMatrix.CreateIdentity(4)); //transformation matrix for kinect 0
for (int i = 1; i < pointsToCalibrate.Length; ++i)
{
transfMatrixList.Add(mp.getTransformationMatrixFromPoint(pointsToCalibrate[0], pointsToCalibrate[i]));
}
//Compute error sum, when is the sum of errors between the corresponding points
List <double> errorList = new List <double>();
for (int j = 0; j < pointsToCalibrate.Length; ++j)
{
double errorSum = 0;
for (int i = 0; i < pointsToCalibrate[0].Count; ++i)
{
DenseMatrix dm0 = (DenseMatrix)pointsToCalibrate[0][i].Clone();
DenseMatrix dm1 = (DenseMatrix)pointsToCalibrate[j][i].Clone();
dm1 = transfMatrixList[j] * dm1;
errorSum += Math.Sqrt((dm0[0, 0] - dm1[0, 0]) * (dm0[0, 0] - dm1[0, 0]) + (dm0[1, 0] - dm1[1, 0]) * (dm0[1, 0] - dm1[1, 0])
+ (dm0[2, 0] - dm1[2, 0]) * (dm0[2, 0] - dm1[2, 0]));
}
errorList.Add(errorSum);
}
//print result
string textToPrint = "";
for (int j = 0; j < pointsToCalibrate.Length; ++j)
{
textToPrint += "[KINECT " + j.ToString() + "] error sum = " + errorList[j] + Environment.NewLine +
mp.printMatrix(transfMatrixList[j]);
}
MessageBox.Show("calibration result = " + Environment.NewLine + textToPrint);
prevMatrices = System.IO.File.ReadAllText("matrices.txt");
TextWriter tw1 = new StreamWriter("matrices.txt");
for (int j = 0; j < pointsToCalibrate.Length; ++j)
{
for (int row = 0; row < 4; ++row)
{
for (int col = 0; col < 4; ++col)
{
tw1.Write(transfMatrixList[j][row, col].ToString() + " ");
}
}
}
tw1.Close();
GUIComponents.fc.fp.getTransformationMatrix();
}