public static void Normalize(Matrix A, Matrix B)
{
B.Scale(A, 1.0 / A.Norm());
}
public static Matrix Homography(List<Matrix> worldPoints, List<System.Drawing.PointF> imagePoints)
{
int n = worldPoints.Count;
// normalize image coordinates
var mu = new Matrix(2, 1);
for (int i = 0; i < n; i++)
{
mu[0] += imagePoints[i].X;
mu[1] += imagePoints[i].Y;
}
mu.Scale(1.0 / n);
var muAbs = new Matrix(2, 1);
for (int i = 0; i < n; i++)
{
muAbs[0] += Math.Abs(imagePoints[i].X - mu[0]);
muAbs[1] += Math.Abs(imagePoints[i].Y - mu[1]);
}
muAbs.Scale(1.0 / n);
var Hnorm = Matrix.Identity(3, 3);
Hnorm[0, 0] = 1 / muAbs[0];
Hnorm[1, 1] = 1 / muAbs[1];
Hnorm[0, 2] = -mu[0] / muAbs[0];
Hnorm[1, 2] = -mu[1] / muAbs[1];
var invHnorm = Matrix.Identity(3, 3);
invHnorm[0, 0] = muAbs[0];
invHnorm[1, 1] = muAbs[1];
invHnorm[0, 2] = mu[0];
invHnorm[1, 2] = mu[1];
var A = Matrix.Zero(2 * n, 9);
for (int i = 0; i < n; i++)
{
var X = worldPoints[i];
var imagePoint = imagePoints[i];
var x = new Matrix(3, 1);
x[0] = imagePoint.X;
x[1] = imagePoint.Y;
x[2] = 1;
var xn = new Matrix(3, 1);
xn.Mult(Hnorm, x);
// Zhang's formulation; Hartley's is similar
int ii = 2 * i;
A[ii, 0] = X[0];
A[ii, 1] = X[1];
A[ii, 2] = 1;
A[ii, 6] = -xn[0] * X[0];
A[ii, 7] = -xn[0] * X[1];
A[ii, 8] = -xn[0];
ii++; // next row
A[ii, 3] = X[0];
A[ii, 4] = X[1];
A[ii, 5] = 1;
A[ii, 6] = -xn[1] * X[0];
A[ii, 7] = -xn[1] * X[1];
A[ii, 8] = -xn[1];
}
// h is the eigenvector of ATA with the smallest eignvalue
var h = new Matrix(9, 1);
{
var ATA = new Matrix(9, 9);
ATA.MultATA(A, A);
var V = new Matrix(9, 9);
var ww = new Matrix(9, 1);
ATA.Eig(V, ww);
h.CopyCol(V, 0);
}
var Hn = new Matrix(3, 3);
Hn.Reshape(h);
var H = new Matrix(3, 3);
H.Mult(invHnorm, Hn);
return H;
}
// Use DLT to obtain estimate of calibration rig pose; in our case this is the pose of the Kinect camera.
// This pose estimate will provide a good initial estimate for subsequent projector calibration.
// Note for a full PnP solution we should probably refine with Levenberg-Marquardt.
// DLT is described in Hartley and Zisserman p. 178
public static void DLT(Matrix cameraMatrix, Matrix distCoeffs, List<Matrix> worldPoints, List<System.Drawing.PointF> imagePoints, out Matrix R, out Matrix t)
{
int n = worldPoints.Count;
var A = Matrix.Zero(2 * n, 12);
for (int j = 0; j < n; j++)
{
var X = worldPoints[j];
var imagePoint = imagePoints[j];
double x, y;
Undistort(cameraMatrix, distCoeffs, imagePoint.X, imagePoint.Y, out x, out y);
int ii = 2 * j;
A[ii, 4] = -X[0];
A[ii, 5] = -X[1];
A[ii, 6] = -X[2];
A[ii, 7] = -1;
A[ii, 8] = y * X[0];
A[ii, 9] = y * X[1];
A[ii, 10] = y * X[2];
A[ii, 11] = y;
ii++; // next row
A[ii, 0] = X[0];
A[ii, 1] = X[1];
A[ii, 2] = X[2];
A[ii, 3] = 1;
A[ii, 8] = -x * X[0];
A[ii, 9] = -x * X[1];
A[ii, 10] = -x * X[2];
A[ii, 11] = -x;
}
// Pcolumn is the eigenvector of ATA with the smallest eignvalue
var Pcolumn = new Matrix(12, 1);
{
var ATA = new Matrix(12, 12);
ATA.MultATA(A, A);
var V = new Matrix(12, 12);
var ww = new Matrix(12, 1);
ATA.Eig(V, ww);
Pcolumn.CopyCol(V, 0);
}
// reshape into 3x4 projection matrix
var P = new Matrix(3, 4);
P.Reshape(Pcolumn);
R = new Matrix(3, 3);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
R[i, j] = P[i, j];
if (R.Det3x3() < 0)
{
R.Scale(-1);
P.Scale(-1);
}
// orthogonalize R
{
var U = new Matrix(3, 3);
var V = new Matrix(3, 3);
var ww = new Matrix(3, 1);
R.SVD(U, ww, V);
R.MultAAT(U, V);
}
// determine scale factor
var RP = new Matrix(3, 3);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
RP[i, j] = P[i, j];
double s = RP.Norm() / R.Norm();
t = new Matrix(3, 1);
for (int i = 0; i < 3; i++)
t[i] = P[i, 3];
t.Scale(1.0 / s);
}
public static void PlanarDLT(Matrix cameraMatrix, Matrix distCoeffs, List<Matrix> worldPoints, List<System.Drawing.PointF> imagePoints, out Matrix R, out Matrix t)
{
int n = worldPoints.Count;
var undistortedImagePoints = new List<System.Drawing.PointF>();
for (int i = 0; i < n; i++)
{
var imagePoint = imagePoints[i];
double x, y;
Undistort(cameraMatrix, distCoeffs, imagePoint.X, imagePoint.Y, out x, out y);
var undistorted = new System.Drawing.PointF();
undistorted.X = (float)x;
undistorted.Y = (float)y;
undistortedImagePoints.Add(undistorted);
}
var H = Homography(worldPoints, undistortedImagePoints);
H.Scale(1.0 / H[2, 2]);
//Console.WriteLine(H);
var r1 = new Matrix(3, 1);
r1.CopyCol(H, 0);
var r2 = new Matrix(3, 1);
r2.CopyCol(H, 1);
t = new Matrix(3, 1);
t.CopyCol(H, 2);
t.Scale(1 / ((r1.Norm() + r2.Norm()) / 2.0));
r1.Scale(1 / r1.Norm());
r2.Scale(1 / r2.Norm());
var r3 = new Matrix(3, 1);
r3.Cross(r1, r2);
R = new Matrix(3, 3);
for (int i = 0; i < 3; i++)
{
R[i, 0] = r1[i];
R[i, 1] = r2[i];
R[i, 2] = r3[i];
}
}
static public void PlaneFit(IList<Matrix> X, out Matrix R, out Matrix t, out Matrix d2)
{
int n = X.Count;
var mu = new Matrix(3, 1);
for (int i = 0; i < n; i++)
mu.Add(X[i]);
mu.Scale(1f / (float)n);
var A = new Matrix(3, 3);
var xc = new Matrix(3, 1);
var M = new Matrix(3, 3);
for (int i = 0; i < X.Count; i++)
{
var x = X[i];
xc.Sub(x, mu);
M.Outer(xc, xc);
A.Add(M);
}
var V = new Matrix(3, 3);
var d = new Matrix(3, 1);
A.Eig(V, d); // eigenvalues in ascending order
// arrange in descending order so that z = 0
var V2 = new Matrix(3, 3);
for (int i = 0; i < 3; i++)
{
V2[i, 2] = V[i, 0];
V2[i, 1] = V[i, 1];
V2[i, 0] = V[i, 2];
}
d2 = new Matrix(3, 1);
d2[2] = d[0];
d2[1] = d[1];
d2[0] = d[2];
R = new Matrix(3, 3);
R.Transpose(V2);
if (R.Det3x3() < 0)
R.Scale(-1);
t = new Matrix(3, 1);
t.Mult(R, mu);
t.Scale(-1);
// eigenvalues are the sum of squared distances in each direction
// i.e., min eigenvalue is the sum of squared distances to the plane = d2[2]
// compute the distance to the plane by transforming to the plane and take z-coordinate:
// xPlane = R*x + t; distance = xPlane[2]
}
public void SaveToOBJ(string directory, string objPath)
{
var objFilename = Path.GetFileNameWithoutExtension(objPath);
var objDirectory = Path.GetDirectoryName(objPath);
if (!Directory.Exists(objDirectory))
Directory.CreateDirectory(objDirectory);
// Because we need to form triangles, we go back to the depth image
var quadOffsets = new System.Drawing.Point[]
{
new System.Drawing.Point(0, 0),
new System.Drawing.Point(1, 0),
new System.Drawing.Point(0, 1),
new System.Drawing.Point(1, 0),
new System.Drawing.Point(1, 1),
new System.Drawing.Point(0, 1),
};
var streamWriter = new CultureInvariantStreamWriter(objDirectory + "/" + objFilename + ".obj");
var mtlFileWriter = new CultureInvariantStreamWriter(objDirectory + "/" + objFilename + ".mtl");
streamWriter.WriteLine("mtllib " + objFilename + ".mtl");
uint nextVertexIndex = 1;
var depthImage = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
foreach (var camera in cameras)
{
mtlFileWriter.WriteLine("newmtl camera" + camera.name);
mtlFileWriter.WriteLine("Ka 1.000000 1.000000 1.000000");
mtlFileWriter.WriteLine("Kd 1.000000 1.000000 1.000000");
mtlFileWriter.WriteLine("Ks 0.000000 0.000000 0.000000");
mtlFileWriter.WriteLine("Tr 1.000000");
mtlFileWriter.WriteLine("illum 1");
mtlFileWriter.WriteLine("Ns 0.000000");
mtlFileWriter.WriteLine("map_Kd " + objFilename + "_" + camera.name + ".jpg");
File.Copy(directory + "/camera" + camera.name + "/color.jpg", objDirectory + "/" + objFilename + "_" + camera.name + ".jpg", true);
streamWriter.WriteLine("usemtl camera" + camera.name);
// load depth image
string cameraDirectory = directory + "/camera" + camera.name;
depthImage.LoadFromFile(cameraDirectory + "/mean.bin");
var calibration = camera.calibration;
var depthFrameToCameraSpaceTable = calibration.ComputeDepthFrameToCameraSpaceTable();
var vertices = new Vertex[Kinect2Calibration.depthImageWidth * Kinect2Calibration.depthImageHeight];
var colorCamera = new Matrix(4, 1);
var depthCamera = new Matrix(4, 1);
var world = new Matrix(4, 1);
for (int y = 0; y < Kinect2Calibration.depthImageHeight; y++)
for (int x = 0; x < Kinect2Calibration.depthImageWidth; x++)
{
// depth camera coords
var depth = depthImage[x, y] / 1000f; // m
// convert to depth camera space
var point = depthFrameToCameraSpaceTable[Kinect2Calibration.depthImageWidth * y + x];
depthCamera[0] = point.X * depth;
depthCamera[1] = point.Y * depth;
depthCamera[2] = depth;
depthCamera[3] = 1;
// world coordinates
world.Mult(camera.pose, depthCamera);
//world.Scale(1.0 / world[3]); not necessary for this transform
// convert to color camera space
colorCamera.Mult(calibration.depthToColorTransform, depthCamera);
colorCamera.Scale(1.0 / colorCamera[3]);
// project to color image
double colorU, colorV;
CameraMath.Project(calibration.colorCameraMatrix, calibration.colorLensDistortion, colorCamera[0], colorCamera[1], colorCamera[2], out colorU, out colorV);
colorU /= (double)Kinect2Calibration.colorImageWidth;
colorV /= (double)Kinect2Calibration.colorImageHeight;
var vertex = new Vertex();
vertex.x = (float)world[0];
vertex.y = (float)world[1];
vertex.z = (float)world[2];
vertex.u = (float)colorU;
vertex.v = (float)colorV;
vertices[Kinect2Calibration.depthImageWidth * y + x] = vertex;
}
streamWriter.WriteLine("g camera" + camera.name);
streamWriter.WriteLine("usemtl camera" + camera.name);
// examine each triangle
for (int y = 0; y < Kinect2Calibration.depthImageHeight - 1; y++)
for (int x = 0; x < Kinect2Calibration.depthImageWidth - 1; x++)
{
int offseti = 0;
for (int tri = 0; tri < 2; tri++)
{
// the indexes of the vertices of this triangle
var i0 = Kinect2Calibration.depthImageWidth * (y + quadOffsets[offseti].Y) + (x + quadOffsets[offseti].X);
var i1 = Kinect2Calibration.depthImageWidth * (y + quadOffsets[offseti + 1].Y) + (x + quadOffsets[offseti + 1].X);
var i2 = Kinect2Calibration.depthImageWidth * (y + quadOffsets[offseti + 2].Y) + (x + quadOffsets[offseti + 2].X);
// is triangle valid?
bool nonZero = (vertices[i0].z != 0) && (vertices[i1].z != 0) && (vertices[i2].z != 0);
bool jump01 = Vertex.DistanceSquared(vertices[i0], vertices[i1]) < 0.2 * 0.2;
bool jump02 = Vertex.DistanceSquared(vertices[i0], vertices[i2]) < 0.2 * 0.2;
bool jump12 = Vertex.DistanceSquared(vertices[i1], vertices[i2]) < 0.2 * 0.2;
bool valid = nonZero && jump01 && jump02 && jump12;
if (valid)
{
// only add the vertex if we haven't already
if (vertices[i0].index == 0)
{
streamWriter.WriteLine(vertices[i0]);
vertices[i0].index = nextVertexIndex++;
}
if (vertices[i1].index == 0)
{
streamWriter.WriteLine(vertices[i1]);
vertices[i1].index = nextVertexIndex++;
}
if (vertices[i2].index == 0)
{
streamWriter.WriteLine(vertices[i2]);
vertices[i2].index = nextVertexIndex++;
}
streamWriter.WriteLine("f {0}/{0} {1}/{1} {2}/{2}", vertices[i0].index, vertices[i1].index, vertices[i2].index);
}
offseti += 3;
}
}
}
streamWriter.Close();
mtlFileWriter.Close();
}
public void OptimizePose()
{
UnifyPose();
// joint estimate of projector and camera pose
// minimize wrt T_CjW, T_WPk: Sum_ijk v_ijk [ p_k( T_WPk T_CjW x_i ) - y_ik ]^2
// cameras observe points x_i (in camera coords)
// point x_i is observed to project to point y_ik in projector k
// v_ijk === 1 if point i is observed by camera j and imaged by projector k
// p_k(x) projects point x in projector k; x in projector coordinates
// T_CjW camera j local coordinates to world coordinates
// T_WPk world to projector k coorindates
// efficient implementation: list of points x_ijk for which v_ijk != 0; store j, k with each point x_i
// solve for C_j, P_k; C_0 is not in the set of parameters
// parameters: for each projector and camera: 1 rotation + 1 translation = 6 parameters
// We leave T_C0W fixed, so have 6 * (numProjectors + numCameras - 1) parameters
int nParameters = 6 * (projectors.Count + cameras.Count - 1);
//double[] parameters = new double[nParameters];
var parameters = new Matrix(nParameters, 1);
// loop over room.cameras, room.projectors to form up parameters array
{
int pi = 0; // index into our parameter array
for (int i = 1; i < cameras.Count; i++) // skip first one, which is our root
{
var T = cameras[i].pose;
var R = new Matrix(3, 3);
var t = new Matrix(3, 1);
for (int ii = 0; ii < 3; ii++)
{
t[ii] = T[ii, 3];
for (int jj = 0; jj < 3; jj++)
R[ii, jj] = T[ii, jj];
}
var r = CameraMath.RotationVectorFromRotationMatrix(R);
for (int ii = 0; ii < 3; ii++)
parameters[pi++] = r[ii];
for (int ii = 0; ii < 3; ii++)
parameters[pi++] = t[ii];
}
for (int i = 0; i < projectors.Count; i++)
{
var T = projectors[i].pose;
var R = new Matrix(3, 3);
var t = new Matrix(3, 1);
for (int ii = 0; ii < 3; ii++)
{
t[ii] = T[ii, 3];
for (int jj = 0; jj < 3; jj++)
R[ii, jj] = T[ii, jj];
}
var r = CameraMath.RotationVectorFromRotationMatrix(R);
for (int ii = 0; ii < 3; ii++)
parameters[pi++] = r[ii];
for (int ii = 0; ii < 3; ii++)
parameters[pi++] = t[ii];
}
}
// count the number of values
// use only inliers from previous step
int nValues = 0;
foreach (var projector in projectors)
foreach (var camera in projector.calibrationPointSets.Keys)
nValues += projector.calibrationPointSets[camera].worldPointInliers.Count * 2; // count components
LevenbergMarquardt.Function optimize = delegate(Matrix p)
{
var fvec = new Matrix(nValues, 1);
// convert p to transforms etc.
// convert back to transforms and put back in our structures
int pi = 0; // index into our parameter array
for (int i = 1; i < cameras.Count; i++) // skip first one, which is our root
{
var r = new Matrix(3, 1);
r[0] = p[pi++];
r[1] = p[pi++];
r[2] = p[pi++];
var R = CameraMath.RotationMatrixFromRotationVector(r);
var t = new Matrix(3, 1);
t[0] = p[pi++];
t[1] = p[pi++];
t[2] = p[pi++];
var T = new Matrix(4, 4);
T.Identity();
for (int ii = 0; ii < 3; ii++)
{
for (int jj = 0; jj < 3; jj++)
T[ii, jj] = R[ii, jj];
T[ii, 3] = t[ii];
}
cameras[i].pose = T;
}
for (int i = 0; i < projectors.Count; i++)
{
var r = new Matrix(3, 1);
r[0] = p[pi++];
r[1] = p[pi++];
r[2] = p[pi++];
var R = CameraMath.RotationMatrixFromRotationVector(r);
var t = new Matrix(3, 1);
t[0] = p[pi++];
t[1] = p[pi++];
t[2] = p[pi++];
var T = new Matrix(4, 4);
T.Identity();
for (int ii = 0; ii < 3; ii++)
{
for (int jj = 0; jj < 3; jj++)
T[ii, jj] = R[ii, jj];
T[ii, 3] = t[ii];
}
projectors[i].pose = T;
}
int fveci = 0; // index into our fvec array
foreach (var projector in projectors)
{
// T_WPk is inverse of T_PkW, projector pose
var T_WPk = new Matrix(4, 4);
T_WPk.Inverse(projector.pose);
foreach (var camera in projector.calibrationPointSets.Keys)
{
var cameraPoints = projector.calibrationPointSets[camera].worldPointInliers;
var projectorPoints = projector.calibrationPointSets[camera].imagePointInliers;
// transforms camera to projector coordinates
var T_CjW = camera.pose;
var T_CjPk = new Matrix(4, 4);
T_CjPk.Mult(T_WPk, T_CjW);
var cameraInProjector4 = new Matrix(4, 1);
cameraInProjector4[3] = 1;
var cameraPoint4 = new Matrix(4, 1);
cameraPoint4[3] = 1;
for (int i = 0; i < cameraPoints.Count; i++)
{
var cameraPoint = cameraPoints[i];
cameraPoint4[0] = cameraPoint[0];
cameraPoint4[1] = cameraPoint[1];
cameraPoint4[2] = cameraPoint[2];
cameraInProjector4.Mult(T_CjPk, cameraPoint4);
cameraInProjector4.Scale(1.0 / cameraInProjector4[3]);
// fvec_i = y_i - p_k( T_CjPk x_i );
double u, v;
CameraMath.Project(projector.cameraMatrix, projector.lensDistortion, cameraInProjector4[0], cameraInProjector4[1], cameraInProjector4[2], out u, out v);
var projectorPoint = projectorPoints[i];
fvec[fveci++] = projectorPoint.X - u;
fvec[fveci++] = projectorPoint.Y - v;
}
}
}
//double sum = 0;
//for (int i = 0; i < nValues; i++)
// sum += fvec[i] * fvec[i];
//double rms = Math.Sqrt(sum / (double)nValues);
//Console.WriteLine("in functor, rms == " + rms);
return fvec;
};
// TODO: maybe compute error before final optimization
var calibrate = new LevenbergMarquardt(optimize);
calibrate.minimumReduction = 1.0e-4;
while (calibrate.State == LevenbergMarquardt.States.Running)
{
double rmsError = calibrate.MinimizeOneStep(parameters);
Console.WriteLine("rms error = " + rmsError);
}
//for (int i = 0; i < nParameters; i++)
// Console.WriteLine(parameters[i] + "\t");
//Console.WriteLine();
// convert back to transforms and put back in our structures
{
int pi = 0; // index into our parameter array
for (int i = 1; i < cameras.Count; i++) // skip first one, which is our root
{
var r = new Matrix(3, 1);
r[0] = parameters[pi++];
r[1] = parameters[pi++];
r[2] = parameters[pi++];
var R = CameraMath.RotationMatrixFromRotationVector(r);
var t = new Matrix(3, 1);
t[0] = parameters[pi++];
t[1] = parameters[pi++];
t[2] = parameters[pi++];
var T = new Matrix(4, 4);
T.Identity();
for (int ii = 0; ii < 3; ii++)
{
for (int jj = 0; jj < 3; jj++)
T[ii, jj] = R[ii, jj];
T[ii, 3] = t[ii];
}
cameras[i].pose = T;
}
for (int i = 0; i < projectors.Count; i++)
{
var r = new Matrix(3, 1);
r[0] = parameters[pi++];
r[1] = parameters[pi++];
r[2] = parameters[pi++];
var R = CameraMath.RotationMatrixFromRotationVector(r);
var t = new Matrix(3, 1);
t[0] = parameters[pi++];
t[1] = parameters[pi++];
t[2] = parameters[pi++];
var T = new Matrix(4, 4);
T.Identity();
for (int ii = 0; ii < 3; ii++)
{
for (int jj = 0; jj < 3; jj++)
T[ii, jj] = R[ii, jj];
T[ii, 3] = t[ii];
}
projectors[i].pose = T;
}
}
Console.WriteLine("elapsed time " + stopWatch.ElapsedMilliseconds);
}
// Use DLT to obtain estimate of calibration rig pose; in our case this is the pose of the Kinect camera.
// This pose estimate will provide a good initial estimate for subsequent projector calibration.
// Note for a full PnP solution we should probably refine with Levenberg-Marquardt.
// DLT is described in Hartley and Zisserman p. 178
public static void DLT(Matrix cameraMatrix, Matrix distCoeffs, List <Matrix> worldPoints, List <System.Drawing.PointF> imagePoints, out Matrix R, out Matrix t)
{
int n = worldPoints.Count;
var A = Matrix.Zero(2 * n, 12);
for (int j = 0; j < n; j++)
{
var X = worldPoints[j];
var imagePoint = imagePoints[j];
double x, y;
Undistort(cameraMatrix, distCoeffs, imagePoint.X, imagePoint.Y, out x, out y);
int ii = 2 * j;
A[ii, 4] = -X[0];
A[ii, 5] = -X[1];
A[ii, 6] = -X[2];
A[ii, 7] = -1;
A[ii, 8] = y * X[0];
A[ii, 9] = y * X[1];
A[ii, 10] = y * X[2];
A[ii, 11] = y;
ii++; // next row
A[ii, 0] = X[0];
A[ii, 1] = X[1];
A[ii, 2] = X[2];
A[ii, 3] = 1;
A[ii, 8] = -x * X[0];
A[ii, 9] = -x * X[1];
A[ii, 10] = -x * X[2];
A[ii, 11] = -x;
}
// Pcolumn is the eigenvector of ATA with the smallest eignvalue
var Pcolumn = new Matrix(12, 1);
{
var ATA = new Matrix(12, 12);
ATA.MultATA(A, A);
var V = new Matrix(12, 12);
var ww = new Matrix(12, 1);
ATA.Eig(V, ww);
Pcolumn.CopyCol(V, 0);
}
// reshape into 3x4 projection matrix
var P = new Matrix(3, 4);
P.Reshape(Pcolumn);
R = new Matrix(3, 3);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
R[i, j] = P[i, j];
}
}
if (R.Det3x3() < 0)
{
R.Scale(-1);
P.Scale(-1);
}
// orthogonalize R
{
var U = new Matrix(3, 3);
var V = new Matrix(3, 3);
var ww = new Matrix(3, 1);
R.SVD(U, ww, V);
R.MultAAT(U, V);
}
// determine scale factor
var RP = new Matrix(3, 3);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
RP[i, j] = P[i, j];
}
}
double s = RP.Norm() / R.Norm();
t = new Matrix(3, 1);
for (int i = 0; i < 3; i++)
{
t[i] = P[i, 3];
}
t.Scale(1.0 / s);
}
// Use DLT to obtain estimate of calibration rig pose; in our case this is the pose of the Kinect camera.
// This pose estimate will provide a good initial estimate for subsequent projector calibration.
// Note for a full PnP solution we should probably refine with Levenberg-Marquardt.
// DLT is described in Hartley and Zisserman p. 178
public static void DLT(Matrix cameraMatrix, Matrix distCoeffs, List <Matrix> worldPoints, List <System.Drawing.PointF> imagePoints, out Matrix R, out Matrix t)
{
int n = worldPoints.Count;
var A = Matrix.Zero(2 * n, 12);
for (int j = 0; j < n; j++)
{
var X = worldPoints[j];
var imagePoint = imagePoints[j];
double x, y;
Undistort(cameraMatrix, distCoeffs, imagePoint.X, imagePoint.Y, out x, out y);
double w = 1;
int ii = 2 * j;
A[ii, 4] = -w * X[0];
A[ii, 5] = -w * X[1];
A[ii, 6] = -w * X[2];
A[ii, 7] = -w;
A[ii, 8] = y * X[0];
A[ii, 9] = y * X[1];
A[ii, 10] = y * X[2];
A[ii, 11] = y;
ii++; // next row
A[ii, 0] = w * X[0];
A[ii, 1] = w * X[1];
A[ii, 2] = w * X[2];
A[ii, 3] = w;
A[ii, 8] = -x * X[0];
A[ii, 9] = -x * X[1];
A[ii, 10] = -x * X[2];
A[ii, 11] = -x;
}
var Pcolumn = new Matrix(12, 1);
{
var U = new Matrix(2 * n, 2 * n); // full SVD, alas, supports small number of points
var V = new Matrix(12, 12);
var ww = new Matrix(12, 1);
A.SVD(U, ww, V);
// find smallest singular value
int min = 0;
ww.Minimum(ref min);
// Pcolumn is last column of V
Pcolumn.CopyCol(V, min);
}
// reshape into 3x4 projection matrix
var P = new Matrix(3, 4);
P.Reshape(Pcolumn);
// x = P * X
// P = K [ R | t ]
// inv(K) P = [ R | t ]
//var Kinv = new Matrix(3, 3);
//Kinv.Inverse(cameraMatrix);
//var Rt = new Matrix(3, 4);
//Rt.Mult(Kinv, P);
var Rt = new Matrix(3, 4);
Rt.Copy(P); // P does not contain camera matrix (by earlier undistort)
R = new Matrix(3, 3);
t = new Matrix(3, 1);
for (int ii = 0; ii < 3; ii++)
{
t[ii] = Rt[ii, 3];
for (int jj = 0; jj < 3; jj++)
{
R[ii, jj] = Rt[ii, jj];
}
}
//R.Copy(0, 0, Rt);
//t.CopyCol(Rt, 3);
if (R.Det3x3() < 0)
{
R.Scale(-1); t.Scale(-1);
}
// orthogonalize R
{
var U = new Matrix(3, 3);
var Vt = new Matrix(3, 3);
var V = new Matrix(3, 3);
var ww = new Matrix(3, 1);
R.SVD(U, ww, V);
Vt.Transpose(V);
R.Mult(U, Vt);
double s = ww.Sum() / 3.0;
t.Scale(1.0 / s);
}
// compute error?
}
static public void PlaneFit(IList <Matrix> X, out Matrix R, out Matrix t, out Matrix d2)
{
int n = X.Count;
var mu = new Matrix(3, 1);
for (int i = 0; i < n; i++)
{
mu.Add(X[i]);
}
mu.Scale(1f / (float)n);
var A = new Matrix(3, 3);
var xc = new Matrix(3, 1);
var M = new Matrix(3, 3);
for (int i = 0; i < X.Count; i++)
{
var x = X[i];
xc.Sub(x, mu);
M.Outer(xc, xc);
A.Add(M);
}
var V = new Matrix(3, 3);
var d = new Matrix(3, 1);
A.Eig(V, d); // eigenvalues in ascending order
// arrange in descending order so that z = 0
var V2 = new Matrix(3, 3);
for (int i = 0; i < 3; i++)
{
V2[i, 2] = V[i, 0];
V2[i, 1] = V[i, 1];
V2[i, 0] = V[i, 2];
}
d2 = new Matrix(3, 1);
d2[2] = d[0];
d2[1] = d[1];
d2[0] = d[2];
R = new Matrix(3, 3);
R.Transpose(V2);
if (R.Det3x3() < 0)
{
R.Scale(-1);
}
t = new Matrix(3, 1);
t.Mult(R, mu);
t.Scale(-1);
// eigenvalues are the sum of squared distances in each direction
// i.e., min eigenvalue is the sum of squared distances to the plane = d2[2]
// compute the distance to the plane by transforming to the plane and take z-coordinate:
// xPlane = R*x + t; distance = xPlane[2]
}
public static Matrix Homography(List <Matrix> worldPoints, List <System.Drawing.PointF> imagePoints)
{
int n = worldPoints.Count;
// normalize image coordinates
var mu = new Matrix(2, 1);
for (int i = 0; i < n; i++)
{
mu[0] += imagePoints[i].X;
mu[1] += imagePoints[i].Y;
}
mu.Scale(1.0 / n);
var muAbs = new Matrix(2, 1);
for (int i = 0; i < n; i++)
{
muAbs[0] += Math.Abs(imagePoints[i].X - mu[0]);
muAbs[1] += Math.Abs(imagePoints[i].Y - mu[1]);
}
muAbs.Scale(1.0 / n);
var Hnorm = Matrix.Identity(3, 3);
Hnorm[0, 0] = 1 / muAbs[0];
Hnorm[1, 1] = 1 / muAbs[1];
Hnorm[0, 2] = -mu[0] / muAbs[0];
Hnorm[1, 2] = -mu[1] / muAbs[1];
var invHnorm = Matrix.Identity(3, 3);
invHnorm[0, 0] = muAbs[0];
invHnorm[1, 1] = muAbs[1];
invHnorm[0, 2] = mu[0];
invHnorm[1, 2] = mu[1];
var A = Matrix.Zero(2 * n, 9);
for (int i = 0; i < n; i++)
{
var X = worldPoints[i];
var imagePoint = imagePoints[i];
var x = new Matrix(3, 1);
x[0] = imagePoint.X;
x[1] = imagePoint.Y;
x[2] = 1;
var xn = new Matrix(3, 1);
xn.Mult(Hnorm, x);
// Zhang's formulation; Hartley's is similar
int ii = 2 * i;
A[ii, 0] = X[0];
A[ii, 1] = X[1];
A[ii, 2] = 1;
A[ii, 6] = -xn[0] * X[0];
A[ii, 7] = -xn[0] * X[1];
A[ii, 8] = -xn[0];
ii++; // next row
A[ii, 3] = X[0];
A[ii, 4] = X[1];
A[ii, 5] = 1;
A[ii, 6] = -xn[1] * X[0];
A[ii, 7] = -xn[1] * X[1];
A[ii, 8] = -xn[1];
}
// h is the eigenvector of ATA with the smallest eignvalue
var h = new Matrix(9, 1);
{
var ATA = new Matrix(9, 9);
ATA.MultATA(A, A);
var V = new Matrix(9, 9);
var ww = new Matrix(9, 1);
ATA.Eig(V, ww);
h.CopyCol(V, 0);
}
var Hn = new Matrix(3, 3);
Hn.Reshape(h);
var H = new Matrix(3, 3);
H.Mult(invHnorm, Hn);
return(H);
}
public static void Normalize(Matrix A, Matrix B)
{
B.Scale(A, 1.0/A.Norm());
}
public void RecoverCalibrationFromSensor(KinectSensor kinectSensor)
{
var stopWatch = new System.Diagnostics.Stopwatch();
stopWatch.Start();
var objectPoints1 = new List <RoomAliveToolkit.Matrix>();
var colorPoints1 = new List <System.Drawing.PointF>();
var depthPoints1 = new List <System.Drawing.PointF>();
int n = 0;
for (float x = -2f; x < 2f; x += 0.2f)
{
for (float y = -2f; y < 2f; y += 0.2f)
{
for (float z = 0.4f; z < 4.5f; z += 0.4f)
{
var kinectCameraPoint = new CameraSpacePoint();
kinectCameraPoint.X = x;
kinectCameraPoint.Y = y;
kinectCameraPoint.Z = z;
// use SDK's projection
// adjust Y to make RH cooridnate system that is a projection of Kinect 3D points
var kinectColorPoint = kinectSensor.CoordinateMapper.MapCameraPointToColorSpace(kinectCameraPoint);
kinectColorPoint.Y = colorImageHeight - kinectColorPoint.Y;
var kinectDepthPoint = kinectSensor.CoordinateMapper.MapCameraPointToDepthSpace(kinectCameraPoint);
kinectDepthPoint.Y = depthImageHeight - kinectDepthPoint.Y;
if ((kinectColorPoint.X >= 0) && (kinectColorPoint.X < colorImageWidth) &&
(kinectColorPoint.Y >= 0) && (kinectColorPoint.Y < colorImageHeight) &&
(kinectDepthPoint.X >= 0) && (kinectDepthPoint.X < depthImageWidth) &&
(kinectDepthPoint.Y >= 0) && (kinectDepthPoint.Y < depthImageHeight))
{
n++;
var objectPoint = new RoomAliveToolkit.Matrix(3, 1);
objectPoint[0] = kinectCameraPoint.X;
objectPoint[1] = kinectCameraPoint.Y;
objectPoint[2] = kinectCameraPoint.Z;
objectPoints1.Add(objectPoint);
var colorPoint = new System.Drawing.PointF();
colorPoint.X = kinectColorPoint.X;
colorPoint.Y = kinectColorPoint.Y;
colorPoints1.Add(colorPoint);
//Console.WriteLine(objectPoint[0] + "\t" + objectPoint[1] + "\t" + colorPoint.X + "\t" + colorPoint.Y);
var depthPoint = new System.Drawing.PointF();
depthPoint.X = kinectDepthPoint.X;
depthPoint.Y = kinectDepthPoint.Y;
depthPoints1.Add(depthPoint);
}
}
}
}
colorCameraMatrix[0, 0] = 1000; //fx
colorCameraMatrix[1, 1] = 1000; //fy
colorCameraMatrix[0, 2] = colorImageWidth / 2; //cx
colorCameraMatrix[1, 2] = colorImageHeight / 2; //cy
colorCameraMatrix[2, 2] = 1;
var rotation = new Matrix(3, 1);
var translation = new Matrix(3, 1);
var colorError = CalibrateColorCamera(objectPoints1, colorPoints1, colorCameraMatrix, colorLensDistortion, rotation, translation);
//var rotationMatrix = Orientation.Rodrigues(rotation);
var rotationMatrix = RoomAliveToolkit.ProjectorCameraEnsemble.RotationMatrixFromRotationVector(rotation);
depthToColorTransform = Matrix.Identity(4, 4);
for (int i = 0; i < 3; i++)
{
depthToColorTransform[i, 3] = translation[i];
for (int j = 0; j < 3; j++)
{
depthToColorTransform[i, j] = rotationMatrix[i, j];
}
}
depthCameraMatrix[0, 0] = 360; //fx
depthCameraMatrix[1, 1] = 360; //fy
depthCameraMatrix[0, 2] = depthImageWidth / 2; //cx
depthCameraMatrix[1, 2] = depthImageHeight / 2; //cy
depthCameraMatrix[2, 2] = 1;
var depthError = CalibrateDepthCamera(objectPoints1, depthPoints1, depthCameraMatrix, depthLensDistortion);
//// latest SDK gives access to depth intrinsics directly -- this gives slightly higher projection error; not sure why
//var depthIntrinsics = kinectSensor.CoordinateMapper.GetDepthCameraIntrinsics();
//depthCameraMatrix[0, 0] = depthIntrinsics.FocalLengthX;
//depthCameraMatrix[1, 1] = depthIntrinsics.FocalLengthY;
//depthCameraMatrix[0, 2] = depthIntrinsics.PrincipalPointX;
//depthCameraMatrix[1, 2] = depthImageHeight - depthIntrinsics.PrincipalPointY; // note flip in Y!
//depthDistCoeffs[0] = depthIntrinsics.RadialDistortionSecondOrder;
//depthDistCoeffs[1] = depthIntrinsics.RadialDistortionFourthOrder;
// check projections
double depthProjectionError = 0;
double colorProjectionError = 0;
var color = new RoomAliveToolkit.Matrix(4, 1);
var testObjectPoint4 = new RoomAliveToolkit.Matrix(4, 1);
for (int i = 0; i < n; i++)
{
var testObjectPoint = objectPoints1[i];
var testDepthPoint = depthPoints1[i];
var testColorPoint = colorPoints1[i];
// "camera space" == depth camera space
// depth camera projection
double depthU, depthV;
CameraMath.Project(depthCameraMatrix, depthLensDistortion, testObjectPoint[0], testObjectPoint[1], testObjectPoint[2], out depthU, out depthV);
double dx = testDepthPoint.X - depthU;
double dy = testDepthPoint.Y - depthV;
depthProjectionError += (dx * dx) + (dy * dy);
// color camera projection
testObjectPoint4[0] = testObjectPoint[0];
testObjectPoint4[1] = testObjectPoint[1];
testObjectPoint4[2] = testObjectPoint[2];
testObjectPoint4[3] = 1;
color.Mult(depthToColorTransform, testObjectPoint4);
color.Scale(1.0 / color[3]); // not necessary for this transform
double colorU, colorV;
CameraMath.Project(colorCameraMatrix, colorLensDistortion, color[0], color[1], color[2], out colorU, out colorV);
dx = testColorPoint.X - colorU;
dy = testColorPoint.Y - colorV;
colorProjectionError += (dx * dx) + (dy * dy);
}
depthProjectionError /= n;
colorProjectionError /= n;
stopWatch.Stop();
Console.WriteLine("FakeCalibration :");
Console.WriteLine("n = " + n);
Console.WriteLine("color error = " + colorError);
Console.WriteLine("depth error = " + depthError);
Console.WriteLine("depth reprojection error = " + depthProjectionError);
Console.WriteLine("color reprojection error = " + colorProjectionError);
Console.WriteLine("depth camera matrix = \n" + depthCameraMatrix);
Console.WriteLine("depth lens distortion = \n" + depthLensDistortion);
Console.WriteLine("color camera matrix = \n" + colorCameraMatrix);
Console.WriteLine("color lens distortion = \n" + colorLensDistortion);
Console.WriteLine(stopWatch.ElapsedMilliseconds + " ms");
//// get camera space table
//// this does not change frame to frame (or so I believe)
//var tableEntries = kinectSensor.CoordinateMapper.GetDepthFrameToCameraSpaceTable();
//// compute our own version of the camera space table and compare it to the SDK's
//stopWatch.Restart();
//var tableEntries2 = ComputeDepthFrameToCameraSpaceTable();
//Console.WriteLine("ComputeDepthFrameToCameraSpaceTable took " + stopWatch.ElapsedMilliseconds + " ms");
//{
// float error = 0;
// for (int framey = 0; framey < depthImageHeight; framey++)
// for (int framex = 0; framex < depthImageWidth; framex++)
// {
// var point1 = tableEntries[depthImageWidth * framey + framex];
// var point2 = tableEntries2[depthImageWidth * framey + framex];
// error += (float)Math.Sqrt((point1.X - point2.X) * (point1.X - point2.X) + (point1.Y - point2.Y) * (point1.Y - point2.Y));
// }
// error /= (float)(depthImageHeight * depthImageWidth);
// Console.WriteLine("error = " + error);
//}
}
public void RecoverCalibrationFromSensor(KinectSensor kinectSensor)
{
var stopWatch = new System.Diagnostics.Stopwatch();
stopWatch.Start();
var objectPoints1 = new List<RoomAliveToolkit.Matrix>();
var colorPoints1 = new List<System.Drawing.PointF>();
var depthPoints1 = new List<System.Drawing.PointF>();
int n = 0;
for (float x = -2f; x < 2f; x += 0.2f)
for (float y = -2f; y < 2f; y += 0.2f)
for (float z = 0.4f; z < 4.5f; z += 0.4f)
{
var kinectCameraPoint = new CameraSpacePoint();
kinectCameraPoint.X = x;
kinectCameraPoint.Y = y;
kinectCameraPoint.Z = z;
// use SDK's projection
// adjust Y to make RH cooridnate system that is a projection of Kinect 3D points
var kinectColorPoint = kinectSensor.CoordinateMapper.MapCameraPointToColorSpace(kinectCameraPoint);
kinectColorPoint.Y = colorImageHeight - kinectColorPoint.Y;
var kinectDepthPoint = kinectSensor.CoordinateMapper.MapCameraPointToDepthSpace(kinectCameraPoint);
kinectDepthPoint.Y = depthImageHeight - kinectDepthPoint.Y;
if ((kinectColorPoint.X >= 0) && (kinectColorPoint.X < colorImageWidth) &&
(kinectColorPoint.Y >= 0) && (kinectColorPoint.Y < colorImageHeight) &&
(kinectDepthPoint.X >= 0) && (kinectDepthPoint.X < depthImageWidth) &&
(kinectDepthPoint.Y >= 0) && (kinectDepthPoint.Y < depthImageHeight))
{
n++;
var objectPoint = new RoomAliveToolkit.Matrix(3, 1);
objectPoint[0] = kinectCameraPoint.X;
objectPoint[1] = kinectCameraPoint.Y;
objectPoint[2] = kinectCameraPoint.Z;
objectPoints1.Add(objectPoint);
var colorPoint = new System.Drawing.PointF();
colorPoint.X = kinectColorPoint.X;
colorPoint.Y = kinectColorPoint.Y;
colorPoints1.Add(colorPoint);
//Console.WriteLine(objectPoint[0] + "\t" + objectPoint[1] + "\t" + colorPoint.X + "\t" + colorPoint.Y);
var depthPoint = new System.Drawing.PointF();
depthPoint.X = kinectDepthPoint.X;
depthPoint.Y = kinectDepthPoint.Y;
depthPoints1.Add(depthPoint);
}
}
colorCameraMatrix[0, 0] = 1000; //fx
colorCameraMatrix[1, 1] = 1000; //fy
colorCameraMatrix[0, 2] = colorImageWidth / 2; //cx
colorCameraMatrix[1, 2] = colorImageHeight / 2; //cy
colorCameraMatrix[2, 2] = 1;
var rotation = new Matrix(3, 1);
var translation = new Matrix(3, 1);
var colorError = CalibrateColorCamera(objectPoints1, colorPoints1, colorCameraMatrix, colorLensDistortion, rotation, translation);
//var rotationMatrix = Orientation.Rodrigues(rotation);
var rotationMatrix = RoomAliveToolkit.ProjectorCameraEnsemble.RotationMatrixFromRotationVector(rotation);
depthToColorTransform = Matrix.Identity(4, 4);
for (int i = 0; i < 3; i++)
{
depthToColorTransform[i, 3] = translation[i];
for (int j = 0; j < 3; j++)
depthToColorTransform[i, j] = rotationMatrix[i, j];
}
depthCameraMatrix[0, 0] = 360; //fx
depthCameraMatrix[1, 1] = 360; //fy
depthCameraMatrix[0, 2] = depthImageWidth / 2; //cx
depthCameraMatrix[1, 2] = depthImageHeight / 2; //cy
depthCameraMatrix[2, 2] = 1;
var depthError = CalibrateDepthCamera(objectPoints1, depthPoints1, depthCameraMatrix, depthLensDistortion);
//// latest SDK gives access to depth intrinsics directly -- this gives slightly higher projection error; not sure why
//var depthIntrinsics = kinectSensor.CoordinateMapper.GetDepthCameraIntrinsics();
//depthCameraMatrix[0, 0] = depthIntrinsics.FocalLengthX;
//depthCameraMatrix[1, 1] = depthIntrinsics.FocalLengthY;
//depthCameraMatrix[0, 2] = depthIntrinsics.PrincipalPointX;
//depthCameraMatrix[1, 2] = depthImageHeight - depthIntrinsics.PrincipalPointY; // note flip in Y!
//depthDistCoeffs[0] = depthIntrinsics.RadialDistortionSecondOrder;
//depthDistCoeffs[1] = depthIntrinsics.RadialDistortionFourthOrder;
// check projections
double depthProjectionError = 0;
double colorProjectionError = 0;
var color = new RoomAliveToolkit.Matrix(4, 1);
var testObjectPoint4 = new RoomAliveToolkit.Matrix(4, 1);
for (int i = 0; i < n; i++)
{
var testObjectPoint = objectPoints1[i];
var testDepthPoint = depthPoints1[i];
var testColorPoint = colorPoints1[i];
// "camera space" == depth camera space
// depth camera projection
double depthU, depthV;
CameraMath.Project(depthCameraMatrix, depthLensDistortion, testObjectPoint[0], testObjectPoint[1], testObjectPoint[2], out depthU, out depthV);
double dx = testDepthPoint.X - depthU;
double dy = testDepthPoint.Y - depthV;
depthProjectionError += (dx * dx) + (dy * dy);
// color camera projection
testObjectPoint4[0] = testObjectPoint[0];
testObjectPoint4[1] = testObjectPoint[1];
testObjectPoint4[2] = testObjectPoint[2];
testObjectPoint4[3] = 1;
color.Mult(depthToColorTransform, testObjectPoint4);
color.Scale(1.0 / color[3]); // not necessary for this transform
double colorU, colorV;
CameraMath.Project(colorCameraMatrix, colorLensDistortion, color[0], color[1], color[2], out colorU, out colorV);
dx = testColorPoint.X - colorU;
dy = testColorPoint.Y - colorV;
colorProjectionError += (dx * dx) + (dy * dy);
}
depthProjectionError /= n;
colorProjectionError /= n;
stopWatch.Stop();
Console.WriteLine("FakeCalibration :");
Console.WriteLine("n = " + n);
Console.WriteLine("color error = " + colorError);
Console.WriteLine("depth error = " + depthError);
Console.WriteLine("depth reprojection error = " + depthProjectionError);
Console.WriteLine("color reprojection error = " + colorProjectionError);
Console.WriteLine("depth camera matrix = \n" + depthCameraMatrix);
Console.WriteLine("depth lens distortion = \n" + depthLensDistortion);
Console.WriteLine("color camera matrix = \n" + colorCameraMatrix);
Console.WriteLine("color lens distortion = \n" + colorLensDistortion);
Console.WriteLine(stopWatch.ElapsedMilliseconds + " ms");
//// get camera space table
//// this does not change frame to frame (or so I believe)
//var tableEntries = kinectSensor.CoordinateMapper.GetDepthFrameToCameraSpaceTable();
//// compute our own version of the camera space table and compare it to the SDK's
//stopWatch.Restart();
//var tableEntries2 = ComputeDepthFrameToCameraSpaceTable();
//Console.WriteLine("ComputeDepthFrameToCameraSpaceTable took " + stopWatch.ElapsedMilliseconds + " ms");
//{
// float error = 0;
// for (int framey = 0; framey < depthImageHeight; framey++)
// for (int framex = 0; framex < depthImageWidth; framex++)
// {
// var point1 = tableEntries[depthImageWidth * framey + framex];
// var point2 = tableEntries2[depthImageWidth * framey + framex];
// error += (float)Math.Sqrt((point1.X - point2.X) * (point1.X - point2.X) + (point1.Y - point2.Y) * (point1.Y - point2.Y));
// }
// error /= (float)(depthImageHeight * depthImageWidth);
// Console.WriteLine("error = " + error);
//}
}
public static List<RoomAliveToolkit.Matrix> TransformPoints(RoomAliveToolkit.Matrix A, List<RoomAliveToolkit.Matrix> points)
{
var transformedPoints = new List<RoomAliveToolkit.Matrix>();
var point4 = new RoomAliveToolkit.Matrix(4, 1);
point4[3] = 1;
var transformedPoint4 = new RoomAliveToolkit.Matrix(4, 1);
foreach (var point in points)
{
point4[0] = point[0]; point4[1] = point[1]; point4[2] = point[2];
transformedPoint4.Mult(A, point4);
transformedPoint4.Scale(1.0f / transformedPoint4[3]);
var transformedPoint = new RoomAliveToolkit.Matrix(3, 1);
transformedPoint[0] = transformedPoint4[0]; transformedPoint[1] = transformedPoint4[1]; transformedPoint[2] = transformedPoint4[2];
transformedPoints.Add(transformedPoint);
}
return transformedPoints;
}
public static double PlaneFit(IList<Matrix> points, out Matrix X, out double D)
{
X = new Matrix(3, 1);
var mu = new RoomAliveToolkit.Matrix(3, 1);
for (int i = 0; i < points.Count; i++)
mu.Add(points[i]);
mu.Scale(1f / (float)points.Count);
var A = new RoomAliveToolkit.Matrix(3, 3);
var pc = new RoomAliveToolkit.Matrix(3, 1);
var M = new RoomAliveToolkit.Matrix(3, 3);
for (int i = 0; i < points.Count; i++)
{
var p = points[i];
pc.Sub(p, mu);
M.Outer(pc, pc);
A.Add(M);
}
var V = new RoomAliveToolkit.Matrix(3, 3);
var d = new RoomAliveToolkit.Matrix(3, 1);
A.Eig(V, d); // TODO: replace with 3x3 version?
//Console.WriteLine("------");
//Console.WriteLine(A);
//Console.WriteLine(V);
//Console.WriteLine(d);
double minEigenvalue = Double.MaxValue;
int minEigenvaluei = 0;
for (int i = 0; i < 3; i++)
if (d[i] < minEigenvalue)
{
minEigenvalue = d[i];
minEigenvaluei = i;
}
X.CopyCol(V, minEigenvaluei);
D = -X.Dot(mu);
// min eigenvalue is the sum of squared distances to the plane
// signed distance is: double distance = X.Dot(point) + D;
return minEigenvalue;
}