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