static double CalibrateColorCamera(List <Matrix> worldPoints, List <System.Drawing.PointF> imagePoints, Matrix cameraMatrix, Matrix distCoeffs, Matrix rotation, Matrix translation, bool silent)
{
int nPoints = worldPoints.Count;
{
Matrix R, t;
CameraMath.DLT(cameraMatrix, distCoeffs, worldPoints, imagePoints, out R, out t);
var r = CameraMath.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 = CameraMath.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);
if (!silent)
{
Console.WriteLine("rms error = " + rmsError);
}
}
if (!silent)
{
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 static double CalibrateCameraExtrinsicsOnly(List <List <Matrix> > worldPointSets, List <List <System.Drawing.PointF> > imagePointSets,
Matrix cameraMatrix, ref List <Matrix> rotations, ref List <Matrix> translations)
{
int nSets = worldPointSets.Count;
int nPoints = 0;
for (int i = 0; i < nSets; i++)
{
nPoints += worldPointSets[i].Count; // for later
}
var distCoeffs = Matrix.Zero(2, 1);
//// if necessary run DLT on each point set to get initial rotation and translations
//if (rotations == null)
//{
// rotations = new List<Matrix>();
// translations = new List<Matrix>();
// for (int i = 0; i < nSets; i++)
// {
// Matrix R, t;
// CameraMath.DLT(cameraMatrix, distCoeffs, worldPointSets[i], imagePointSets[i], out R, out t);
// var r = CameraMath.RotationVectorFromRotationMatrix(R);
// rotations.Add(r);
// translations.Add(t);
// }
//}
// Levenberg-Marquardt for camera matrix (ignore lens distortion for now)
// pack parameters into vector
// parameters: camera has f, cx, cy; each point set has rotation + translation (6)
//int nParameters = 3 + 6 * nSets;
int nParameters = 6 * nSets;
var parameters = new Matrix(nParameters, 1);
{
int pi = 0;
//parameters[pi++] = cameraMatrix[0, 0]; // f
//parameters[pi++] = cameraMatrix[0, 2]; // cx
//parameters[pi++] = cameraMatrix[1, 2]; // cy
for (int i = 0; i < nSets; i++)
{
parameters[pi++] = rotations[i][0];
parameters[pi++] = rotations[i][1];
parameters[pi++] = rotations[i][2];
parameters[pi++] = translations[i][0];
parameters[pi++] = translations[i][1];
parameters[pi++] = translations[i][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 f = p[pi++];
//double cx = p[pi++];
//double cy = p[pi++];
var K = Matrix.Identity(3, 3);
//K[0, 0] = f;
//K[1, 1] = f;
//K[0, 2] = cx;
//K[1, 2] = cy;
K[0, 0] = cameraMatrix[0, 0];
K[1, 1] = cameraMatrix[1, 1];
K[0, 2] = cameraMatrix[0, 2];
K[1, 2] = cameraMatrix[1, 2];
var d = Matrix.Zero(2, 1);
int fveci = 0;
for (int i = 0; i < nSets; i++)
{
var rotation = new Matrix(3, 1);
rotation[0] = p[pi++];
rotation[1] = p[pi++];
rotation[2] = p[pi++];
var R = RotationMatrixFromRotationVector(rotation);
var t = new Matrix(3, 1);
t[0] = p[pi++];
t[1] = p[pi++];
t[2] = p[pi++];
var worldPoints = worldPointSets[i];
var imagePoints = imagePointSets[i];
var x = new Matrix(3, 1);
for (int j = 0; j < worldPoints.Count; j++)
{
// transform world point to local camera coordinates
x.Mult(R, worldPoints[j]);
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[j];
fvec[fveci++] = imagePoint.X - u;
fvec[fveci++] = imagePoint.Y - v;
}
}
return(fvec);
};
// optimize
var calibrate = new LevenbergMarquardt(function);
calibrate.minimumReduction = 1.0e-4;
calibrate.Minimize(parameters);
//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 f = parameters[pi++];
//double cx = parameters[pi++];
//double cy = parameters[pi++];
//cameraMatrix[0, 0] = f;
//cameraMatrix[1, 1] = f;
//cameraMatrix[0, 2] = cx;
//cameraMatrix[1, 2] = cy;
for (int i = 0; i < nSets; i++)
{
rotations[i][0] = parameters[pi++];
rotations[i][1] = parameters[pi++];
rotations[i][2] = parameters[pi++];
translations[i][0] = parameters[pi++];
translations[i][1] = parameters[pi++];
translations[i][2] = parameters[pi++];
}
}
return(calibrate.RMSError);
}
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 void TestPlanarDLT()
{
var cameraMatrix = Matrix.Identity(3, 3);
cameraMatrix[0, 0] = 300;
cameraMatrix[1, 1] = 300;
cameraMatrix[0, 2] = 250;
cameraMatrix[1, 2] = 220;
var distCoeffs = new Matrix(5, 1);
distCoeffs[0] = 0.05;
distCoeffs[1] = -0.1;
// generate a bunch of points in a plane
// project under some other camera (view)
var R = new Matrix(3, 3);
R.RotEuler2Matrix(0.3, -0.2, 0.6);
var t = new Matrix(3, 1);
t[0] = 0.2;
t[1] = 0.3;
t[2] = 2;
var modelR = new Matrix(3, 3);
modelR.RotEuler2Matrix(-0.6, 0.2, 0.3);
var modelT = new Matrix(3, 1);
modelT[0] = -0.1;
modelT[1] = 1.0;
modelT[2] = 1.5;
var worldPoints = new List <Matrix>();
var worldTransformedPoints = new List <Matrix>();
var imagePoints = new List <System.Drawing.PointF>();
var zero3 = Matrix.Zero(3, 1);
for (float y = -1f; y <= 1.0f; y += 0.2f)
{
for (float x = -1f; x <= 1.0f; x += 0.2f)
{
var model = new Matrix(3, 1);
model[0] = x;
model[1] = y;
model[2] = 0;
var noise = Matrix.GaussianSample(zero3, 0.1 * 0.1);
var world = new Matrix(3, 1);
world.Mult(modelR, model);
world.Add(modelT);
world.Add(noise);
worldPoints.Add(world);
// under some camera:
var worldTransformed = new Matrix(3, 1);
worldTransformed.Mult(R, world);
worldTransformed.Add(t);
worldTransformedPoints.Add(worldTransformed);
double u, v;
Project(cameraMatrix, distCoeffs, worldTransformed[0], worldTransformed[1], worldTransformed[2], out u, out v);
var image = new System.Drawing.PointF();
image.X = (float)u;
image.Y = (float)v;
imagePoints.Add(image);
}
}
Console.WriteLine("R\n" + R);
Console.WriteLine("t\n" + t);
var Rplane = new Matrix(3, 1);
var Tplane = new Matrix(3, 1);
PlaneFit(worldPoints, out Rplane, out Tplane);
var Rest = new Matrix(3, 3);
var test = new Matrix(3, 1);
PlanarDLT(cameraMatrix, distCoeffs, worldPoints, imagePoints, Rplane, Tplane, out Rest, out test);
Console.WriteLine("Rest\n" + Rest);
Console.WriteLine("test\n" + test);
}
public static void TestDLT()
{
var cameraMatrix = Matrix.Identity(3, 3);
cameraMatrix[0, 0] = 700;
cameraMatrix[1, 1] = 700;
cameraMatrix[0, 2] = 250;
cameraMatrix[1, 2] = 220;
var distCoeffs = new Matrix(5, 1);
distCoeffs[0] = 0.05;
distCoeffs[1] = -0.1;
// generate a bunch of points in a volume
// project under some other camera (view)
var R = new Matrix(3, 3);
R.RotEuler2Matrix(0.2, 0.3, 0.3);
var t = new Matrix(3, 1);
t[0] = 2;
t[1] = 0;
t[2] = -4;
var modelPoints = new List <Matrix>();
var imagePoints = new List <System.Drawing.PointF>();
var zero3 = Matrix.Zero(3, 1);
for (float z = 1f; z <= 3.0f; z += 0.4f)
{
for (float y = -1f; y <= 1.0f; y += 0.4f)
{
for (float x = -1f; x <= 1.0f; x += 0.4f)
{
var model = new Matrix(3, 1);
model[0] = x;
model[1] = y;
model[2] = z;
modelPoints.Add(model);
// under our camera:
var transformedPoint = new Matrix(3, 1);
transformedPoint.Mult(R, model);
transformedPoint.Add(t);
var noise = Matrix.GaussianSample(zero3, 0.1 * 0.1);
transformedPoint.Add(noise);
double u, v;
Project(cameraMatrix, distCoeffs, transformedPoint[0], transformedPoint[1], transformedPoint[2], out u, out v);
var image = new System.Drawing.PointF();
image.X = (float)u;
image.Y = (float)v;
imagePoints.Add(image);
}
}
}
Console.WriteLine("x = [");
for (int i = 0; i < imagePoints.Count; i++)
{
Console.WriteLine("{0} {1}", imagePoints[i].X, imagePoints[i].Y);
}
Console.WriteLine("]';");
Console.WriteLine("X = [");
for (int i = 0; i < modelPoints.Count; i++)
{
Console.WriteLine("{0} {1} {2}", modelPoints[i][0], modelPoints[i][1], modelPoints[i][2]);
}
Console.WriteLine("]';");
Console.WriteLine("fc = [{0} {1}];", cameraMatrix[0, 0], cameraMatrix[1, 1]);
Console.WriteLine("cc = [{0} {1}];", cameraMatrix[0, 2], cameraMatrix[1, 2]);
Console.WriteLine("kc = [{0} {1} 0 0 0];", distCoeffs[0], distCoeffs[1]);
Console.WriteLine();
Console.WriteLine("R\n" + R);
Console.WriteLine("t\n" + t);
var Rest = new Matrix(3, 3);
var test = new Matrix(3, 1);
DLT(cameraMatrix, distCoeffs, modelPoints, imagePoints, out Rest, out test);
Console.WriteLine("Rest\n" + Rest);
Console.WriteLine("test\n" + test);
}
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;
}