public KinectCalibrator(CalibrationResult calib)
{
this.calib = calib;
var ir2rgbExtrin = new CV.ExtrinsicCameraParameters(
new CV.RotationVector3D(new double[] { 0.56 * Math.PI / 180.0, 0.07 * Math.PI / 180.0, +0.05 * Math.PI / 180.0 }),
new CV.Matrix<double>(new double[,] { { -0.0256 }, { 0.00034 }, { 0.00291 } })).ExtrinsicMatrix;
IR2RGB = DenseMatrix.OfArray(new float[,]
{
{ (float)ir2rgbExtrin.Data[0,0], (float)ir2rgbExtrin.Data[0,1], (float)ir2rgbExtrin.Data[0,2], (float)ir2rgbExtrin.Data[0,3] },
{ (float)ir2rgbExtrin.Data[1,0], (float)ir2rgbExtrin.Data[1,1], (float)ir2rgbExtrin.Data[1,2], (float)ir2rgbExtrin.Data[1,3] },
{ (float)ir2rgbExtrin.Data[2,0], (float)ir2rgbExtrin.Data[2,1], (float)ir2rgbExtrin.Data[2,2], (float)ir2rgbExtrin.Data[2,3] },
{0,0,0,1}
});
var rt = calib.Extrinsic.ExtrinsicMatrix;
K2G = DenseMatrix.OfArray(new Single[,]
{
{ (float)rt.Data[0,0], (float)rt.Data[0,1], (float)rt.Data[0,2], (float)rt.Data[0,3] },
{ (float)rt.Data[1,0], (float)rt.Data[1,1], (float)rt.Data[1,2], (float)rt.Data[1,3] },
{ (float)rt.Data[2,0], (float)rt.Data[2,1], (float)rt.Data[2,2], (float)rt.Data[2,3] },
{0,0,0,1}
}).Inverse();
var flip = DenseMatrix.OfArray(new Single[,]
{
{ -1,0,0,0 },
{ 0,-1,0,0 },
{ 0,0,1,0 },
{ 0,0,0,1 }
});
EXTRA = K2G * IR2RGB * flip;
}
public void SetProjection(CalibrationResult calib)
{
parent.MakeCurrent();
var Width = parent.Width;
var Height = parent.Height;
GL.Viewport(0,0, Width, Height);
var rt = calib.Extrinsic.ExtrinsicMatrix.Data;
var extrin = new OpenTK.Matrix4d(
rt[0, 0], rt[1, 0], rt[2, 0], 0,
rt[0, 1], rt[1, 1], rt[2, 1], 0,
rt[0, 2], rt[1, 2], rt[2, 2], 0,
rt[0, 3], rt[1, 3], rt[2, 3], 1);
GL.MatrixMode(MatrixMode.Projection);
GL.LoadMatrix(ref extrin);
GL.MatrixMode(MatrixMode.Modelview);
GL.LoadIdentity();
var intrin = calib.Intrinsic.IntrinsicMatrix.Data;
var dist = calib.Intrinsic.DistortionCoeffs.Data;
F = new Vector2((float)intrin[0, 0], (float)intrin[1, 1]);
C = new Vector2((float)intrin[0, 2], (float)intrin[1, 2]);
K = new Matrix4(
(float)dist[0, 0], (float)dist[1, 0], (float)dist[2, 0], (float)dist[3, 0],
(float)dist[4, 0], (float)dist[5, 0], (float)dist[6, 0], (float)dist[7, 0],
0,0,0,0,
0,0,0,0);
}
static CalibrationResult CalibrateProjector(KinectSensor sensor, Camera camera, Projector projector, CalibrationResult cameraCalib, PointF[][] cacalibdata, bool reload = false, bool save = true)
{
CalibrationResult result;
string datafile = "projectorcalibration.xml";
if (!reload && File.Exists(datafile))
result = Utils.DeSerializeObject<CalibrationResult>(datafile);
else
{
VoiceCommander commander = new VoiceCommander(sensor);
commander.LoadChoices("Shoot", "Ready", "Next", "Continue", "Carry on", "Smaller", "Bigger", "Go", "Stop");
List<PointF[]> cam = new List<PointF[]>(), proj = new List<PointF[]>();
Size pattern = new Size(8, 7);
PointF[] cc;
PointF[] pc;
double size = 0.5;
pc = projector.DrawCheckerboard(pattern, 0, 0, 0, size);
while (true)
{
var word = commander.Recognize("Ready?");
if (word == "Bigger")
{
size += 0.1;
pc = projector.DrawCheckerboard(pattern, 0, 0, 0, size);
continue;
}
else if (word == "Smaller")
{
size -= 0.1;
pc = projector.DrawCheckerboard(pattern, 0, 0, 0, size);
continue;
}
if (word == "Stop")
break;
cc = StereoCalibration.FindDualPlaneCorners(camera, pattern);
if (cc != null && pc != null)
{
cam.Add(cc);
proj.Add(pc);
}
}
projector.DrawBackground(Color.Black);
result = StereoCalibration.CalibrateProjector(projector, cacalibdata, cam.ToArray(), proj.ToArray(), new Size(7, 4), 0.05f);
//result = StereoCalibration.CalibrateProjector(projector, camera, new Size(8, 7), cameraCalib, cacalibdata, new Size(7, 4), 0.05f);
if (save)
Utils.SerializeObject(result, datafile);
}
return result;
}
public static PointF[] Undistort(CalibrationResult calib, PointF[] points)
{
return calib.Intrinsic.Undistort(points, null, null);
}
public static CalibrationResult CalibrateProjector(Projector projector, Camera camera, Size pattern, CalibrationResult cameraCalib, PointF[][] cacalibdata, Size cameraPattern, float checkerboardSize)
{
List<PointF[]> cameraCorners = new List<PointF[]>();
List<PointF[]> projectorCorners = new List<PointF[]>();
var cpattern = new Size(pattern.Width - 1, pattern.Height - 1);
int steps = 15;
double rotx = 0, roty = 0, rotz = 0;
for (int i = 0; i < steps; i++)
{
var di = (double)i / (double)steps;
rotx = 0;
roty = Math.Sin(di * Math.PI / 2) * 0.8;
rotz = Math.Sin(di * Math.PI / 2) * 0.6;
var pcs = projector.DrawCheckerboard(pattern,
rotx,
roty,
rotz, 0.5);
var img = camera.TakePicture(3);
var ccs = GetCameraCorners(img, cpattern, false);
if (ccs != null)
{
projectorCorners.Add(pcs);
cameraCorners.Add(ccs);
var withCorners = camera.TakePicture(0);
if (DebugWindow != null)
{
QuickDraw.Start(withCorners)
.Color(Color.White)
.DrawPoint(ccs, 5)
.Finish();
DebugWindow.DrawBitmap(withCorners);
}
}
}
//for (int i = 0; i < steps; i++)
//{
// var di = (double)i / (double)steps;
// rotx += 0.04;
// roty *= 0.90;
// var pcs = projector.DrawCheckerboard(pattern,
// rotx,
// roty,
// rotz, 0.7);
// var ccs = GetCameraCorners(camera, cpattern, false);
// if (ccs != null)
// {
// projectorCorners.Add(pcs);
// cameraCorners.Add(ccs);
// var withCorners = camera.TakePicture(0);
// if (DebugWindow != null)
// {
// QuickDraw.Start(withCorners)
// .Color(Color.White)
// .DrawPoint(ccs, 5)
// .Finish();
// DebugWindow.DrawBitmap(withCorners);
// }
// }
//}
var hm = CreateHomography(cameraCorners.ToArray(), projectorCorners.ToArray());
var globals = GenerateCheckerBoard(cameraPattern, checkerboardSize, 0);
var globalCorners = cacalibdata.Select(row => globals).ToArray();
var proj = cacalibdata.Select(cs => cs.Select(c => new PointF(c.X, c.Y)).ToArray()).ToArray();
foreach (var p in proj)
hm.ProjectPoints(p);
IntrinsicCameraParameters projIntrin = new IntrinsicCameraParameters();
ExtrinsicCameraParameters[] projExtrins;
Emgu.CV.CameraCalibration.CalibrateCamera(globalCorners, proj, projector.Size, projIntrin, Emgu.CV.CvEnum.CALIB_TYPE.CV_CALIB_RATIONAL_MODEL, out projExtrins);
return new CalibrationResult() { Intrinsic = projIntrin, Extrinsic = projExtrins.First() };
}
public static CalibrationResult CalibrateCamera(PointF[] cameraCorners, Size pattern, float checkerBoardSize, CalibrationResult useIntrinsic)
{
var globals = GenerateCheckerBoard(pattern, checkerBoardSize, 0);
var globalCorners = new MCvPoint3D32f[][] { globals };
var intrinsic = useIntrinsic.Intrinsic;
var extrinsic = Emgu.CV.CameraCalibration.FindExtrinsicCameraParams2(globals, cameraCorners, intrinsic);
return new CalibrationResult() { Intrinsic = intrinsic, Extrinsic = extrinsic };
}
public void SetProjection(CalibrationResult calib, Matrix4? modelView = null, Matrix4? offset = null)
{
this.calib = calib;
parent.MakeCurrent();
var Width = parent.Width;
var Height = parent.Height;
GL.Viewport(0, 0, Width, Height);
var rt = calib.Extrinsic.ExtrinsicMatrix.Data;
var extrin = new OpenTK.Matrix4(
(float)rt[0, 0], (float)rt[1, 0], (float)rt[2, 0], 0,
(float)rt[0, 1], (float)rt[1, 1], (float)rt[2, 1], 0,
(float)rt[0, 2], (float)rt[1, 2], (float)rt[2, 2], 0,
(float)rt[0, 3], (float)rt[1, 3], (float)rt[2, 3], 1) * (offset.HasValue ? offset.Value : Matrix4.Identity);
GL.MatrixMode(MatrixMode.Projection);
GL.LoadMatrix(ref extrin);
GL.MatrixMode(MatrixMode.Modelview);
if (modelView == null)
GL.LoadIdentity();
else
{
var mat = modelView.Value;
GL.LoadMatrix(ref mat);
}
var intrin = calib.Intrinsic.IntrinsicMatrix.Data;
var dist = calib.Intrinsic.DistortionCoeffs.Data;
F = new Vector2((float)intrin[0, 0], (float)intrin[1, 1]);
C = new Vector2((float)intrin[0, 2], (float)intrin[1, 2]);
K = new Matrix4(
(float)dist[0, 0], (float)dist[1, 0], (float)dist[2, 0], (float)dist[3, 0],
(float)dist[4, 0], (float)dist[5, 0], (float)dist[6, 0], (float)dist[7, 0],
0, 0, 0, 0,
0, 0, 0, 0);
}
