private Matrix4x4 GetProjectionMatrix(RoomAliveToolkit.Matrix intrinsics, float zNear, float zFar)
{
float c_x = (float)intrinsics[0, 2];
float c_y = (float)intrinsics[1, 2];
//the intrinsics are in Kinect coordinates: X - left, Y - up, Z, forward
//we need the coordinates to be: X - right, Y - down, Z - forward
c_x = imageWidth - c_x;
c_y = imageHeight - c_y;
// http://spottrlabs.blogspot.com/2012/07/opencv-and-opengl-not-always-friends.html
// http://opencv.willowgarage.com/wiki/Posit
Matrix4x4 projMat = new Matrix4x4();
projMat[0, 0] = (float)(2.0 * intrinsics[0, 0] / imageWidth);
projMat[1, 1] = (float)(2.0 * intrinsics[1, 1] / imageHeight);
projMat[2, 0] = (float)(-1.0f + 2 * c_x / imageWidth);
projMat[2, 1] = (float)(-1.0f + 2 * c_y / imageHeight);
// Note this changed from previous code
// see here: http://www.songho.ca/opengl/gl_projectionmatrix.html
projMat[2, 2] = -(zFar + zNear) / (zFar - zNear);
projMat[3, 2] = -2.0f * zNear * zFar / (zFar - zNear);
projMat[2, 3] = -1;
// Transpose tp fit Unity's column major matrix (in contrast to vision raw major ones).
projMat = projMat.transpose;
return(projMat);
}
public void RecoverCalibrationFromSensor(KinectSensor kinectSensor)
{
var stopWatch = new System.Diagnostics.Stopwatch();
stopWatch.Start();
var objectPoints1 = new List <RoomAliveToolkit.Matrix>();
var colorPoints1 = new List <System.Drawing.PointF>();
var depthPoints1 = new List <System.Drawing.PointF>();
int n = 0;
for (float x = -2f; x < 2f; x += 0.2f)
{
for (float y = -2f; y < 2f; y += 0.2f)
{
for (float z = 0.4f; z < 4.5f; z += 0.4f)
{
var kinectCameraPoint = new CameraSpacePoint();
kinectCameraPoint.X = x;
kinectCameraPoint.Y = y;
kinectCameraPoint.Z = z;
// use SDK's projection
// adjust Y to make RH cooridnate system that is a projection of Kinect 3D points
var kinectColorPoint = kinectSensor.CoordinateMapper.MapCameraPointToColorSpace(kinectCameraPoint);
kinectColorPoint.Y = colorImageHeight - kinectColorPoint.Y;
var kinectDepthPoint = kinectSensor.CoordinateMapper.MapCameraPointToDepthSpace(kinectCameraPoint);
kinectDepthPoint.Y = depthImageHeight - kinectDepthPoint.Y;
if ((kinectColorPoint.X >= 0) && (kinectColorPoint.X < colorImageWidth) &&
(kinectColorPoint.Y >= 0) && (kinectColorPoint.Y < colorImageHeight) &&
(kinectDepthPoint.X >= 0) && (kinectDepthPoint.X < depthImageWidth) &&
(kinectDepthPoint.Y >= 0) && (kinectDepthPoint.Y < depthImageHeight))
{
n++;
var objectPoint = new RoomAliveToolkit.Matrix(3, 1);
objectPoint[0] = kinectCameraPoint.X;
objectPoint[1] = kinectCameraPoint.Y;
objectPoint[2] = kinectCameraPoint.Z;
objectPoints1.Add(objectPoint);
var colorPoint = new System.Drawing.PointF();
colorPoint.X = kinectColorPoint.X;
colorPoint.Y = kinectColorPoint.Y;
colorPoints1.Add(colorPoint);
//Console.WriteLine(objectPoint[0] + "\t" + objectPoint[1] + "\t" + colorPoint.X + "\t" + colorPoint.Y);
var depthPoint = new System.Drawing.PointF();
depthPoint.X = kinectDepthPoint.X;
depthPoint.Y = kinectDepthPoint.Y;
depthPoints1.Add(depthPoint);
}
}
}
}
colorCameraMatrix[0, 0] = 1000; //fx
colorCameraMatrix[1, 1] = 1000; //fy
colorCameraMatrix[0, 2] = colorImageWidth / 2; //cx
colorCameraMatrix[1, 2] = colorImageHeight / 2; //cy
colorCameraMatrix[2, 2] = 1;
var rotation = new Matrix(3, 1);
var translation = new Matrix(3, 1);
var colorError = CalibrateColorCamera(objectPoints1, colorPoints1, colorCameraMatrix, colorLensDistortion, rotation, translation);
//var rotationMatrix = Orientation.Rodrigues(rotation);
var rotationMatrix = RoomAliveToolkit.ProjectorCameraEnsemble.RotationMatrixFromRotationVector(rotation);
depthToColorTransform = Matrix.Identity(4, 4);
for (int i = 0; i < 3; i++)
{
depthToColorTransform[i, 3] = translation[i];
for (int j = 0; j < 3; j++)
{
depthToColorTransform[i, j] = rotationMatrix[i, j];
}
}
depthCameraMatrix[0, 0] = 360; //fx
depthCameraMatrix[1, 1] = 360; //fy
depthCameraMatrix[0, 2] = depthImageWidth / 2; //cx
depthCameraMatrix[1, 2] = depthImageHeight / 2; //cy
depthCameraMatrix[2, 2] = 1;
var depthError = CalibrateDepthCamera(objectPoints1, depthPoints1, depthCameraMatrix, depthLensDistortion);
//// latest SDK gives access to depth intrinsics directly -- this gives slightly higher projection error; not sure why
//var depthIntrinsics = kinectSensor.CoordinateMapper.GetDepthCameraIntrinsics();
//depthCameraMatrix[0, 0] = depthIntrinsics.FocalLengthX;
//depthCameraMatrix[1, 1] = depthIntrinsics.FocalLengthY;
//depthCameraMatrix[0, 2] = depthIntrinsics.PrincipalPointX;
//depthCameraMatrix[1, 2] = depthImageHeight - depthIntrinsics.PrincipalPointY; // note flip in Y!
//depthDistCoeffs[0] = depthIntrinsics.RadialDistortionSecondOrder;
//depthDistCoeffs[1] = depthIntrinsics.RadialDistortionFourthOrder;
// check projections
double depthProjectionError = 0;
double colorProjectionError = 0;
var color = new RoomAliveToolkit.Matrix(4, 1);
var testObjectPoint4 = new RoomAliveToolkit.Matrix(4, 1);
for (int i = 0; i < n; i++)
{
var testObjectPoint = objectPoints1[i];
var testDepthPoint = depthPoints1[i];
var testColorPoint = colorPoints1[i];
// "camera space" == depth camera space
// depth camera projection
double depthU, depthV;
CameraMath.Project(depthCameraMatrix, depthLensDistortion, testObjectPoint[0], testObjectPoint[1], testObjectPoint[2], out depthU, out depthV);
double dx = testDepthPoint.X - depthU;
double dy = testDepthPoint.Y - depthV;
depthProjectionError += (dx * dx) + (dy * dy);
// color camera projection
testObjectPoint4[0] = testObjectPoint[0];
testObjectPoint4[1] = testObjectPoint[1];
testObjectPoint4[2] = testObjectPoint[2];
testObjectPoint4[3] = 1;
color.Mult(depthToColorTransform, testObjectPoint4);
color.Scale(1.0 / color[3]); // not necessary for this transform
double colorU, colorV;
CameraMath.Project(colorCameraMatrix, colorLensDistortion, color[0], color[1], color[2], out colorU, out colorV);
dx = testColorPoint.X - colorU;
dy = testColorPoint.Y - colorV;
colorProjectionError += (dx * dx) + (dy * dy);
}
depthProjectionError /= n;
colorProjectionError /= n;
stopWatch.Stop();
Console.WriteLine("FakeCalibration :");
Console.WriteLine("n = " + n);
Console.WriteLine("color error = " + colorError);
Console.WriteLine("depth error = " + depthError);
Console.WriteLine("depth reprojection error = " + depthProjectionError);
Console.WriteLine("color reprojection error = " + colorProjectionError);
Console.WriteLine("depth camera matrix = \n" + depthCameraMatrix);
Console.WriteLine("depth lens distortion = \n" + depthLensDistortion);
Console.WriteLine("color camera matrix = \n" + colorCameraMatrix);
Console.WriteLine("color lens distortion = \n" + colorLensDistortion);
Console.WriteLine(stopWatch.ElapsedMilliseconds + " ms");
//// get camera space table
//// this does not change frame to frame (or so I believe)
//var tableEntries = kinectSensor.CoordinateMapper.GetDepthFrameToCameraSpaceTable();
//// compute our own version of the camera space table and compare it to the SDK's
//stopWatch.Restart();
//var tableEntries2 = ComputeDepthFrameToCameraSpaceTable();
//Console.WriteLine("ComputeDepthFrameToCameraSpaceTable took " + stopWatch.ElapsedMilliseconds + " ms");
//{
// float error = 0;
// for (int framey = 0; framey < depthImageHeight; framey++)
// for (int framex = 0; framex < depthImageWidth; framex++)
// {
// var point1 = tableEntries[depthImageWidth * framey + framex];
// var point2 = tableEntries2[depthImageWidth * framey + framex];
// error += (float)Math.Sqrt((point1.X - point2.X) * (point1.X - point2.X) + (point1.Y - point2.Y) * (point1.Y - point2.Y));
// }
// error /= (float)(depthImageHeight * depthImageWidth);
// Console.WriteLine("error = " + error);
//}
}
public void RecoverCalibrationFromSensor(KinectSensor kinectSensor)
{
var stopWatch = new System.Diagnostics.Stopwatch();
stopWatch.Start();
var objectPoints1 = new List<RoomAliveToolkit.Matrix>();
var colorPoints1 = new List<System.Drawing.PointF>();
var depthPoints1 = new List<System.Drawing.PointF>();
int n = 0;
for (float x = -2f; x < 2f; x += 0.2f)
for (float y = -2f; y < 2f; y += 0.2f)
for (float z = 0.4f; z < 4.5f; z += 0.4f)
{
var kinectCameraPoint = new CameraSpacePoint();
kinectCameraPoint.X = x;
kinectCameraPoint.Y = y;
kinectCameraPoint.Z = z;
// use SDK's projection
// adjust Y to make RH cooridnate system that is a projection of Kinect 3D points
var kinectColorPoint = kinectSensor.CoordinateMapper.MapCameraPointToColorSpace(kinectCameraPoint);
kinectColorPoint.Y = colorImageHeight - kinectColorPoint.Y;
var kinectDepthPoint = kinectSensor.CoordinateMapper.MapCameraPointToDepthSpace(kinectCameraPoint);
kinectDepthPoint.Y = depthImageHeight - kinectDepthPoint.Y;
if ((kinectColorPoint.X >= 0) && (kinectColorPoint.X < colorImageWidth) &&
(kinectColorPoint.Y >= 0) && (kinectColorPoint.Y < colorImageHeight) &&
(kinectDepthPoint.X >= 0) && (kinectDepthPoint.X < depthImageWidth) &&
(kinectDepthPoint.Y >= 0) && (kinectDepthPoint.Y < depthImageHeight))
{
n++;
var objectPoint = new RoomAliveToolkit.Matrix(3, 1);
objectPoint[0] = kinectCameraPoint.X;
objectPoint[1] = kinectCameraPoint.Y;
objectPoint[2] = kinectCameraPoint.Z;
objectPoints1.Add(objectPoint);
var colorPoint = new System.Drawing.PointF();
colorPoint.X = kinectColorPoint.X;
colorPoint.Y = kinectColorPoint.Y;
colorPoints1.Add(colorPoint);
//Console.WriteLine(objectPoint[0] + "\t" + objectPoint[1] + "\t" + colorPoint.X + "\t" + colorPoint.Y);
var depthPoint = new System.Drawing.PointF();
depthPoint.X = kinectDepthPoint.X;
depthPoint.Y = kinectDepthPoint.Y;
depthPoints1.Add(depthPoint);
}
}
colorCameraMatrix[0, 0] = 1000; //fx
colorCameraMatrix[1, 1] = 1000; //fy
colorCameraMatrix[0, 2] = colorImageWidth / 2; //cx
colorCameraMatrix[1, 2] = colorImageHeight / 2; //cy
colorCameraMatrix[2, 2] = 1;
var rotation = new Matrix(3, 1);
var translation = new Matrix(3, 1);
var colorError = CalibrateColorCamera(objectPoints1, colorPoints1, colorCameraMatrix, colorLensDistortion, rotation, translation);
//var rotationMatrix = Orientation.Rodrigues(rotation);
var rotationMatrix = RoomAliveToolkit.ProjectorCameraEnsemble.RotationMatrixFromRotationVector(rotation);
depthToColorTransform = Matrix.Identity(4, 4);
for (int i = 0; i < 3; i++)
{
depthToColorTransform[i, 3] = translation[i];
for (int j = 0; j < 3; j++)
depthToColorTransform[i, j] = rotationMatrix[i, j];
}
depthCameraMatrix[0, 0] = 360; //fx
depthCameraMatrix[1, 1] = 360; //fy
depthCameraMatrix[0, 2] = depthImageWidth / 2; //cx
depthCameraMatrix[1, 2] = depthImageHeight / 2; //cy
depthCameraMatrix[2, 2] = 1;
var depthError = CalibrateDepthCamera(objectPoints1, depthPoints1, depthCameraMatrix, depthLensDistortion);
//// latest SDK gives access to depth intrinsics directly -- this gives slightly higher projection error; not sure why
//var depthIntrinsics = kinectSensor.CoordinateMapper.GetDepthCameraIntrinsics();
//depthCameraMatrix[0, 0] = depthIntrinsics.FocalLengthX;
//depthCameraMatrix[1, 1] = depthIntrinsics.FocalLengthY;
//depthCameraMatrix[0, 2] = depthIntrinsics.PrincipalPointX;
//depthCameraMatrix[1, 2] = depthImageHeight - depthIntrinsics.PrincipalPointY; // note flip in Y!
//depthDistCoeffs[0] = depthIntrinsics.RadialDistortionSecondOrder;
//depthDistCoeffs[1] = depthIntrinsics.RadialDistortionFourthOrder;
// check projections
double depthProjectionError = 0;
double colorProjectionError = 0;
var color = new RoomAliveToolkit.Matrix(4, 1);
var testObjectPoint4 = new RoomAliveToolkit.Matrix(4, 1);
for (int i = 0; i < n; i++)
{
var testObjectPoint = objectPoints1[i];
var testDepthPoint = depthPoints1[i];
var testColorPoint = colorPoints1[i];
// "camera space" == depth camera space
// depth camera projection
double depthU, depthV;
CameraMath.Project(depthCameraMatrix, depthLensDistortion, testObjectPoint[0], testObjectPoint[1], testObjectPoint[2], out depthU, out depthV);
double dx = testDepthPoint.X - depthU;
double dy = testDepthPoint.Y - depthV;
depthProjectionError += (dx * dx) + (dy * dy);
// color camera projection
testObjectPoint4[0] = testObjectPoint[0];
testObjectPoint4[1] = testObjectPoint[1];
testObjectPoint4[2] = testObjectPoint[2];
testObjectPoint4[3] = 1;
color.Mult(depthToColorTransform, testObjectPoint4);
color.Scale(1.0 / color[3]); // not necessary for this transform
double colorU, colorV;
CameraMath.Project(colorCameraMatrix, colorLensDistortion, color[0], color[1], color[2], out colorU, out colorV);
dx = testColorPoint.X - colorU;
dy = testColorPoint.Y - colorV;
colorProjectionError += (dx * dx) + (dy * dy);
}
depthProjectionError /= n;
colorProjectionError /= n;
stopWatch.Stop();
Console.WriteLine("FakeCalibration :");
Console.WriteLine("n = " + n);
Console.WriteLine("color error = " + colorError);
Console.WriteLine("depth error = " + depthError);
Console.WriteLine("depth reprojection error = " + depthProjectionError);
Console.WriteLine("color reprojection error = " + colorProjectionError);
Console.WriteLine("depth camera matrix = \n" + depthCameraMatrix);
Console.WriteLine("depth lens distortion = \n" + depthLensDistortion);
Console.WriteLine("color camera matrix = \n" + colorCameraMatrix);
Console.WriteLine("color lens distortion = \n" + colorLensDistortion);
Console.WriteLine(stopWatch.ElapsedMilliseconds + " ms");
//// get camera space table
//// this does not change frame to frame (or so I believe)
//var tableEntries = kinectSensor.CoordinateMapper.GetDepthFrameToCameraSpaceTable();
//// compute our own version of the camera space table and compare it to the SDK's
//stopWatch.Restart();
//var tableEntries2 = ComputeDepthFrameToCameraSpaceTable();
//Console.WriteLine("ComputeDepthFrameToCameraSpaceTable took " + stopWatch.ElapsedMilliseconds + " ms");
//{
// float error = 0;
// for (int framey = 0; framey < depthImageHeight; framey++)
// for (int framex = 0; framex < depthImageWidth; framex++)
// {
// var point1 = tableEntries[depthImageWidth * framey + framex];
// var point2 = tableEntries2[depthImageWidth * framey + framex];
// error += (float)Math.Sqrt((point1.X - point2.X) * (point1.X - point2.X) + (point1.Y - point2.Y) * (point1.Y - point2.Y));
// }
// error /= (float)(depthImageHeight * depthImageWidth);
// Console.WriteLine("error = " + error);
//}
}
public void CalibrateProjectorGroups(string directory)
{
// for all cameras, take depth image points to color image points
var depthImage = new FloatImage(depthWidth, depthHeight);
var varianceImage = new FloatImage(depthWidth, depthHeight);
var validMask = new ByteImage(depthWidth, depthHeight);
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 < depthHeight; y += 1)
for (int x = 0; x < depthWidth; 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 * depthWidth + 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 < (colorWidth - 1)) && (colorV >= 0) && (colorV < (colorHeight - 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)(depthWidth * depthHeight) + "% 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(colorWidth, colorHeight);
var decodedRows = new ShortImage(colorWidth, colorHeight);
var mask = new ByteImage(colorWidth, colorHeight);
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 = colorHeight - (int)(colorImagePoint[1] + 0.5f);
if ((x < 0) || (x >= colorWidth) || (y < 0) || (y >= colorHeight))
{
//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
for (int i = 0; i < 4; 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)
{
Console.WriteLine("Unable to find noncoplanar points.");
return; // TODO: should return an error, throw an exception, or try to recover
// 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 outliers
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);
// 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 = Vision.Orientation.Rodrigues(rotations[setIndex]);
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++;
}
}
}
}
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 static List<RoomAliveToolkit.Matrix> TransformPoints(RoomAliveToolkit.Matrix A, List<RoomAliveToolkit.Matrix> points)
{
var transformedPoints = new List<RoomAliveToolkit.Matrix>();
var point4 = new RoomAliveToolkit.Matrix(4, 1);
point4[3] = 1;
var transformedPoint4 = new RoomAliveToolkit.Matrix(4, 1);
foreach (var point in points)
{
point4[0] = point[0]; point4[1] = point[1]; point4[2] = point[2];
transformedPoint4.Mult(A, point4);
transformedPoint4.Scale(1.0f / transformedPoint4[3]);
var transformedPoint = new RoomAliveToolkit.Matrix(3, 1);
transformedPoint[0] = transformedPoint4[0]; transformedPoint[1] = transformedPoint4[1]; transformedPoint[2] = transformedPoint4[2];
transformedPoints.Add(transformedPoint);
}
return transformedPoints;
}
public static double PlaneFit(IList<Matrix> points, out Matrix X, out double D)
{
X = new Matrix(3, 1);
var mu = new RoomAliveToolkit.Matrix(3, 1);
for (int i = 0; i < points.Count; i++)
mu.Add(points[i]);
mu.Scale(1f / (float)points.Count);
var A = new RoomAliveToolkit.Matrix(3, 3);
var pc = new RoomAliveToolkit.Matrix(3, 1);
var M = new RoomAliveToolkit.Matrix(3, 3);
for (int i = 0; i < points.Count; i++)
{
var p = points[i];
pc.Sub(p, mu);
M.Outer(pc, pc);
A.Add(M);
}
var V = new RoomAliveToolkit.Matrix(3, 3);
var d = new RoomAliveToolkit.Matrix(3, 1);
A.Eig(V, d); // TODO: replace with 3x3 version?
//Console.WriteLine("------");
//Console.WriteLine(A);
//Console.WriteLine(V);
//Console.WriteLine(d);
double minEigenvalue = Double.MaxValue;
int minEigenvaluei = 0;
for (int i = 0; i < 3; i++)
if (d[i] < minEigenvalue)
{
minEigenvalue = d[i];
minEigenvaluei = i;
}
X.CopyCol(V, minEigenvaluei);
D = -X.Dot(mu);
// min eigenvalue is the sum of squared distances to the plane
// signed distance is: double distance = X.Dot(point) + D;
return minEigenvalue;
}