// 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);
}
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]
}
// 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]
}
