public void Copy(FloatImage x, FloatImage y, FloatImage z)
{
Float3* p = data;
float* px = x.Data(0, 0);
float* py = y.Data(0, 0);
float* pz = z.Data(0, 0);
for (int i = 0; i < width * height; i++)
{
p->x = *px++;
p->y = *py++;
p->z = *pz++;
p++;
}
}
public void Copy(FloatImage a, float min, float max)
{
float *pa = a.Data(0, 0);
byte * p = data;
float s = 255.0f / (max - min);
for (int i = 0; i < width * height; i++)
{
int value = (int)(s * (*pa++ - min));
if (value < 0)
{
*p++ = 0;
}
else if (value > 255)
{
*p++ = (byte)255;
}
else
{
*p++ = (byte)value;
}
}
}
public void CaptureDepthAndColor(string directory)
{
// foreach camera:
// average a bunch of frames to find a good depth image
// get calibration
// TODO: parallelize
foreach (var camera in cameras)
{
string cameraDirectory = directory + "/camera" + camera.name;
if (!Directory.Exists(cameraDirectory))
Directory.CreateDirectory(cameraDirectory);
// compute mean and variance of depth image
var sum = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
sum.Zero();
var sumSquared = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
sumSquared.Zero();
var count = new ShortImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
count.Zero();
var depth = new ShortImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
for (int i = 0; i < 100; i++)
{
var depthBytes = camera.Client.LatestDepthImage();
Marshal.Copy(depthBytes, 0, depth.DataIntPtr, Kinect2Calibration.depthImageWidth * Kinect2Calibration.depthImageHeight * 2);
Console.WriteLine("acquired depth image " + i);
for (int y = 0; y < Kinect2Calibration.depthImageHeight; y++)
for (int x = 0; x < Kinect2Calibration.depthImageWidth; x++)
if (depth[x, y] != 0)
{
ushort d = depth[x, y];
count[x, y]++;
sum[x, y] += d;
sumSquared[x, y] += d * d;
}
}
var meanImage = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
meanImage.Zero(); // not all pixels will be assigned
var varianceImage = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
varianceImage.Zero(); // not all pixels will be assigned
for (int y = 0; y < Kinect2Calibration.depthImageHeight; y++)
for (int x = 0; x < Kinect2Calibration.depthImageWidth; x++)
{
if (count[x, y] > 50)
{
float mean = sum[x, y] / count[x, y];
meanImage[x, y] = mean;
float variance = sumSquared[x, y] / count[x, y] - mean * mean;
varianceImage[x, y] = variance;
}
}
// WIC doesn't support encoding float tiff images, so for now we write to a binary file
meanImage.SaveToFile(cameraDirectory + "/mean.bin");
varianceImage.SaveToFile(cameraDirectory + "/variance.bin");
// create a short version that we can write, used only for debugging
var meanDepthShortImage = new ShortImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
for (int y = 0; y < Kinect2Calibration.depthImageHeight; y++)
for (int x = 0; x < Kinect2Calibration.depthImageWidth; x++)
meanDepthShortImage[x, y] = (ushort)meanImage[x, y];
SaveToTiff(imagingFactory, meanDepthShortImage, cameraDirectory + "/mean.tiff");
// convert to world coordinates and save to ply file
camera.calibration = camera.Client.GetCalibration();
var depthFrameToCameraSpaceTable = camera.calibration.ComputeDepthFrameToCameraSpaceTable();
var world = new Float3Image(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight); // TODO: move out/reuse
for (int y = 0; y < Kinect2Calibration.depthImageHeight; y++)
for (int x = 0; x < Kinect2Calibration.depthImageWidth; x++)
{
var pointF = depthFrameToCameraSpaceTable[y * Kinect2Calibration.depthImageWidth + x];
float meanDepthMeters = meanImage[x, y] / 1000.0f;
Float3 worldPoint;
worldPoint.x = pointF.X * meanDepthMeters;
worldPoint.y = pointF.Y * meanDepthMeters;
worldPoint.z = meanDepthMeters;
world[x, y] = worldPoint;
}
SaveToPly(cameraDirectory + "/mean.ply", world);
// TODO: consider writing OBJ instead
}
//// connect to projectors
//foreach (var projector in projectors)
//{
// projector.Client.OpenDisplay(projector.displayIndex);
//}
// collect color images; this is not necessary for calibration, but is nice to have for visualization
//foreach (var projector in projectors)
// projector.Client.SetColor(projector.displayIndex, 0f, 0f, 0f);
//System.Threading.Thread.Sleep(5000);
foreach (var camera in cameras)
{
// save color image
string cameraDirectory = directory + "/camera" + camera.name;
var jpegBytes = camera.Client.LatestJPEGImage();
File.WriteAllBytes(cameraDirectory + "/color.jpg", jpegBytes);
var colorBytes = camera.Client.LatestRGBImage();
var image = new ARGBImage(Kinect2Calibration.colorImageWidth, Kinect2Calibration.colorImageHeight);
Marshal.Copy(colorBytes, 0, image.DataIntPtr, Kinect2Calibration.colorImageWidth * Kinect2Calibration.colorImageHeight * 4);
SaveToTiff(imagingFactory, image, cameraDirectory + "/color.tiff");
image.Dispose();
}
//// close all displays
//foreach (var projector in projectors)
//{
// projector.Client.CloseDisplay(projector.displayIndex);
//}
}
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 SaveToOBJ(string directory, string objPath)
{
var objFilename = Path.GetFileNameWithoutExtension(objPath);
var objDirectory = Path.GetDirectoryName(objPath);
if (!Directory.Exists(objDirectory))
Directory.CreateDirectory(objDirectory);
// Because we need to form triangles, we go back to the depth image
var quadOffsets = new System.Drawing.Point[]
{
new System.Drawing.Point(0, 0),
new System.Drawing.Point(1, 0),
new System.Drawing.Point(0, 1),
new System.Drawing.Point(1, 0),
new System.Drawing.Point(1, 1),
new System.Drawing.Point(0, 1),
};
var streamWriter = new CultureInvariantStreamWriter(objDirectory + "/" + objFilename + ".obj");
var mtlFileWriter = new CultureInvariantStreamWriter(objDirectory + "/" + objFilename + ".mtl");
streamWriter.WriteLine("mtllib " + objFilename + ".mtl");
uint nextVertexIndex = 1;
var depthImage = new FloatImage(Kinect2Calibration.depthImageWidth, Kinect2Calibration.depthImageHeight);
foreach (var camera in cameras)
{
mtlFileWriter.WriteLine("newmtl camera" + camera.name);
mtlFileWriter.WriteLine("Ka 1.000000 1.000000 1.000000");
mtlFileWriter.WriteLine("Kd 1.000000 1.000000 1.000000");
mtlFileWriter.WriteLine("Ks 0.000000 0.000000 0.000000");
mtlFileWriter.WriteLine("Tr 1.000000");
mtlFileWriter.WriteLine("illum 1");
mtlFileWriter.WriteLine("Ns 0.000000");
mtlFileWriter.WriteLine("map_Kd " + objFilename + "_" + camera.name + ".jpg");
File.Copy(directory + "/camera" + camera.name + "/color.jpg", objDirectory + "/" + objFilename + "_" + camera.name + ".jpg", true);
streamWriter.WriteLine("usemtl camera" + camera.name);
// load depth image
string cameraDirectory = directory + "/camera" + camera.name;
depthImage.LoadFromFile(cameraDirectory + "/mean.bin");
var calibration = camera.calibration;
var depthFrameToCameraSpaceTable = calibration.ComputeDepthFrameToCameraSpaceTable();
var vertices = new Vertex[Kinect2Calibration.depthImageWidth * Kinect2Calibration.depthImageHeight];
var colorCamera = new Matrix(4, 1);
var depthCamera = new Matrix(4, 1);
var world = new Matrix(4, 1);
for (int y = 0; y < Kinect2Calibration.depthImageHeight; y++)
for (int x = 0; x < Kinect2Calibration.depthImageWidth; x++)
{
// depth camera coords
var depth = depthImage[x, y] / 1000f; // m
// convert to depth camera space
var point = depthFrameToCameraSpaceTable[Kinect2Calibration.depthImageWidth * y + x];
depthCamera[0] = point.X * depth;
depthCamera[1] = point.Y * depth;
depthCamera[2] = depth;
depthCamera[3] = 1;
// world coordinates
world.Mult(camera.pose, depthCamera);
//world.Scale(1.0 / world[3]); not necessary for this transform
// convert to color camera space
colorCamera.Mult(calibration.depthToColorTransform, depthCamera);
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);
colorU /= (double)Kinect2Calibration.colorImageWidth;
colorV /= (double)Kinect2Calibration.colorImageHeight;
var vertex = new Vertex();
vertex.x = (float)world[0];
vertex.y = (float)world[1];
vertex.z = (float)world[2];
vertex.u = (float)colorU;
vertex.v = (float)colorV;
vertices[Kinect2Calibration.depthImageWidth * y + x] = vertex;
}
streamWriter.WriteLine("g camera" + camera.name);
streamWriter.WriteLine("usemtl camera" + camera.name);
// examine each triangle
for (int y = 0; y < Kinect2Calibration.depthImageHeight - 1; y++)
for (int x = 0; x < Kinect2Calibration.depthImageWidth - 1; x++)
{
int offseti = 0;
for (int tri = 0; tri < 2; tri++)
{
// the indexes of the vertices of this triangle
var i0 = Kinect2Calibration.depthImageWidth * (y + quadOffsets[offseti].Y) + (x + quadOffsets[offseti].X);
var i1 = Kinect2Calibration.depthImageWidth * (y + quadOffsets[offseti + 1].Y) + (x + quadOffsets[offseti + 1].X);
var i2 = Kinect2Calibration.depthImageWidth * (y + quadOffsets[offseti + 2].Y) + (x + quadOffsets[offseti + 2].X);
// is triangle valid?
bool nonZero = (vertices[i0].z != 0) && (vertices[i1].z != 0) && (vertices[i2].z != 0);
bool jump01 = Vertex.DistanceSquared(vertices[i0], vertices[i1]) < 0.2 * 0.2;
bool jump02 = Vertex.DistanceSquared(vertices[i0], vertices[i2]) < 0.2 * 0.2;
bool jump12 = Vertex.DistanceSquared(vertices[i1], vertices[i2]) < 0.2 * 0.2;
bool valid = nonZero && jump01 && jump02 && jump12;
if (valid)
{
// only add the vertex if we haven't already
if (vertices[i0].index == 0)
{
streamWriter.WriteLine(vertices[i0]);
vertices[i0].index = nextVertexIndex++;
}
if (vertices[i1].index == 0)
{
streamWriter.WriteLine(vertices[i1]);
vertices[i1].index = nextVertexIndex++;
}
if (vertices[i2].index == 0)
{
streamWriter.WriteLine(vertices[i2]);
vertices[i2].index = nextVertexIndex++;
}
streamWriter.WriteLine("f {0}/{0} {1}/{1} {2}/{2}", vertices[i0].index, vertices[i1].index, vertices[i2].index);
}
offseti += 3;
}
}
}
streamWriter.Close();
mtlFileWriter.Close();
}
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);
}
public void Blur3x3(FloatImage a)
{
float* output = data + width + 1;
float* pb02 = a.data + 2;
float* pb12 = a.data + width + 2;
float* pb22 = a.data + 2 * width + 2;
float s0, s1, s2;
float h;
for (int y = 0; y < height - 2; y++)
{
h = 0;
s0 = 0; s1 = 0; s2 = 0;
for (int x = 0; x < width; x++)
{
h -= s0;
s0 = s1;
s1 = s2;
s2 = *pb02++ + *pb12++ + *pb22++;
h += s2;
float g = h / 9;
*output++ = (float)g;
}
}
}
public void Copy(FloatImage a, float min, float max)
{
float* pa = a.Data(0, 0);
byte* p = data;
float s = 255.0f / (max - min);
for (int i = 0; i < width * height; i++)
{
int value = (int)(s * (*pa++ - min));
if (value < 0)
*p++ = 0;
else if (value > 255)
*p++ = (byte)255;
else
*p++ = (byte)value;
}
}
public void Threshold(FloatImage a, float threshold)
{
float* pa = a.Data(0, 0);
byte* p = data;
for (int i = 0; i < width * height; i++)
{
if (*pa++ > threshold)
*p++ = 255;
else
*p++ = 0;
}
}
public void CaptureDepthAndColor(string directory)
{
// foreach camera:
// average a bunch of frames to find a good depth image
// get calibration
// TODO: parallelize
foreach (var camera in cameras)
{
string cameraDirectory = directory + "/camera" + camera.name;
if (!Directory.Exists(cameraDirectory))
Directory.CreateDirectory(cameraDirectory);
// compute mean and variance of depth image
var sum = new FloatImage(depthWidth, depthHeight);
sum.Zero();
var sumSquared = new FloatImage(depthWidth, depthHeight);
sumSquared.Zero();
var count = new ShortImage(depthWidth, depthHeight);
count.Zero();
var depth = new ShortImage(depthWidth, depthHeight);
for (int i = 0; i < 100; i++)
{
var depthBytes = camera.Client.LatestDepthImage();
Marshal.Copy(depthBytes, 0, depth.DataIntPtr, depthWidth * depthHeight * 2);
Console.WriteLine("acquired depth image " + i);
for (int y = 0; y < depthHeight; y++)
for (int x = 0; x < depthWidth; x++)
if (depth[x, y] != 0)
{
ushort d = depth[x, y];
count[x, y]++;
sum[x, y] += d;
sumSquared[x, y] += d * d;
}
}
var meanImage = new FloatImage(depthWidth, depthHeight);
meanImage.Zero(); // not all pixels will be assigned
var varianceImage = new FloatImage(depthWidth, depthHeight);
varianceImage.Zero(); // not all pixels will be assigned
for (int y = 0; y < depthHeight; y++)
for (int x = 0; x < depthWidth; x++)
{
if (count[x, y] > 50)
{
float mean = sum[x, y] / count[x, y];
meanImage[x, y] = mean;
float variance = sumSquared[x, y] / count[x, y] - mean * mean;
varianceImage[x, y] = variance;
}
}
// WIC doesn't support encoding float tiff images, so for now we write to a binary file
meanImage.SaveToFile(cameraDirectory + "/mean.bin");
varianceImage.SaveToFile(cameraDirectory + "/variance.bin");
// create a short version that we can write, used only for debugging
var meanDepthShortImage = new ShortImage(depthWidth, depthHeight);
for (int y = 0; y < depthHeight; y++)
for (int x = 0; x < depthWidth; x++)
meanDepthShortImage[x, y] = (ushort)meanImage[x, y];
SaveToTiff(imagingFactory, meanDepthShortImage, cameraDirectory + "/mean.tiff");
// convert to world coordinates and save to ply file
camera.calibration = camera.Client.GetCalibration();
var depthFrameToCameraSpaceTable = camera.calibration.ComputeDepthFrameToCameraSpaceTable();
var world = new Float3Image(depthWidth, depthHeight); // TODO: move out/reuse
for (int y = 0; y < depthHeight; y++)
for (int x = 0; x < depthWidth; x++)
{
var pointF = depthFrameToCameraSpaceTable[y * depthWidth + x];
Float3 worldPoint;
worldPoint.x = pointF.X * meanImage[x, y];
worldPoint.y = pointF.Y * meanImage[x, y];
worldPoint.z = meanImage[x, y];
world[x, y] = worldPoint;
}
SaveToPly(cameraDirectory + "/mean.ply", world);
// TODO: consider writing OBJ instead
}
// connect to projectors
foreach (var projector in projectors)
{
//var binding = new NetTcpBinding();
//binding.Security.Mode = SecurityMode.None;
//var uri = "net.tcp://" + projector.hostNameOrAddress + ":9001/ProjectorServer/service";
//var address = new EndpointAddress(uri);
//projector.client = new ProjectorServerClient(binding, address);
projector.Client.OpenDisplay(projector.displayIndex);
}
// collect color images when projecting all white and all black
// set projectors to white
foreach (var projector in projectors)
projector.Client.SetColor(projector.displayIndex, 1f, 1f, 1f);
System.Threading.Thread.Sleep(5000);
foreach (var camera in cameras)
{
// save color image
string cameraDirectory = directory + "/camera" + camera.name;
var jpegBytes = camera.Client.LatestJPEGImage();
File.WriteAllBytes(cameraDirectory + "/color.jpg", jpegBytes);
var colorBytes = camera.Client.LatestRGBImage();
var image = new ARGBImage(colorWidth, colorHeight);
Marshal.Copy(colorBytes, 0, image.DataIntPtr, colorWidth * colorHeight * 4);
SaveToTiff(imagingFactory, image, cameraDirectory + "/color.tiff");
image.Dispose();
}
foreach (var projector in projectors)
projector.Client.SetColor(projector.displayIndex, 0f, 0f, 0f);
System.Threading.Thread.Sleep(5000);
foreach (var camera in cameras)
{
// save color image
string cameraDirectory = directory + "/camera" + camera.name;
var jpegBytes = camera.Client.LatestJPEGImage();
File.WriteAllBytes(cameraDirectory + "/colorDark.jpg", jpegBytes);
var colorBytes = camera.Client.LatestRGBImage();
var image = new ARGBImage(colorWidth, colorHeight);
Marshal.Copy(colorBytes, 0, image.DataIntPtr, colorWidth * colorHeight * 4);
SaveToTiff(imagingFactory, image, cameraDirectory + "/colorDark.tiff");
image.Dispose();
}
// close all displays
foreach (var projector in projectors)
{
projector.Client.CloseDisplay(projector.displayIndex);
}
}
