public void CalibrateProjectorGroups(string directory)
{
// for all cameras, take depth image points to color image points
var depthImage = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
var varianceImage = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
var validMask = new ByteImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
foreach (var camera in cameras)
{
Console.WriteLine("projecting depth points to color camera " + camera.name);
// load depth image
string cameraDirectory = directory + "/camera" + camera.name;
depthImage.LoadFromFile(cameraDirectory + "/mean.bin");
varianceImage.LoadFromFile(cameraDirectory + "/variance.bin");
validMask.Zero();
var calibration = camera.calibration;
var depthFrameToCameraSpaceTable = calibration.ComputeDepthFrameToCameraSpaceTable();
// TODO: consider using just one 4x4 in calibration class
var colorCamera = new Matrix(4, 1);
camera.colorImagePoints = new List<Matrix>();
camera.depthCameraPoints = new List<Matrix>();
var depthCamera4 = new Matrix(4, 1);
// for each valid point in depth image
int numRejected = 0;
for (int y = 0; y < Kinect2Calibration.depthImageHeight; y += 1)
for (int x = 0; x < Kinect2Calibration.depthImageWidth; x += 1)
{
float depth = depthImage[x, y] / 1000f; // m
float variance = varianceImage[x, y];
if (depth == 0)
continue;
if (variance > 6 * 6)
{
numRejected++;
continue;
}
validMask[x, y] = (byte)255;
// convert to depth camera space
var point = depthFrameToCameraSpaceTable[y * Kinect2Calibration.depthImageWidth + x];
depthCamera4[0] = point.X * depth;
depthCamera4[1] = point.Y * depth;
depthCamera4[2] = depth;
depthCamera4[3] = 1;
// convert to color camera space
colorCamera.Mult(calibration.depthToColorTransform, depthCamera4);
//colorCamera.Scale(1.0 / colorCamera[3]);
// project to color image
double colorU, colorV;
CameraMath.Project(calibration.colorCameraMatrix, calibration.colorLensDistortion, colorCamera[0], colorCamera[1], colorCamera[2], out colorU, out colorV);
if ((colorU >= 0) && (colorU < (Kinect2Calibration.colorImageWidth - 1)) && (colorV >= 0) && (colorV < (Kinect2Calibration.colorImageHeight - 1))) // BEWARE: later do we round or truncate??
{
var colorImagePoint = new Matrix(2, 1);
colorImagePoint[0] = colorU;
colorImagePoint[1] = colorV;
camera.colorImagePoints.Add(colorImagePoint);
// expect a 3-vector?
var depthCamera = new Matrix(3, 1);
depthCamera[0] = depthCamera4[0];
depthCamera[1] = depthCamera4[1];
depthCamera[2] = depthCamera4[2];
camera.depthCameraPoints.Add(depthCamera);
//Console.WriteLine(depthCamera[0] + "\t" + depthCamera[1] + "\t -> " + colorImagePoint[0] + "\t" + colorImagePoint[1]);
}
}
SaveToTiff(imagingFactory, validMask, cameraDirectory + "/validMask.tiff");
Console.WriteLine("rejected " + 100 * (float)numRejected / (float)(Kinect2Calibration.depthImageWidth * Kinect2Calibration.depthImageHeight) + "% pixels for high variance");
}
// we never save colorImagePoints, depthCameraPoints, so we must remember to run previous
Console.WriteLine("elapsed time " + stopWatch.ElapsedMilliseconds);
// use decoded Gray code images to create calibration point sets
foreach (var projector in projectors)
{
string projectorDirectory = directory + "/projector" + projector.name;
projector.calibrationPointSets = new Dictionary<Camera, CalibrationPointSet>();
foreach (var camera in cameras)
{
string cameraDirectory = projectorDirectory + "/camera" + camera.name;
var decodedColumns = new ShortImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight);
var decodedRows = new ShortImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight);
var mask = new ByteImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight);
LoadFromTiff(imagingFactory, decodedColumns, cameraDirectory + "/decodedColumns.tiff");
LoadFromTiff(imagingFactory, decodedRows, cameraDirectory + "/decodedRows.tiff");
LoadFromTiff(imagingFactory, mask, cameraDirectory + "/mask.tiff");
// we have a bunch of color camera / depth camera point corrspondences
// use the Gray code to find the position of the color camera point in the projector frame
// find 2D projector coordinates from decoded Gray code images
var imagePoints = new List<System.Drawing.PointF>();
var worldPoints = new List<Matrix>();
for (int i = 0; i < camera.colorImagePoints.Count; i++)
{
var colorImagePoint = camera.colorImagePoints[i];
// We would like to relate projected color points to color images stored in memory.
// The Kinect SDK and our camera calibration assumes X left, Y up (from the POV of the camera).
// We index images in memory with X right and Y down.
// Our Gray code images are flipped in the horizontal direction.
// Therefore to map an image space coordinate to a memory location we flip Y (and not X):
int x = (int)(colorImagePoint[0] + 0.5f);
int y = Kinect2Calibration.colorImageHeight - (int)(colorImagePoint[1] + 0.5f);
if ((x < 0) || (x >= Kinect2Calibration.colorImageWidth) || (y < 0) || (y >= Kinect2Calibration.colorImageHeight))
{
//Console.WriteLine("out of bounds");
continue;
}
if (mask[x, y] > 0) // Gray code is valid
{
// We would like to relate decoded row/column values to projector coordinates.
// To match the camera, we want projector's coordinate system X left, Y up (from the POV of the projector).
// We assume that the projector is configured in front projection mode (i.e., projected text looks correct in the real world).
// In that case decoded columns run X right (in the real world), decoded rows run Y down (in the real world).
// So we need to flip both X and Y decoded values.
var projectorImagePoint = new System.Drawing.PointF(projector.width - decodedColumns[x, y], projector.height - decodedRows[x, y]);
var depthCameraPoint = camera.depthCameraPoints[i];
imagePoints.Add(projectorImagePoint);
worldPoints.Add(depthCameraPoint);
//Console.WriteLine(depthCameraPoint[0] + "\t" + depthCameraPoint[1] + "\t" + depthCameraPoint[2] + "-> \t" + x + "\t" + y + "-> \t" + projectorImagePoint.X + "\t" + projectorImagePoint.Y);
}
}
if (worldPoints.Count > 1000)
{
var pointSet = new CalibrationPointSet();
pointSet.worldPoints = worldPoints;
pointSet.imagePoints = imagePoints;
projector.calibrationPointSets[camera] = pointSet;
Console.WriteLine("projector " + projector.name + " is seen by camera " + camera.name + " (" + worldPoints.Count + " points)");
}
}
}
Console.WriteLine("elapsed time " + stopWatch.ElapsedMilliseconds);
// calibration
foreach (var projector in projectors)
{
Console.WriteLine("calibrating projector " + projector.name);
string projectorDirectory = directory + "/projector" + projector.name;
// RANSAC
double minError = Double.PositiveInfinity;
var random = new Random(0); // provide seed to ease debugging
int numCompletedFits = 0;
for (int i = 0; (numCompletedFits < 4) && (i < 10); i++)
{
Console.WriteLine("RANSAC iteration " + i);
// randomly select small number of points from each calibration set
var worldPointSubsets = new List<List<Matrix>>();
var imagePointSubsets = new List<List<System.Drawing.PointF>>();
bool foundNonplanarSubset = false;
foreach (var pointSet in projector.calibrationPointSets.Values)
{
var worldPointSubset = new List<Matrix>();
var imagePointSubset = new List<System.Drawing.PointF>();
// try to find a nonplanar subset
bool planar = true;
int nTries = 0;
while (planar && (nTries++ < 1000))
{
worldPointSubset.Clear();
imagePointSubset.Clear();
for (int j = 0; j < 100; j++)
{
int k = random.Next(pointSet.worldPoints.Count);
worldPointSubset.Add(pointSet.worldPoints[k]);
imagePointSubset.Add(pointSet.imagePoints[k]);
}
// planar?
Matrix Rplane, tplane, d;
CameraMath.PlaneFit(worldPointSubset, out Rplane, out tplane, out d);
//Console.WriteLine("planar : " + d[2] / d[1]);
planar = (d[2] / d[1]) < 0.001f;
}
worldPointSubsets.Add(worldPointSubset);
imagePointSubsets.Add(imagePointSubset);
// we can't initialize extrinsics yet, because we don't know which intrinsics we'll be using
if (!planar)
foundNonplanarSubset = true;
}
// we do not optimize intrinsics if all the point sets are planar, or if the projector intrinsics are marked as locked
bool fixIntrinsics = (!foundNonplanarSubset) || (projector.lockIntrinsics); // TODO: add option to lock intrinsics
var rotations = new List<Matrix>();
var translations = new List<Matrix>();
var cameraMatrix = new Matrix(3, 3);
var distCoeffs = new Matrix(2, 1);
if (fixIntrinsics)
{
cameraMatrix.Copy(projector.cameraMatrix);
distCoeffs.Copy(projector.lensDistortion);
}
else // nonplanar, so we can optimize intrinsics
{
cameraMatrix[0, 0] = 1000; //fx TODO: can we instead init this from FOV?
cameraMatrix[1, 1] = 1000; //fy
cameraMatrix[0, 2] = projector.width / 2; //cx
cameraMatrix[1, 2] = 0; // projector lens shift; note this assumes desktop projection mode
cameraMatrix[2, 2] = 1;
}
// init extrinsics
for (int ii = 0; ii < worldPointSubsets.Count; ii++)
{
Matrix R, t;
CameraMath.ExtrinsicsInit(cameraMatrix, distCoeffs, worldPointSubsets[ii], imagePointSubsets[ii], out R, out t);
rotations.Add(CameraMath.RotationVectorFromRotationMatrix(R));
translations.Add(t);
}
// initial RANSAC fit on subset of points
double error;
if (fixIntrinsics)
error = CameraMath.CalibrateCameraExtrinsicsOnly(worldPointSubsets, imagePointSubsets, cameraMatrix, ref rotations, ref translations);
else
error = CameraMath.CalibrateCamera(worldPointSubsets, imagePointSubsets, cameraMatrix, ref rotations, ref translations);
Console.WriteLine("error on subset = " + error);
// RANSAC: find inliers from overall dataset
var worldPointInlierSets = new List<List<Matrix>>();
var imagePointInlierSets = new List<List<System.Drawing.PointF>>();
int setIndex = 0;
bool enoughInliers = true;
double sumError = 0;
int pointsInSum = 0;
int totalInliers = 0;
int totalPoints = 0;
foreach (var pointSet in projector.calibrationPointSets.Values)
{
var worldPointInlierSet = new List<Matrix>();
var imagePointInlierSet = new List<System.Drawing.PointF>();
var R = CameraMath.RotationMatrixFromRotationVector(rotations[setIndex]);
var t = translations[setIndex];
var p = new Matrix(3, 1);
for (int k = 0; k < pointSet.worldPoints.Count; k++)
{
p.Mult(R, pointSet.worldPoints[k]);
p.Add(t);
double u, v;
CameraMath.Project(cameraMatrix, distCoeffs, p[0], p[1], p[2], out u, out v);
double dx = pointSet.imagePoints[k].X - u;
double dy = pointSet.imagePoints[k].Y - v;
double thisError = Math.Sqrt((dx * dx) + (dy * dy));
if (thisError < 2.0f) // TODO: how to set this?
{
worldPointInlierSet.Add(pointSet.worldPoints[k]);
imagePointInlierSet.Add(pointSet.imagePoints[k]);
}
sumError += thisError * thisError;
pointsInSum++;
}
setIndex++;
// require that each view has a minimum number of inliers
enoughInliers = enoughInliers && (worldPointInlierSet.Count > 500); // should be related to min number of points in set (above)
totalPoints += pointSet.worldPoints.Count;
totalInliers += worldPointInlierSet.Count;
worldPointInlierSets.Add(worldPointInlierSet);
imagePointInlierSets.Add(imagePointInlierSet);
}
Console.WriteLine("{0}/{1} inliers", totalInliers, totalPoints);
// if number of inliers > some threshold (should be for each subset)
if (enoughInliers) // should this threshold be a function of the number of cameras, a percentage?
{
double error2;
if (fixIntrinsics)
error2 = CameraMath.CalibrateCameraExtrinsicsOnly(worldPointInlierSets, imagePointInlierSets, cameraMatrix, ref rotations, ref translations);
else
error2 = CameraMath.CalibrateCamera(worldPointInlierSets, imagePointInlierSets, cameraMatrix, ref rotations, ref translations);
Console.WriteLine("error with inliers = " + error2);
Console.Write("camera matrix = \n" + cameraMatrix);
numCompletedFits++;
// if reduced error save model (save rotation and translation to calibrationPointSets, cameraMatrix and distortion coeffs to projector)
if (error2 < minError)
{
minError = error2;
projector.cameraMatrix = cameraMatrix;
projector.lensDistortion = distCoeffs;
setIndex = 0;
foreach (var pointSet in projector.calibrationPointSets.Values)
{
// convert to 4x4 transform
var R = CameraMath.RotationMatrixFromRotationVector(rotations[setIndex]);
var t = translations[setIndex];
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];
}
pointSet.pose = T;
pointSet.worldPointInliers = worldPointInlierSets[setIndex];
pointSet.imagePointInliers = imagePointInlierSets[setIndex];
setIndex++;
}
}
}
}
if (numCompletedFits == 0)
throw new CalibrationFailedException("Unable to successfully calibrate projector: " + projector.name);
Console.WriteLine("final calibration:");
Console.Write("camera matrix = \n" + projector.cameraMatrix);
Console.Write("distortion = \n" + projector.lensDistortion);
Console.WriteLine("error = " + minError);
foreach (var camera in projector.calibrationPointSets.Keys)
{
Console.WriteLine("camera " + camera.name + " pose:");
Console.Write(projector.calibrationPointSets[camera].pose);
}
}
Console.WriteLine("elapsed time " + stopWatch.ElapsedMilliseconds);
//Console.WriteLine("x = [");
//for (int ii = 0; ii < imagePointSubsets[0].Count; ii++)
// Console.WriteLine("{0} {1}", imagePointSubsets[0][ii].X, imagePointSubsets[0][ii].Y);
//Console.WriteLine("]';");
//Console.WriteLine("X = [");
//for (int ii = 0; ii < worldPointSubsets[0].Count; ii++)
// Console.WriteLine("{0} {1} {2}", worldPointSubsets[0][ii][0], worldPointSubsets[0][ii][1], worldPointSubsets[0][ii][2]);
//Console.WriteLine("]';");
//Console.WriteLine("fc = [{0} {1}];", projector.cameraMatrix[0, 0], projector.cameraMatrix[1, 1]);
//Console.WriteLine("cc = [{0} {1}];", projector.cameraMatrix[0, 2], projector.cameraMatrix[1, 2]);
//Matrix thisR, thist;
//{
// Matrix Rplane, tplane;
// CameraMath.PlaneFit(worldPointSubsets[0], out Rplane, out tplane);
// CameraMath.PlanarDLT(projector.cameraMatrix, projector.lensDistortion, worldPointSubsets[0], imagePointSubsets[0], Rplane, tplane, out thisR, out thist);
// //Console.WriteLine("DLT---------");
// //Console.WriteLine(thisR);
// //Console.WriteLine(thist);
//}
//// if pattern is not planar, we can recover projector intrinsics
//List<RoomAliveToolkit.Matrix> rotations = null;
//List<RoomAliveToolkit.Matrix> translations = null;
//var error = CalibrateCamera(worldPointSubsets, imagePointSubsets, cameraMatrix, ref rotations, ref translations);
//Console.WriteLine("error = " + error);
// we check whether each view is planar, so that we can use the correct version of DLT
// the overall set may not be planar however, so we have to check the union of points
// if overall set is planar, leave intrinsics alone
//
}
public void CalibrateProjectorGroups(string directory)
{
// for all cameras, take depth image points to color image points
var depthImage = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
var varianceImage = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
var validMask = new ByteImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
foreach (var camera in cameras)
{
Console.WriteLine("projecting depth points to color camera " + camera.name);
// load depth image
string cameraDirectory = directory + "/camera" + camera.name;
depthImage.LoadFromFile(cameraDirectory + "/mean.bin");
varianceImage.LoadFromFile(cameraDirectory + "/variance.bin");
validMask.Zero();
var calibration = camera.calibration;
var depthFrameToCameraSpaceTable = calibration.ComputeDepthFrameToCameraSpaceTable();
// TODO: consider using just one 4x4 in calibration class
var colorCamera = new Matrix(4, 1);
camera.colorImagePoints = new List<Matrix>();
camera.depthCameraPoints = new List<Matrix>();
var depthCamera4 = new Matrix(4, 1);
// for each valid point in depth image
int numRejected = 0;
for (int y = 0; y < Kinect2Calibration.depthImageHeight; y += 1)
for (int x = 0; x < Kinect2Calibration.depthImageWidth; x += 1)
{
float depth = depthImage[x, y] / 1000f; // m
float variance = varianceImage[x, y];
if (depth == 0)
continue;
if (variance > 6 * 6)
{
numRejected++;
continue;
}
validMask[x, y] = (byte)255;
// convert to depth camera space
var point = depthFrameToCameraSpaceTable[y * Kinect2Calibration.depthImageWidth + x];
depthCamera4[0] = point.X * depth;
depthCamera4[1] = point.Y * depth;
depthCamera4[2] = depth;
depthCamera4[3] = 1;
// convert to color camera space
colorCamera.Mult(calibration.depthToColorTransform, depthCamera4);
//colorCamera.Scale(1.0 / colorCamera[3]);
// project to color image
double colorU, colorV;
CameraMath.Project(calibration.colorCameraMatrix, calibration.colorLensDistortion, colorCamera[0], colorCamera[1], colorCamera[2], out colorU, out colorV);
if ((colorU >= 0) && (colorU < (Kinect2Calibration.colorImageWidth - 1)) && (colorV >= 0) && (colorV < (Kinect2Calibration.colorImageHeight - 1))) // BEWARE: later do we round or truncate??
{
var colorImagePoint = new Matrix(2, 1);
colorImagePoint[0] = colorU;
colorImagePoint[1] = colorV;
camera.colorImagePoints.Add(colorImagePoint);
// expect a 3-vector?
var depthCamera = new Matrix(3, 1);
depthCamera[0] = depthCamera4[0];
depthCamera[1] = depthCamera4[1];
depthCamera[2] = depthCamera4[2];
camera.depthCameraPoints.Add(depthCamera);
//Console.WriteLine(depthCamera[0] + "\t" + depthCamera[1] + "\t -> " + colorImagePoint[0] + "\t" + colorImagePoint[1]);
}
}
SaveToTiff(imagingFactory, validMask, cameraDirectory + "/validMask.tiff");
Console.WriteLine("rejected " + 100 * (float)numRejected / (float)(Kinect2Calibration.depthImageWidth * Kinect2Calibration.depthImageHeight) + "% pixels for high variance");
}
// we never save colorImagePoints, depthCameraPoints, so we must remember to run previous
Console.WriteLine("elapsed time " + stopWatch.ElapsedMilliseconds);
// use decoded Gray code images to create calibration point sets
foreach (var projector in projectors)
{
string projectorDirectory = directory + "/projector" + projector.name;
projector.calibrationPointSets = new Dictionary<Camera, CalibrationPointSet>();
foreach (var camera in cameras)
{
string cameraDirectory = projectorDirectory + "/camera" + camera.name;
var decodedColumns = new ShortImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight);
var decodedRows = new ShortImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight);
var mask = new ByteImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight);
LoadFromTiff(imagingFactory, decodedColumns, cameraDirectory + "/decodedColumns.tiff");
LoadFromTiff(imagingFactory, decodedRows, cameraDirectory + "/decodedRows.tiff");
LoadFromTiff(imagingFactory, mask, cameraDirectory + "/mask.tiff");
// we have a bunch of color camera / depth camera point corrspondences
// use the Gray code to find the position of the color camera point in the projector frame
// find 2D projector coordinates from decoded Gray code images
var imagePoints = new List<System.Drawing.PointF>();
var worldPoints = new List<Matrix>();
for (int i = 0; i < camera.colorImagePoints.Count; i++)
{
var colorImagePoint = camera.colorImagePoints[i];
// We would like to relate projected color points to color images stored in memory.
// The Kinect SDK and our camera calibration assumes X left, Y up (from the POV of the camera).
// We index images in memory with X right and Y down.
// Our Gray code images are flipped in the horizontal direction.
// Therefore to map an image space coordinate to a memory location we flip Y (and not X):
int x = (int)(colorImagePoint[0] + 0.5f);
int y = Kinect2Calibration.colorImageHeight - (int)(colorImagePoint[1] + 0.5f);
if ((x < 0) || (x >= Kinect2Calibration.colorImageWidth) || (y < 0) || (y >= Kinect2Calibration.colorImageHeight))
{
//Console.WriteLine("out of bounds");
continue;
}
if (mask[x, y] > 0) // Gray code is valid
{
// We would like to relate decoded row/column values to projector coordinates.
// To match the camera, we want projector's coordinate system X left, Y up (from the POV of the projector).
// We assume that the projector is configured in front projection mode (i.e., projected text looks correct in the real world).
// In that case decoded columns run X right (in the real world), decoded rows run Y down (in the real world).
// So we need to flip both X and Y decoded values.
var projectorImagePoint = new System.Drawing.PointF(projector.width - decodedColumns[x, y], projector.height - decodedRows[x, y]);
var depthCameraPoint = camera.depthCameraPoints[i];
imagePoints.Add(projectorImagePoint);
worldPoints.Add(depthCameraPoint);
//Console.WriteLine(depthCameraPoint[0] + "\t" + depthCameraPoint[1] + "\t" + depthCameraPoint[2] + "-> \t" + x + "\t" + y + "-> \t" + projectorImagePoint.X + "\t" + projectorImagePoint.Y);
}
}
if (worldPoints.Count > 1000)
{
var pointSet = new CalibrationPointSet();
pointSet.worldPoints = worldPoints;
pointSet.imagePoints = imagePoints;
projector.calibrationPointSets[camera] = pointSet;
Console.WriteLine("projector " + projector.name + " is seen by camera " + camera.name + " (" + worldPoints.Count + " points)");
}
}
}
Console.WriteLine("elapsed time " + stopWatch.ElapsedMilliseconds);
// calibration
foreach (var projector in projectors)
{
Console.WriteLine("calibrating projector " + projector.name);
string projectorDirectory = directory + "/projector" + projector.name;
// RANSAC
double minError = Double.PositiveInfinity;
var random = new Random(0); // provide seed to ease debugging
int numCompletedFits = 0;
for (int i = 0; (numCompletedFits < 4) && (i < 10); i++)
{
Console.WriteLine("RANSAC iteration " + i);
// randomly select small number of points from each calibration set
var worldPointSubsets = new List<List<Matrix>>();
var imagePointSubsets = new List<List<System.Drawing.PointF>>();
foreach (var pointSet in projector.calibrationPointSets.Values)
{
var worldPointSubset = new List<Matrix>();
var imagePointSubset = new List<System.Drawing.PointF>();
bool nonCoplanar = false;
int nTries = 0;
while (!nonCoplanar)
{
for (int j = 0; j < 100; j++)
{
int k = random.Next(pointSet.worldPoints.Count);
worldPointSubset.Add(pointSet.worldPoints[k]);
imagePointSubset.Add(pointSet.imagePoints[k]);
}
// check that points are not coplanar
Matrix X;
double D;
double ssdToPlane = PlaneFit(worldPointSubset, out X, out D);
int numOutliers = 0;
foreach (var point in worldPointSubset)
{
double distanceFromPlane = X.Dot(point) + D;
if (Math.Abs(distanceFromPlane) > 0.1f)
numOutliers++;
}
nonCoplanar = (numOutliers > worldPointSubset.Count * 0.10f);
if (!nonCoplanar)
{
Console.WriteLine("points are coplanar (try #{0})", nTries);
worldPointSubset.Clear();
imagePointSubset.Clear();
}
if (nTries++ > 1000)
{
throw new CalibrationFailedException("Unable to find noncoplanar points.");
// consider moving this check up with variance check (when calibration point sets are formed)
}
}
worldPointSubsets.Add(worldPointSubset);
imagePointSubsets.Add(imagePointSubset);
}
var cameraMatrix = new Matrix(3, 3);
cameraMatrix[0, 0] = 1000; //fx TODO: can we instead init this from FOV?
cameraMatrix[1, 1] = 1000; //fy
cameraMatrix[0, 2] = projector.width / 2; //cx
cameraMatrix[1, 2] = 0; // projector lens shift; note this assumes desktop projection mode
cameraMatrix[2, 2] = 1;
var distCoeffs = new RoomAliveToolkit.Matrix(2, 1);
List<RoomAliveToolkit.Matrix> rotations = null;
List<RoomAliveToolkit.Matrix> translations = null;
var error = CalibrateCamera(worldPointSubsets, imagePointSubsets, cameraMatrix, ref rotations, ref translations);
Console.WriteLine("error = " + error);
//Console.WriteLine("intrinsics = \n" + cameraMatrix);
//// we differ from opencv's 'error' in that we do not distinguish between x and y.
//// i.e. opencv uses the method below; this number would match if we used pointsInSum2*2 in the divisor.
//// double check opencv's error
//{
// double sumError2 = 0;
// int pointsInSum2 = 0;
// for (int ii = 0; ii < worldPointSubsets.Count; ii++)
// {
// var R = Orientation.Rodrigues(rotations[ii]);
// var t = translations[ii];
// var p = new Matrix(3, 1);
// var worldPointSet = worldPointSubsets[ii];
// var imagePointSet = imagePointSubsets[ii];
// for (int k = 0; k < worldPointSet.Count; k++)
// {
// p.Mult(R, worldPointSet[k]);
// p.Add(t);
// double u, v;
// Kinect2.Kinect2Calibration.Project(cameraMatrix, distCoeffs, p[0], p[1], p[2], out u, out v);
// double dx = imagePointSet[k].X - u;
// double dy = imagePointSet[k].Y - v;
// double thisError = dx * dx + dy * dy;
// sumError2 += thisError;
// pointsInSum2++;
// }
// }
// // opencv's error is rms but over both x and y combined
// Console.WriteLine("average projection error = " + Math.Sqrt(sumError2 / (float)(pointsInSum2)));
//}
// find inliers from overall dataset
var worldPointInlierSets = new List<List<Matrix>>();
var imagePointInlierSets = new List<List<System.Drawing.PointF>>();
int setIndex = 0;
bool enoughInliers = true;
double sumError = 0;
int pointsInSum = 0;
foreach (var pointSet in projector.calibrationPointSets.Values)
{
var worldPointInlierSet = new List<Matrix>();
var imagePointInlierSet = new List<System.Drawing.PointF>();
//var R = Vision.Orientation.Rodrigues(rotations[setIndex]);
var R = RotationMatrixFromRotationVector(rotations[setIndex]);
var t = translations[setIndex];
var p = new Matrix(3, 1);
for (int k = 0; k < pointSet.worldPoints.Count; k++)
{
p.Mult(R, pointSet.worldPoints[k]);
p.Add(t);
double u, v;
CameraMath.Project(cameraMatrix, distCoeffs, p[0], p[1], p[2], out u, out v);
double dx = pointSet.imagePoints[k].X - u;
double dy = pointSet.imagePoints[k].Y - v;
double thisError = Math.Sqrt((dx * dx) + (dy * dy));
if (thisError < 1.0f)
{
worldPointInlierSet.Add(pointSet.worldPoints[k]);
imagePointInlierSet.Add(pointSet.imagePoints[k]);
}
sumError += thisError * thisError;
pointsInSum++;
}
setIndex++;
// require that each view has a minimum number of inliers
enoughInliers = enoughInliers && (worldPointInlierSet.Count > 1000);
worldPointInlierSets.Add(worldPointInlierSet);
imagePointInlierSets.Add(imagePointInlierSet);
}
// if number of inliers > some threshold (should be for each subset)
if (enoughInliers) // should this threshold be a function of the number of cameras, a percentage?
{
var error2 = CalibrateCamera(worldPointInlierSets, imagePointInlierSets, cameraMatrix, ref rotations, ref translations);
Console.WriteLine("error with inliers = " + error2);
Console.Write("camera matrix = \n" + cameraMatrix);
numCompletedFits++;
// if err < besterr save model (save rotation and translation to calibrationPointSets, cameraMatrix and distortion coeffs to projector)
if (error < minError)
{
minError = error;
projector.cameraMatrix = cameraMatrix;
projector.lensDistortion = distCoeffs;
setIndex = 0;
foreach (var pointSet in projector.calibrationPointSets.Values)
{
// convert to 4x4 transform
var R = RotationMatrixFromRotationVector(rotations[setIndex]);
var t = translations[setIndex];
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];
}
pointSet.pose = T;
pointSet.worldPointInliers = worldPointInlierSets[setIndex];
pointSet.imagePointInliers = imagePointInlierSets[setIndex];
setIndex++;
}
}
}
}
if (numCompletedFits == 0)
throw new CalibrationFailedException("Unable to successfully calibrate projector: " + projector.name);
Console.WriteLine("final calibration:");
Console.Write("camera matrix = \n" + projector.cameraMatrix);
Console.Write("distortion = \n" + projector.lensDistortion);
Console.WriteLine("error = " + minError);
foreach (var camera in projector.calibrationPointSets.Keys)
{
Console.WriteLine("camera " + camera.name + " pose:");
Console.Write(projector.calibrationPointSets[camera].pose);
}
}
Console.WriteLine("elapsed time " + stopWatch.ElapsedMilliseconds);
}
