private double[] fitNearestSurface(List <DistanceAndPoint> distancesAndPoints, int n)
{
if (DEBUG)
{
Debug.Log("TangoUpdate pca , for " + n + " points");
}
MatrixStats mc = getCovar(distancesAndPoints, n);
OnePC pc1 = powerMethod(mc.covar);
if (DEBUG)
{
Debug.Log("TangoUpdate pca1 " + pc1.pc[0] + ", " + pc1.pc[1] + ", " + pc1.pc[2]);
}
double[,] covar2 = new double[3, 3];
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
covar2[i, j] = mc.covar[i, j] - (pc1.shrink * pc1.pc[i] * pc1.pc[j]);
}
}
OnePC pc2 = powerMethod(covar2);
if (DEBUG)
{
Debug.Log("TangoUpdate pca2 " + pc2.pc[0] + ", " + pc2.pc[1] + ", " + pc2.pc[2]);
}
// c(x=y1 * z2 - z1 * y2, y=z1 * x2 - x1 * z2, z=x1 * y2 - y1 * x2)
double[] planeCoefficients =
{
pc1.pc [1] * pc2.pc [2] - pc1.pc [2] * pc2.pc [1],
pc1.pc [2] * pc2.pc [0] - pc1.pc [0] * pc2.pc [2],
pc1.pc [0] * pc2.pc [1] - pc1.pc [1] * pc2.pc [0],
0.0f
};
planeCoefficients[3] = planeCoefficients[0] * mc.mean[0] + planeCoefficients[1] * mc.mean[1] + planeCoefficients[2] * mc.mean[2];
return(planeCoefficients);
}
private MatrixStats getCovar(List<DistanceAndPoint> distancesAndPoints, int n)
{
if (DEBUG) Debug.Log ("TangoUpdate pca start getCovar");
Vector3 sum = new Vector3(0.0f, 0.0f, 0.0f);
for (int i=0; i < n; i++) {
sum += distancesAndPoints[i].point;
}
if (DEBUG) Debug.Log ("TangoUpdate pca vector sum " + sum.x + ", " + sum.y + ", " + sum.z);
double[] mean = {sum.x / n, sum.y / n, sum.z / n};
if (DEBUG) Debug.Log ("TangoUpdate pca vector mean " + mean[0] + ", " + mean[1] + ", " + mean[2]);
double[,] covar = {{0,0,0}, {0,0,0}, {0,0,0}};
if (DEBUG) Debug.Log ("TangoUpdate pca initial, " + covar[0,0] + "," + covar[0,1] + "," + covar[1,2]);
for (int i=0; i < n; i++) {
covar[0,0] += (distancesAndPoints[i].point.x - mean[0]) * (distancesAndPoints[i].point.x - mean[0]);
covar[0,1] += (distancesAndPoints[i].point.x - mean[0]) * (distancesAndPoints[i].point.y - mean[1]);
covar[0,2] += (distancesAndPoints[i].point.x - mean[0]) * (distancesAndPoints[i].point.z - mean[2]);
covar[1,1] += (distancesAndPoints[i].point.y - mean[1]) * (distancesAndPoints[i].point.y - mean[1]);
covar[1,2] += (distancesAndPoints[i].point.y - mean[1]) * (distancesAndPoints[i].point.z - mean[2]);
covar[2,2] += (distancesAndPoints[i].point.z - mean[2]) * (distancesAndPoints[i].point.z - mean[2]);
}
covar [1, 0] = covar [0, 1];
covar [2, 0] = covar [0, 2];
covar [2, 1] = covar [1, 2];
if (DEBUG) Debug.Log ("TangoUpdate pca , " + covar[0,0] + "," + covar[0,1] + "," + covar[1,2]);
for (int i=0; i < 3; i++) {
for (int j=0; j < 3; j++) {
covar [i, j] /= (n - 1);
if (DEBUG) Debug.Log ("TangoUpdate pca , " + i + "," + j + "," + covar[i,j]);
}
}
MatrixStats mstats = new MatrixStats (mean, covar);
return mstats;
}
private MatrixStats getCovar(List <DistanceAndPoint> distancesAndPoints, int n)
{
if (DEBUG)
{
Debug.Log("TangoUpdate pca start getCovar");
}
Vector3 sum = new Vector3(0.0f, 0.0f, 0.0f);
for (int i = 0; i < n; i++)
{
sum += distancesAndPoints[i].point;
}
if (DEBUG)
{
Debug.Log("TangoUpdate pca vector sum " + sum.x + ", " + sum.y + ", " + sum.z);
}
double[] mean = { sum.x / n, sum.y / n, sum.z / n };
if (DEBUG)
{
Debug.Log("TangoUpdate pca vector mean " + mean[0] + ", " + mean[1] + ", " + mean[2]);
}
double[,] covar = { { 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 } };
if (DEBUG)
{
Debug.Log("TangoUpdate pca initial, " + covar[0, 0] + "," + covar[0, 1] + "," + covar[1, 2]);
}
for (int i = 0; i < n; i++)
{
covar[0, 0] += (distancesAndPoints[i].point.x - mean[0]) * (distancesAndPoints[i].point.x - mean[0]);
covar[0, 1] += (distancesAndPoints[i].point.x - mean[0]) * (distancesAndPoints[i].point.y - mean[1]);
covar[0, 2] += (distancesAndPoints[i].point.x - mean[0]) * (distancesAndPoints[i].point.z - mean[2]);
covar[1, 1] += (distancesAndPoints[i].point.y - mean[1]) * (distancesAndPoints[i].point.y - mean[1]);
covar[1, 2] += (distancesAndPoints[i].point.y - mean[1]) * (distancesAndPoints[i].point.z - mean[2]);
covar[2, 2] += (distancesAndPoints[i].point.z - mean[2]) * (distancesAndPoints[i].point.z - mean[2]);
}
covar [1, 0] = covar [0, 1];
covar [2, 0] = covar [0, 2];
covar [2, 1] = covar [1, 2];
if (DEBUG)
{
Debug.Log("TangoUpdate pca , " + covar[0, 0] + "," + covar[0, 1] + "," + covar[1, 2]);
}
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
covar [i, j] /= (n - 1);
if (DEBUG)
{
Debug.Log("TangoUpdate pca , " + i + "," + j + "," + covar[i, j]);
}
}
}
MatrixStats mstats = new MatrixStats(mean, covar);
return(mstats);
}
