public static void Calibrate(string[] args)
{
var camfile = args.FirstOrDefault() ?? KinectDefaultFileName;
var projfile = args.Skip(1).FirstOrDefault() ?? ProjectorDefaultFileName;
var main = OpenTK.DisplayDevice.AvailableDisplays.First(row => row.IsPrimary);
var window = new BitmapWindow(main.Bounds.Left + main.Width / 2 + 50, 50, 640, 480);
window.Load();
window.ResizeGraphics();
StereoCalibration.DebugWindow = window;
KinectSensor sensor = KinectSensor.KinectSensors.First();
sensor.Start();
Camera camera = new Camera(sensor, ColorImageFormat.RgbResolution1280x960Fps12);
Projector projector = new Projector();
PointF[][] data;
bool proceed = false;
CalibrationResult cc, pc;
do
{
cc = CalibrateCamera(camera, projector, out data, false, true, true);
pc = CalibrateProjector(sensor, camera, projector, cc, data, true, false);
var peas = new float[][] {
new float[] { 0f, 0f, 0.0f },
new float[] { 0.5f, 0f, 0.0f },
new float[] { 0f, -0.5f, 0.0f },
new float[] { 0.5f, -0.5f, 0.0f },
};
var tpe = pc.Transform(peas);
var tpp = cc.Transform(peas);
projector.DrawPoints(tpe, 25);
var pic2 = camera.TakePicture(5);
QuickDraw.Start(pic2).Color(Color.Green).DrawPoint(tpp, 15).Finish();
window.DrawBitmap(pic2);
Console.WriteLine("Save result? (y/n)");
proceed = Console.ReadLine() == "y";
} while (!proceed);
Utils.SerializeObject(cc, camfile);
Utils.SerializeObject(pc, projfile);
window.Close();
window.Dispose();
projector.Close();
camera.Dispose();
sensor.Stop();
}
public static void Run(string[] args)
{
var projfile = args.Skip(1).FirstOrDefault() ?? Calibration.ProjectorDefaultFileName;
if (!File.Exists(projfile))
{
Console.WriteLine("Either calib file could not be found.");
return;
}
var pc = Utils.DeSerializeObject<CalibrationResult>(projfile);
Projector proj = new Projector();
var format = DepthImageFormat.Resolution80x60Fps30;
var inputs = KinectSensor.KinectSensors.Where(k => k.Status == KinectStatus.Connected).Select(k =>
{
k.Start();
return new
{
sensor = k,
depth = new DepthCamera(k, format),
skeleton = new SkeletonCamera(k),
calibrator = new KinectCalibrator(Utils.DeSerializeObject<CalibrationResult>(k.UniqueKinectId.Substring(k.UniqueKinectId.Length - 16) + ".xml"))
};
});
//inputs.First().calibrator.ToGlobal(inputs.First().sensor, new SkeletonPoint() { X = 1, Y = 1, Z = 1 });
while (true)
{
var points = inputs.Select(inp => new
{
Calibrator = inp.calibrator,
Skeletons = inp.depth.Get(100).Where(p => p.HasValue && p.Value.Index > 0)
.Select(p => inp.sensor.CoordinateMapper.MapDepthPointToSkeletonPoint(format, p.Value.Point))
.Select(sp => {
return inp.calibrator.ToGlobal(inp.sensor, sp);
})
.ToArray()
});
var tps = points.SelectMany(p => p.Skeletons).ToArray();
if (tps.Length <= 0)
continue;
var pcp = pc.Transform(tps);
proj.DrawPoints(pcp, 1);
}
}
static StereoCalibrationResult Calculate(Camera camera, Projector projector)
{
List<string> files = new List<string>();
while (true)
{
Console.WriteLine("Enter file name with pass data: (q) when done");
var str = Console.ReadLine();
if (str == "q")
break;
files.Add(str);
}
var data = files.Select(str => Utils.DeSerializeObject<CalibrationData>(str + ".xml")).ToArray();
var calib = StereoCalibration.Calibrate(data, camera, projector, new Size(7, 4), 0.05f);
projector.DrawBackground(System.Drawing.Color.Black);
var peas = new float[][] {
new float[] { 0f, 0f, 0f },
new float[] { 0.1f, 0f, 0f },
new float[] { 0f, 0.1f, 0f },
new float[] { 0f, 0f, 0.1f },
};
var tpe = calib.TransformG2P(peas);
var tpp = calib.TransformG2C(peas);
var cp = camera.TakePicture(0);
QuickDraw.Start(cp).DrawPoint(tpp, 5).Finish();
StereoCalibration.DebugWindow.DrawBitmap(cp);
projector.DrawPoints(tpe, 5);
Console.WriteLine("Enter command: (d)one, (s)tart again");
var command = Console.ReadLine();
return command == "d" ? calib : null;
}
public static CalibrationData GetLocalCorners(PointF[] cameraCorners, Projector projector, Camera camera, Size pattern)
{
if (false)
{
projector.DrawBackground(Color.Black);
var noi = new Image<Bgr, byte>(camera.TakePicture(5));
projector.DrawBackground(Color.FromArgb(0, 255, 0));
var fulli = new Image<Bgr, byte>(camera.TakePicture(5));
projector.DrawPhaseMod(4, 0f, true, Color.FromArgb(0, 255, 0));
var img = new Image<Bgr, byte>(camera.TakePicture(5));
var max = (fulli - noi).Split()[1];
var cur = (img - noi).Split()[1];
var map = new Bitmap(cur.Width, cur.Height);
DebugWindow.DrawBitmap(max.Bitmap);
DebugWindow.DrawBitmap(cur.Bitmap);
using (var fast = new FastBitmap(map))
{
for (int y = 0; y < cur.Height; y++)
{
for (int x = 0; x < cur.Width; x++)
{
var ii = cur[(int)y, (int)x].Intensity / max[(int)y, (int)x].Intensity;
if (ii > 1)
ii = 1;
var i = (byte)(ii * 255);
fast[x, y] = Color.FromArgb(i, i, i);
}
}
}
if (DebugWindow != null)
DebugWindow.DrawBitmap(map);
}
//Determine rough checkerboard coordinates in projector space with graycode or binary structured light
var rough = DetectRoughCorners(cameraCorners, projector, camera, Color.Green);
//Improve accuracy with PhaseModulation
//var phine = PhineTune(projector, camera, cameraCorners, rough, 32);
//var phine2 = PhineTune(projector, camera, cameraCorners, rough, 64);
//var phine3 = PhineTune(projector, camera, cameraCorners, phine2, 128);
projector.DrawPoints(rough, 5);
//Determine corners in projector space
//var projectorCorners = DetectProjectorCorners(nolight, cameraCorners, projOutline, projector, camera);
//Save corners in camera and projector space and store along side global coordinates that matches current checkerboard
var data = new CalibrationData() { CameraCorners = cameraCorners, ProjectorCorners = rough };
return data;
}
public static CalibrationResult Calibrate(Projector projector, Camera camera, Emgu.CV.Structure.MCvPoint3D32f[] globalCorners, int iterations = 10, int iterationTimeout = 500)
{
//if (PhaseModulation.DetermineAlgorithmIntegrity(8, 10000, 0.01) > 0.15)
// throw new Exception("Phase modulation integrity is failing");
//projector.SetBounds(new RectangleF(0.25f, 0.25f, 0.5f, 0.5f));
//projector.DrawBinary(3, true, Color.White);
//Red and blue checkerboard
//For each orientation of checkerboard:
//float w = projector.bitmap.Width;
//float h = projector.bitmap.Height;
//var ps = new PointF[] {
// new PointF(440f, h - 324f), new PointF(640f, h - 168f)
// };
////projector.SetBounds(DetermineBounds(ps, projector.bitmap.Width, projector.bitmap.Height));
//projector.SetBounds(DetermineBounds(ps, w, h));
//projector.DrawBackground(Color.White);
var datas = new CalibrationData[iterations];
for (int i = 0; i < iterations; i++)
{
//Take pic of checkerboard with no proj light
projector.DrawBackground(Color.Black);
//Detect corners in camera space
PointF[] cameraCorners;
Bitmap withCorners;
do
{
var nolight = camera.TakePicture(5);
withCorners = camera.TakePicture();
cameraCorners = DetectCornersRB(nolight, new Size(7, 4));
} while (cameraCorners == null);
//cameraCorners = cameraCorners.Take(1).Union(cameraCorners.Skip(cameraCorners.Length - 1)).ToArray();
if (DebugWindow != null)
{
QuickDraw.Start(withCorners)
.Color(Color.White)
.DrawPoint(cameraCorners, 5)
.Finish();
DebugWindow.DrawBitmap(withCorners);
}
//Determine rough checkerboard coordinates in projector space with graycode or binary structured light
//var smooth = PhaseCalib(projector, camera, cameraCorners);
//projector.DrawPoints(smooth, 5);
Func<PointF[]> Pass = () =>
{
var rough = DetectRoughCorners(cameraCorners, projector, camera, Color.Green);
//var phine = PhineTune(projector, camera, cameraCorners, rough, 32);
var phine2 = PhineTune(projector, camera, cameraCorners, rough, 64);
var phine3 = PhineTune(projector, camera, cameraCorners, phine2, 128);
return phine3;
};
var ids = Range.OfInts(cameraCorners.Length);
var passes = Range.OfInts(10).ToArray();
var data = passes.Select(p => Pass()).ToArray();
var result = ids.Select(row => passes.Select(p => data[p][row])).ToArray();
var avg = ids.Select(id => new PointF(result[id].Select(r => r.X).Average(),
result[id].Select(r => r.Y).Average())).ToArray();
projector.DrawPoints(avg, 5);
var avg2 = ids.Select(id => new PointF(
result[id].Select(r => new { val= r.X, dist = Math.Abs(avg[id].X - r.X) })
.OrderByDescending(row => row.dist).Reverse().Take(5).Select(row => row.val).Average(),
result[id].Select(r => new { val = r.Y, dist = Math.Abs(avg[id].Y - r.Y) })
.OrderByDescending(row => row.dist).Reverse().Take(5).Select(row => row.val).Average())).ToArray();
projector.DrawPoints(avg2, 5);
//var outline = DetermineBounds(rough, projector.bitmap.Width, projector.bitmap.Height);
//projector.SetBounds(outline);
/*
double[] xs, ys;
{
projector.DrawBackground(Color.Black);
var nolight = camera.TakePicture(5);
projector.DrawBackground(Color.Green);
var fulllight = camera.TakePicture(5);
projector.DrawBackground(Color.Black);
camera.TakePicture(5).Dispose();
projector.Draw(PhaseModulation.DrawPhaseModulation(1, -2 * Math.PI / 3, true, Color.Green));
var light1 = camera.TakePicture(2);
projector.DrawBackground(Color.Black);
camera.TakePicture(5).Dispose();
projector.Draw(PhaseModulation.DrawPhaseModulation(1, 0, true, Color.Green));
var light2 = camera.TakePicture(2);
projector.DrawBackground(Color.Black);
camera.TakePicture(5).Dispose();
projector.Draw(PhaseModulation.DrawPhaseModulation(1, 2 * Math.PI / 3, true, Color.Green));
var light3 = camera.TakePicture(2);
var pc = PhaseClassifier(nolight, fulllight, projector.bitmap.Width, projector.bitmap.Height);
var pc1 = pc(light1);
var pc2 = pc(light2);
var pc3 = pc(light3);
xs = cameraCorners.Select(row => PhaseModulation.IntensityToPhase(pc1((int)row.X, (int)row.Y),
pc2((int)row.X, (int)row.Y),
pc3((int)row.X, (int)row.Y))).ToArray();
}
{
projector.DrawBackground(Color.Black);
var nolight = camera.TakePicture(5);
projector.DrawBackground(Color.Green);
var fulllight = camera.TakePicture(5);
projector.DrawBackground(Color.Black);
camera.TakePicture(5).Dispose();
projector.DrawPhaseMod(1, (float)(-2 * Math.PI / 3), false, Color.FromArgb(0, 255, 0));
var light1 = camera.TakePicture(2);
projector.DrawBackground(Color.Black);
camera.TakePicture(5).Dispose();
projector.DrawPhaseMod(1, (float)(0), false, Color.FromArgb(0, 255, 0));
var light2 = camera.TakePicture(2);
projector.DrawBackground(Color.Black);
camera.TakePicture(5).Dispose();
projector.DrawPhaseMod(1, (float)(2 * Math.PI / 3), false, Color.FromArgb(0, 255, 0));
var light3 = camera.TakePicture(2);
var pc = PhaseClassifier(nolight, fulllight, projector.bitmap.Width, projector.bitmap.Height);
var pc1 = pc(light1);
var pc2 = pc(light2);
var pc3 = pc(light3);
ys = cameraCorners.Select(row => PhaseModulation.IntensityToPhase(pc1((int)row.X, (int)row.Y),
pc2((int)row.X, (int)row.Y),
pc3((int)row.X, (int)row.Y))).ToArray();
}
var resx = xs.Select(row => PhaseModulation.AbsolutePhase(new double[] { row }, new int[] { 1 })).Select(row => row * projector.bitmap.Width).ToArray();
var resy = ys.Select(row => PhaseModulation.AbsolutePhase(new double[] { row }, new int[] { 1 })).Select(row => row * projector.bitmap.Height).ToArray();
var res = resx.Zip(resy, (x, y) => new PointF((float)Math.Round(x, 0), (float)Math.Round(y, 0))).ToArray();
projector.SetBounds(new RectangleF(0, 0, 1, 1));
projector.DrawPoints(res, 5f);
*/
//Determine corners in projector space
//var projectorCorners = DetectProjectorCorners(nolight, cameraCorners, projOutline, projector, camera);
//Save corners in camera and projector space and store along side global coordinates that matches current checkerboard
//var data = new CalibrationData() { CameraCorners = cameraCorners, ProjectorCorners = projectorCorners, GlobalCorners = globalCorners };
datas[i] = default(CalibrationData);
Thread.Sleep(iterationTimeout);
}
return CalibrationResult.Make(datas);
}