static public void Test()
{
// generate x_i, y_i observations on test function
var random = new Random();
int n = 200;
var X = new Matrix(n, 1);
var Y = new Matrix(n, 1);
{
double a = 100; double b = 102;
for (int i = 0; i < n; i++)
{
double x = random.NextDouble() / (Math.PI / 4.0) - Math.PI / 8.0;
double y = a * Math.Cos(b * x) + b * Math.Sin(a * x) + random.NextDouble() * 0.1;
X[i] = x;
Y[i] = y;
}
}
Function f = delegate(Matrix parameters)
{
// return y_i - f(x_i, parameters) as column vector
var error = new Matrix(n, 1);
double a = parameters[0];
double b = parameters[1];
for (int i = 0; i < n; i++)
{
double y = a * Math.Cos(b * X[i]) + b * Math.Sin(a * X[i]);
error[i] = Y[i] - y;
}
return(error);
};
var levenbergMarquardt = new LevenbergMarquardt(f);
var parameters0 = new Matrix(2, 1);
parameters0[0] = 90;
parameters0[1] = 96;
var rmsError = levenbergMarquardt.Minimize(parameters0);
}
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 = 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);
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;
}
static double CalibrateDepthCamera(List<Matrix> worldPoints, List<System.Drawing.PointF> imagePoints, Matrix cameraMatrix, Matrix distCoeffs)
{
int nPoints = worldPoints.Count;
// pack parameters into vector
// parameters: fx, fy, cx, cy, k1, k2 = 6 parameters
int nParameters = 6;
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
}
// 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;
int fveci = 0;
for (int i = 0; i < worldPoints.Count; i++)
{
// fvec_i = y_i - f(x_i)
double u, v;
var x = worldPoints[i];
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;
}
return calibrate.RMSError;
}
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);
}
static double CalibrateDepthCamera(List <Matrix> worldPoints, List <System.Drawing.PointF> imagePoints, Matrix cameraMatrix, Matrix distCoeffs)
{
int nPoints = worldPoints.Count;
// pack parameters into vector
// parameters: fx, fy, cx, cy, k1, k2 = 6 parameters
int nParameters = 6;
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
}
// 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;
int fveci = 0;
for (int i = 0; i < worldPoints.Count; i++)
{
// fvec_i = y_i - f(x_i)
double u, v;
var x = worldPoints[i];
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;
}
return(calibrate.RMSError);
}
static public void Test()
{
// generate x_i, y_i observations on test function
var random = new Random();
int n = 200;
var X = new Matrix(n, 1);
var Y = new Matrix(n, 1);
{
double a = 100; double b = 102;
for (int i = 0; i < n; i++)
{
double x = random.NextDouble() / (Math.PI / 4.0) - Math.PI / 8.0;
double y = a * Math.Cos(b * x) + b * Math.Sin(a * x) + random.NextDouble()*0.1;
X[i] = x;
Y[i] = y;
}
}
Function f = delegate(Matrix parameters)
{
// return y_i - f(x_i, parameters) as column vector
var error = new Matrix(n, 1);
double a = parameters[0];
double b = parameters[1];
for (int i = 0; i < n; i++)
{
double y = a * Math.Cos(b * X[i]) + b * Math.Sin(a * X[i]);
error[i] = Y[i] - y;
}
return error;
};
var levenbergMarquardt = new LevenbergMarquardt(f);
var parameters0 = new Matrix(2, 1);
parameters0[0] = 90;
parameters0[1] = 96;
var rmsError = levenbergMarquardt.Minimize(parameters0);
}
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;
}
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);
}
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);
}
