public static void Undistort(Vision.Matrix cameraMatrix, Vision.Matrix distCoeffs, double xin, double yin, out double xout, out double yout)
{
float fx = (float)cameraMatrix[0, 0];
float fy = (float)cameraMatrix[1, 1];
float cx = (float)cameraMatrix[0, 2];
float cy = (float)cameraMatrix[1, 2];
float[] kappa = new float[] { (float)distCoeffs[0], (float)distCoeffs[1] };
Undistort(fx, fy, cx, cy, kappa, xin, yin, out xout, out yout);
}
public static void Project(Vision.Matrix cameraMatrix, Vision.Matrix distCoeffs,
double x, double y, double z, out double u, out double v)
{
double xp = x / z;
double yp = y / z;
double fx = cameraMatrix[0, 0];
double fy = cameraMatrix[1, 1];
double cx = cameraMatrix[0, 2];
double cy = cameraMatrix[1, 2];
double k1 = distCoeffs[0];
double k2 = distCoeffs[1];
// compute f(xp, yp)
double rSquared = xp * xp + yp * yp;
double xpp = xp * (1 + k1 * rSquared + k2 * rSquared * rSquared);
double ypp = yp * (1 + k1 * rSquared + k2 * rSquared * rSquared);
u = fx * xpp + cx;
v = fy * ypp + cy;
}
public void RecoverCalibrationFromSensor(KinectSensor kinectSensor)
{
var stopWatch = new System.Diagnostics.Stopwatch();
stopWatch.Start();
var objectPoints1 = new List <Vision.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 Vision.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, silent);
var rotationMatrix = Orientation.Rodrigues(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, silent);
//// 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 Vision.Matrix(4, 1);
var testObjectPoint4 = new Vision.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;
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;
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();
if (!silent)
{
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");
Console.WriteLine("________________________________________________________");
}
//// 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);
//}
}