// 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 double CalibrateColorCamera(List <Matrix> worldPoints, List <System.Drawing.PointF> imagePoints, Matrix cameraMatrix, Matrix distCoeffs, Matrix rotation, Matrix translation)
{
int nPoints = worldPoints.Count;
{
Matrix R, t;
CameraMath.DLT(cameraMatrix, distCoeffs, worldPoints, imagePoints, out R, out t);
//var r = Orientation.RotationVector(R);
var r = RoomAliveToolkit.ProjectorCameraEnsemble.RotationVectorFromRotationMatrix(R);
rotation.Copy(r);
translation.Copy(t);
}
// pack parameters into vector
// parameters: fx, fy, cx, cy, k1, k2, + 3 for rotation, 3 translation = 12
int nParameters = 12;
var parameters = new Matrix(nParameters, 1);
{
int pi = 0;
parameters[pi++] = cameraMatrix[0, 0]; // fx
parameters[pi++] = cameraMatrix[1, 1]; // fy
parameters[pi++] = cameraMatrix[0, 2]; // cx
parameters[pi++] = cameraMatrix[1, 2]; // cy
parameters[pi++] = distCoeffs[0]; // k1
parameters[pi++] = distCoeffs[1]; // k2
parameters[pi++] = rotation[0];
parameters[pi++] = rotation[1];
parameters[pi++] = rotation[2];
parameters[pi++] = translation[0];
parameters[pi++] = translation[1];
parameters[pi++] = translation[2];
}
// size of our error vector
int nValues = nPoints * 2; // each component (x,y) is a separate entry
LevenbergMarquardt.Function function = delegate(Matrix p)
{
var fvec = new Matrix(nValues, 1);
// unpack parameters
int pi = 0;
double fx = p[pi++];
double fy = p[pi++];
double cx = p[pi++];
double cy = p[pi++];
double k1 = p[pi++];
double k2 = p[pi++];
var K = Matrix.Identity(3, 3);
K[0, 0] = fx;
K[1, 1] = fy;
K[0, 2] = cx;
K[1, 2] = cy;
var d = Matrix.Zero(5, 1);
d[0] = k1;
d[1] = k2;
var r = new Matrix(3, 1);
r[0] = p[pi++];
r[1] = p[pi++];
r[2] = p[pi++];
var t = new Matrix(3, 1);
t[0] = p[pi++];
t[1] = p[pi++];
t[2] = p[pi++];
//var R = Orientation.Rodrigues(r);
var R = RoomAliveToolkit.ProjectorCameraEnsemble.RotationMatrixFromRotationVector(r);
var x = new Matrix(3, 1);
int fveci = 0;
for (int i = 0; i < worldPoints.Count; i++)
{
// transform world point to local camera coordinates
x.Mult(R, worldPoints[i]);
x.Add(t);
// fvec_i = y_i - f(x_i)
double u, v;
CameraMath.Project(K, d, x[0], x[1], x[2], out u, out v);
var imagePoint = imagePoints[i];
fvec[fveci++] = imagePoint.X - u;
fvec[fveci++] = imagePoint.Y - v;
}
return(fvec);
};
// optimize
var calibrate = new LevenbergMarquardt(function);
while (calibrate.State == LevenbergMarquardt.States.Running)
{
var rmsError = calibrate.MinimizeOneStep(parameters);
Console.WriteLine("rms error = " + rmsError);
}
for (int i = 0; i < nParameters; i++)
{
Console.WriteLine(parameters[i] + "\t");
}
Console.WriteLine();
// unpack parameters
{
int pi = 0;
double fx = parameters[pi++];
double fy = parameters[pi++];
double cx = parameters[pi++];
double cy = parameters[pi++];
double k1 = parameters[pi++];
double k2 = parameters[pi++];
cameraMatrix[0, 0] = fx;
cameraMatrix[1, 1] = fy;
cameraMatrix[0, 2] = cx;
cameraMatrix[1, 2] = cy;
distCoeffs[0] = k1;
distCoeffs[1] = k2;
rotation[0] = parameters[pi++];
rotation[1] = parameters[pi++];
rotation[2] = parameters[pi++];
translation[0] = parameters[pi++];
translation[1] = parameters[pi++];
translation[2] = parameters[pi++];
}
return(calibrate.RMSError);
}
public double MinimizeOneStep(Matrix parameters)
{
// initial value of the function; callee knows the size of the returned vector
var errorVector = function(parameters);
var error = errorVector.Dot(errorVector);
// Jacobian; callee knows the size of the returned matrix
var J = jacobianFunction(parameters);
// J'*J
var JtJ = new Matrix(parameters.Size, parameters.Size);
//stopWatch.Restart();
//JtJ.MultATA(J, J); // this is the big calculation that could be parallelized
JtJ.MultATAParallel(J, J);
//Console.WriteLine("JtJ: J size {0}x{1} {2}ms", J.Rows, J.Cols, stopWatch.ElapsedMilliseconds);
// J'*error
var JtError = new Matrix(parameters.Size, 1);
//stopWatch.Restart();
JtError.MultATA(J, errorVector); // error vector must be a column vector
//Console.WriteLine("JtError: errorVector size {0}x{1} {2}ms", errorVector.Rows, errorVector.Cols, stopWatch.ElapsedMilliseconds);
// allocate some space
var JtJaugmented = new Matrix(parameters.Size, parameters.Size);
var JtJinv = new Matrix(parameters.Size, parameters.Size);
var delta = new Matrix(parameters.Size, 1);
var newParameters = new Matrix(parameters.Size, 1);
// find a value of lambda that reduces error
double lambda = initialLambda;
while (true)
{
// augment J'*J: J'*J += lambda*(diag(J))
JtJaugmented.Copy(JtJ);
for (int i = 0; i < parameters.Size; i++)
{
JtJaugmented[i, i] = (1.0 + lambda) * JtJ[i, i];
}
//WriteMatrixToFile(errorVector, "errorVector");
//WriteMatrixToFile(J, "J");
//WriteMatrixToFile(JtJaugmented, "JtJaugmented");
//WriteMatrixToFile(JtError, "JtError");
// solve for delta: (J'*J + lambda*(diag(J)))*delta = J'*error
JtJinv.Inverse(JtJaugmented);
delta.Mult(JtJinv, JtError);
// new parameters = parameters - delta [why not add?]
newParameters.Sub(parameters, delta);
// evaluate function, compute error
var newErrorVector = function(newParameters);
double newError = newErrorVector.Dot(newErrorVector);
// if error is reduced, divide lambda by 10
bool improvement;
if (newError < error)
{
lambda /= lambdaIncrement;
improvement = true;
}
else // if not, multiply lambda by 10
{
lambda *= lambdaIncrement;
improvement = false;
}
// termination criteria:
// reduction in error is too small
var diff = new Matrix(errorVector.Size, 1);
diff.Sub(errorVector, newErrorVector);
double diffSq = diff.Dot(diff);
double errorDelta = Math.Sqrt(diffSq / error);
if (errorDelta < minimumReduction)
{
state = States.ReductionStepTooSmall;
}
// lambda is too big
if (lambda > maximumLambda)
{
state = States.LambdaTooLarge;
}
// change in parameters is too small [not implemented]
// if we made an improvement, accept the new parameters
if (improvement)
{
parameters.Copy(newParameters);
error = newError;
break;
}
// if we meet termination criteria, break
if (state != States.Running)
{
break;
}
}
rmsError = Math.Sqrt(error / errorVector.Size);
return(rmsError);
}
