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