public static void Run(string[] args)
{
int[] ra = Range.OfInts(52).ToArray();
string test = "decode.exe options.ini " + string.Join(" ", ra.Select(i => string.Format("picture-{0:000}.bmp", i)).ToArray());
test.ToArray();
string dir = @"C:\Users\ASUS\git\procamtools-v1\Debug";
Thread.Sleep(100);
ExCamera xcam = new ExCamera();
Projector proj = new Projector();
var main = OpenTK.DisplayDevice.AvailableDisplays.First(row => row.IsPrimary);
var display = new BitmapWindow(main.Bounds.Left + 50, 50, 1280, 960);
display.Load();
display.ResizeGraphics();
proj.DrawBackground(Color.White);
int c = 0;
foreach (var file in Directory.GetFiles(dir, "pattern-*.bmp"))
{
var map = new Bitmap(Path.Combine(dir, file));
proj.DrawBitmap(map);
var pic = xcam.TakePicture();
display.DrawBitmap(pic);
var bits = (Bitmap)pic.Clone(new Rectangle(0, 0, pic.Width, pic.Height), PixelFormat.Format32bppRgb);
bits.Save(Path.Combine(dir, string.Format("picture-{0:000}.bmp", c++)), ImageFormat.Bmp);
}
display.Close();
proj.Close();
xcam.Dispose();
}
public static void RunOld(string[] args)
{
var intrinsicfile = args.FirstOrDefault() ?? Calibration.KinectDefaultFileName;
var camIntrinsic = Utils.DeSerializeObject<CalibrationResult>(intrinsicfile);
var projFile = args.Skip(1).FirstOrDefault() ?? Calibration.ProjectorDefaultFileName;
var projIntrinsic = Utils.DeSerializeObject<CalibrationResult>(projFile);
KinectSensor sensor = KinectSensor.KinectSensors.First();
Camera cam = new Camera(sensor, ColorImageFormat.RgbResolution1280x960Fps12);
Projector proj = new Projector();
var keyl = new KeyboardListener(proj.window.Keyboard);
double offsetx = 0, offsety = 0, scale = 0.5;
bool proceed = false, quit = false;
keyl.AddBinaryAction(0.02, -0.02, OpenTK.Input.Key.Up, OpenTK.Input.Key.Down, new OpenTK.Input.Key[0], (f) => offsety += f);
keyl.AddBinaryAction(0.02, -0.02, OpenTK.Input.Key.Left, OpenTK.Input.Key.Right, new OpenTK.Input.Key[0], (f) => offsetx -= f);
keyl.AddBinaryAction(0.02, -0.02, OpenTK.Input.Key.Up, OpenTK.Input.Key.Down, new OpenTK.Input.Key[] { Key.ShiftLeft }, (f) => scale += f);
keyl.AddAction(() => proceed = true, Key.Space);
keyl.AddAction(() => quit = proceed = true, Key.Q);
PointF[] corners;
proj.DrawBackground(Color.Black);
while (true)
{
Console.WriteLine("Make sure the kinect can see the board");
Console.ReadLine();
corners = StereoCalibration.GetCameraCorners(cam.TakePicture(3), new Size(7, 4), false);
if (corners.All(c => c != null))
{
break;
}
else
Console.WriteLine("Could not find corners");
}
PointF[] projCorners;
var projectedCorners = proj.DrawCheckerboard(new Size(8, 5), 0, 0, 0, scale, offsetx, offsety);
while (true)
{
Console.WriteLine("Make sure the kinect can see the projection");
while (!proceed)
{
projectedCorners = proj.DrawCheckerboard(new Size(8, 5), 0, 0, 0, scale, offsetx, offsety);
proj.window.ProcessEvents();
}
projCorners = StereoCalibration.GetCameraCorners(cam.TakePicture(3), new Size(7, 4), false);
if (corners.All(c => c != null))
{
break;
}
else
Console.WriteLine("Could not find any corners, make sure the checkerboard is visible to all Kinects.");
}
var camResult = StereoCalibration.CalibrateCamera(corners, new Size(7, 4), 0.05f, camIntrinsic);
var transform = StereoCalibration.FindHomography(projCorners, projectedCorners);
var projResult = StereoCalibration.CalibrateCamera(transform(corners), new Size(7, 4), 0.05f, projIntrinsic);
Utils.SerializeObject(camResult, intrinsicfile);
Utils.SerializeObject(projResult, projFile);
proj.Close();
}
static CalibrationResult CalibrateCamera(Camera camera, Projector projector, out PointF[][] data, bool reloadData = false, bool reloadCalc = true, bool save = true)
{
string datafile = "cameradata.xml";
string calcfile = "cameracalibration.xml";
Size pattern = new Size(7,4);
projector.DrawBackground(Color.Black);
var list = new List<PointF[]>();
if (!reloadData && File.Exists(datafile))
list = Utils.DeSerializeObject<List<PointF[]>>(datafile);
else
{
while (true)
{
Console.WriteLine("Place checkerboard at the origo position and press enter.");
Console.ReadLine();
var cc = StereoCalibration.GetCameraCorners(projector, camera, new Size(7, 4), false);
if (cc != null)
{
list.Add(cc);
break;
}
else
Console.WriteLine("Could not find any corners, make sure the checkerboard is visible to the camera.");
}
Console.Write("First image OK. Enter number of required passes: ");
var str = Console.ReadLine();
int passes;
if (!int.TryParse(str, out passes))
passes = 15;
ConcurrentQueue<Bitmap> pics = new ConcurrentQueue<Bitmap>();
ConcurrentQueue<PointF[]> corners = new ConcurrentQueue<PointF[]>();
for (int i = 255; i >= 0; i--)
{
projector.DrawBackground(Color.FromArgb(i, i, i));
}
var cornerTask = Task.Run(() =>
{
Console.Write("Progress: ");
while (corners.Count < passes)
{
Bitmap img;
if (pics.TryDequeue(out img))
{
var cc = StereoCalibration.GetCameraCorners(img, pattern);
if (cc != null)
{
corners.Enqueue(cc);
Console.Write("=");
}
img.Dispose();
}
Thread.Yield();
}
Console.Write("> Done!\n");
});
while (corners.Count < passes)
{
if (pics.Count < passes * 2)
{
Thread.Sleep(200);
projector.DrawBackground(Color.Black);
var pic = camera.TakePicture();
projector.DrawBackground(Color.White);
Thread.Sleep(50);
projector.DrawBackground(Color.Black);
pics.Enqueue(pic);
}
else
{
while (pics.Count > 4 && corners.Count < passes)
{
Thread.Sleep(1000);
Thread.Yield();
}
if (corners.Count < passes)
{
for (int i = 255; i >= 0; i--)
{
projector.DrawBackground(Color.FromArgb(i, i, i));
}
}
}
Thread.Yield();
}
cornerTask.Wait();
foreach (var cc in corners)
list.Add(cc);
if (save)
Utils.SerializeObject(list, datafile);
foreach (var img in pics)
img.Dispose();
//while (true)
//{
// projector.DrawBackground(Color.Black);
// Thread.Sleep(120);
// var cc = StereoCalibration.GetCameraCorners(projector, camera, new Size(7, 4), false);
// if (cc != null)
// {
// list.Add(cc);
// projector.DrawBackground(Color.Green);
// Thread.Sleep(300);
// }
// else
// {
// projector.DrawBackground(Color.Red);
// Thread.Sleep(300);
// }
// if (list.Count > 25)
// {
// if (save)
// SerializeObject(list, datafile);
// break;
// }
//}
Console.WriteLine("Data gather done. Press enter to calculate calibration.");
Console.ReadLine();
}
CalibrationResult calib;
if (!reloadCalc && File.Exists(calcfile))
calib = Utils.DeSerializeObject<CalibrationResult>(calcfile);
else
{
calib = StereoCalibration.CalibrateCamera(list.ToArray()
, camera, new Size(7, 4), 0.05f);
if (save)
Utils.SerializeObject(calib, calcfile);
}
data = list.ToArray();
return calib;
}
public static PointF[] PhineTune(Projector projector, Camera camera, PointF[] cameraCorners, PointF[] rough, int subdiv)
{
var color = Color.Green;
projector.DrawBackground(Color.Black);
var nolight = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
projector.DrawBackground(color);
var fulllight = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
Func<int, bool, Image<Gray, byte>[]> takePics = (step, vertical) =>
{
Image<Gray, byte>[] pics = new Image<Gray, byte>[3];
projector.DrawBackground(Color.Black);
camera.TakePicture(5).Dispose();
projector.DrawPhaseMod(step, (float)(-2.0f * Math.PI / 3.0f), vertical, color);
pics[0] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
projector.DrawBackground(Color.Black);
camera.TakePicture(5).Dispose();
projector.DrawPhaseMod(step, 0f, vertical, color);
pics[1] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
projector.DrawBackground(Color.Black);
camera.TakePicture(5).Dispose();
projector.DrawPhaseMod(step, (float)(2.0f * Math.PI / 3.0f), vertical, color);
pics[2] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
return pics;
};
Func<Image<Gray, byte>[], double[]> itop = (pics) =>
{
return cameraCorners.Select(c =>
{
var mini = nolight[(int)c.Y, (int)c.X].Intensity;
var maxi = fulllight[(int)c.Y, (int)c.X].Intensity;
var i1 = pics[0][(int)c.Y, (int)c.X].Intensity;
i1 = (i1 - mini) / (maxi - mini);
var i2 = pics[1][(int)c.Y, (int)c.X].Intensity;
i2 = (i2 - mini) / (maxi - mini);
var i3 = pics[2][(int)c.Y, (int)c.X].Intensity;
i3 = (i3 - mini) / (maxi - mini);
return PhaseModulation.IntensityToPhase(i1, i2, i3);
}).ToArray();
};
int w = projector.Size.Width;
int h = projector.Size.Height;
var xphs = itop(takePics(subdiv, true));
var yphs = itop(takePics(subdiv, false));
var ids = new int[cameraCorners.Length];
int idx = 0;
ids = ids.Select(i => idx++).ToArray();
return ids.Select(i =>
{
var v = rough[i];
var denom = (double)subdiv;
var xph = xphs[i] * (w / denom);
var yph = yphs[i] * (h / denom);
var phsx = Math.Floor(v.X / denom) * denom;
var phsy = Math.Floor(v.Y / denom) * denom;
double apx = v.X / w;
double phx = xphs[i];
apx = Math.Floor(apx / (1.0 / denom)) * (1.0 / denom) + (phx < 1 ? phx / denom : 0);
apx = Math.Round(apx, 5) * w;
double apy = v.Y / h;
double phy = yphs[i];
apy = Math.Floor(apy / (1.0 / denom)) * (1.0 / denom) + (phy < 1 ? phy / denom : 0);
apy = Math.Round(apy, 5) * h;
return new PointF((float)(apx), (float)(apy));
}).ToArray();
}
public static void Test(Projector projector, Camera camera)
{
if (DebugWindow != null)
for (var i = 1; i < 0; i++)
{
projector.DrawBackground(Color.Black);
var nolight = camera.TakePicture(10);
projector.DrawBinary(i, false, Color.Green);
var fulllight = camera.TakePicture(3);
var n = new Image<Bgr, byte>(nolight);
var f = new Image<Bgr, byte>(fulllight);
Point p = new Point();
var d = (f - n).Split()[1];
DebugWindow.DrawBitmap(d.Bitmap);
double[] min, max;
Point[] minp, maxp;
d.MinMax(out min, out max, out minp, out maxp);
var thresh = (max.Max() - min.Min()) * 0.08 + min.Min();
d = d.ThresholdBinary(new Gray(thresh), new Gray(255)).Erode(2).Dilate(3).Erode(1);
DebugWindow.DrawBitmap(d.Bitmap);
}
}
public static PointF[] DetectRoughCorners(PointF[] cameraCorners, Projector projector, Camera camera, Color fullColor)
{
projector.DrawBackground(Color.Black);
var nolight = camera.TakePicture(10);
var projected = BinarySL(projector, camera, cameraCorners, nolight, fullColor, false)
.Zip(BinarySL(projector, camera, cameraCorners, nolight, fullColor, true), (y,x) => new PointF((float)x, (float)y))
.ToArray();
return projected;
}
public static PointF[] PhaseMod(Projector projector, Camera camera, PointF[] cameraCorners)
{
var color = Color.FromArgb(0, 255, 0);
projector.DrawBackground(Color.Black);
var nolight = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
projector.DrawBackground(color);
var fulllight = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
Func<int, bool, Image<Gray, byte>[]> takePics = (step, vertical) =>
{
Image<Gray, byte>[] pics = new Image<Gray, byte>[3];
projector.DrawBackground(Color.Black);
camera.TakePicture(5).Dispose();
projector.DrawPhaseMod(step, (float)(-2.0f * Math.PI / 3.0f), vertical, color);
pics[0] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
projector.DrawBackground(Color.Black);
camera.TakePicture(5).Dispose();
projector.DrawPhaseMod(step, 0f, vertical, color);
pics[1] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
projector.DrawBackground(Color.Black);
camera.TakePicture(5).Dispose();
projector.DrawPhaseMod(step, (float)(2.0f * Math.PI / 3.0f), vertical, color);
pics[2] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
return pics;
};
Func<Image<Gray, byte>[], PointF[], double[]> itop = (pics, corners) =>
{
return corners.Select(c =>
{
var mini = nolight[(int)c.Y, (int)c.X].Intensity;
var maxi = fulllight[(int)c.Y, (int)c.X].Intensity;
var i1 = pics[0][(int)c.Y, (int)c.X].Intensity;
i1 = (i1 - mini) / (i1 - maxi);
var i2 = pics[1][(int)c.Y, (int)c.X].Intensity;
i2 = (i2 - mini) / (i2 - maxi);
var i3 = pics[2][(int)c.Y, (int)c.X].Intensity;
i3 = (i3 - mini) / (i3 - maxi);
return PhaseModulation.IntensityToPhase(i1, i2, i3);
}).ToArray();
};
var ids = new int[cameraCorners.Length];
int idx = 0;
ids = ids.Select(i => idx++).ToArray();
int steps = 7;
int[] stepa = new int[steps];
idx = 1;
stepa = stepa.Select(i => idx++).ToArray();
int[] stepx = stepa.Select(row => (int)Math.Pow(2, row - 1)).ToArray();
double[][] verticals = new double[steps][];
for (var i = 1; i <= steps; i++)
{
var pics = takePics(stepx[i - 1], true);
verticals[i - 1] = itop(pics, cameraCorners);
}
var apv = ids.Select(i => PhaseModulation.AbsolutePhase(
stepa.Select(s => verticals[s - 1][i]).ToArray(), stepa))
.Select(ph => ph * projector.Size.Width)
.ToArray();
double[][] horizontals = new double[steps][];
for (var i = 1; i <= steps; i++)
{
var pics = takePics(stepx[i - 1], false);
horizontals[i - 1] = itop(pics, cameraCorners);
}
var aph = ids.Select(i => PhaseModulation.AbsolutePhase(
stepa.Select(s => horizontals[s - 1][i]).ToArray(), stepx))
.Select(ph => ph * projector.Size.Height)
.ToArray();
return ids.Select(i => new PointF((float)apv[i], (float)aph[i])).ToArray();
}
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 double[] GreySL(Projector projector, Camera camera, PointF[] corners, Bitmap nolight, Color fullColor, bool vertical, int offset)
{
uint[] horizontal = new uint[corners.Length];
int max = (int)Math.Floor(Math.Log((vertical ? projector.bitmap.Width : projector.bitmap.Height), 2)) + 1;
int subdivisions = 1;
var nol = Classifier(nolight);
for (var step = 0; step < max - 4; step++)
{
projector.DrawBackground(Color.Black);
camera.TakePicture(2).Dispose();
projector.DrawGrey(step, vertical, offset, fullColor);
var light = camera.TakePicture(2);
var classifier = nol(light, step);
int idx = 0;
Bitmap withCorners = null;
foreach (var point in corners)
{
var hit = classifier(point);
var h = horizontal[idx];
h = h << 1;
h = h | (hit ? (uint)1 : (uint)0);
horizontal[idx] = h;
idx++;
if (DebugWindow != null)
{
withCorners = light;
QuickDraw.Start(withCorners)
.Color(hit ? Color.Gray : Color.White)
.DrawPoint(point.X, point.Y, 5)
.Finish();
}
}
if (DebugWindow != null)
DebugWindow.DrawBitmap(withCorners);
light.Dispose();
subdivisions++;
}
var result = horizontal
.Select(h =>
{
uint num = h;
uint mask;
for (mask = num >> 1; mask != 0; mask = mask >> 1)
{
num = num ^ mask;
}
return num;
})
.Select(row => (1 - (double)row / Math.Pow(2, max - 4)) * (vertical ? projector.bitmap.Width : projector.bitmap.Height)).ToArray();
return result;
}
public static PointF[] DetectRoughCorners(PointF[] cameraCorners, Projector projector, Camera camera, Color fullColor)
{
projector.DrawBackground(Color.Black);
var nolight = camera.TakePicture(10);
var ids = new int[cameraCorners.Length];
for (var i = 0; i < cameraCorners.Length; i++) ids[i] = i;
var points = new List<MathNet.Numerics.LinearAlgebra.Double.DenseVector[]>();
for (int i = 0; i < 18; i += 6)
{
var projected = GreySL(projector, camera, cameraCorners, nolight, fullColor, false, i)
.Zip(GreySL(projector, camera, cameraCorners, nolight, fullColor, true, i),
(y, x) => new MathNet.Numerics.LinearAlgebra.Double.DenseVector(new double[] { x - i, y - i }))
.ToArray();
points.Add(projected);
}
return ids.Select(i => points.Select(row => row[i])
.Aggregate((v1, v2) => v1 + v2) / points.Count)
.Select(r => new PointF((float)r[0], (float)r[1]))
.ToArray();
}
public static PointF[] GetCameraCorners(Projector projector, Camera camera, Size pattern, bool RB = true)
{
//Take pic of checkerboard with no proj light
projector.DrawBackground(Color.Gray);
//Detect corners in camera space
PointF[] cameraCorners;
Bitmap withCorners;
withCorners = camera.TakePicture();
var img = camera.TakePicture(2);
cameraCorners = GetCameraCorners(img, pattern, RB);
if (cameraCorners == null)
return null;
if (DebugWindow != null)
{
QuickDraw.Start(withCorners)
.Color(Color.White)
.DrawPoint(cameraCorners, 5)
.Finish();
DebugWindow.DrawBitmap(withCorners);
}
return cameraCorners;
}
public static double[] BinarySL(Projector projector, Camera camera, PointF[] corners, Bitmap nolight, Color fullColor, bool vertical)
{
int[] horizontal = new int[corners.Length];
int max = (int)Math.Floor(Math.Log((vertical ? projector.bitmap.Width : projector.bitmap.Height), 2)) + 1;
int subdivisions = 1;
var nol = Classifier(nolight);
for (var step = 1; step <= max - 4; step++)
{
projector.DrawBackground(Color.Black);
camera.TakePicture(5).Dispose();
projector.DrawBinary(step, vertical, fullColor);
var light = camera.TakePicture(2);
var classifier = nol(light, step);
int idx = 0;
Bitmap withCorners = null;
foreach (var point in corners)
{
var hit = classifier(point);
var h = horizontal[idx];
h = h << 1;
h = h | (hit ? 1 : 0);
horizontal[idx] = h;
idx++;
if (DebugWindow != null)
{
withCorners = light;
QuickDraw.Start(withCorners)
.Color(hit ? Color.Gray : Color.White)
.DrawPoint(point.X, point.Y, 5)
.Finish();
}
}
if (DebugWindow != null)
DebugWindow.DrawBitmap(withCorners);
light.Dispose();
subdivisions++;
}
var result = horizontal.Select(row => ((double)row / Math.Pow(2, max - 4)) * (vertical ? projector.bitmap.Width : projector.bitmap.Height)).ToArray();
using (var bitmap = new Bitmap(projector.bitmap.Width, projector.bitmap.Height))
{
using (var fast = new FastBitmap(bitmap))
{
for (var x = 0; x < bitmap.Width; x++)
for (var y = 0; y < bitmap.Height; y++)
if (result.Contains(vertical ? x : y))
fast[x, y] = Color.FromArgb(255, 255, 255, 255);
else
fast[x, y] = Color.FromArgb(255, 0, 0, 0);
}
projector.DrawBitmap(bitmap);
}
return result;
}
public static void DrawNoFull(Projector proj, Camera camera, out Bitmap result)
{
var range = new int[10];
proj.DrawBackground(Color.Black);
var nolight = range.Select(r => new Image<Gray, byte>(camera.TakePicture())).Last();
proj.DrawBackground(Color.Black);
var fulllight = range.Select(r => new Image<Gray, byte>(camera.TakePicture())).Last();
result = (fulllight - nolight).Bitmap;
}
public static bool DrawCorners(Projector projector, Camera camera, out Bitmap nolight)
{
projector.DrawBackground(Color.Black);
PointF[] cameraCorners;
do
{
nolight = camera.TakePicture();
//cameraCorners = DetectCorners(nolight, new Size(7, 4));
cameraCorners = Emgu.CV.CameraCalibration.FindChessboardCorners(new Image<Gray, byte>(nolight), new Size(7, 4), Emgu.CV.CvEnum.CALIB_CB_TYPE.ADAPTIVE_THRESH);
} while (cameraCorners == null);
//var projCorners = DetectRoughCorners(cameraCorners, projector, camera);
//projector.DrawPoints(projCorners, 10);
return true;
}
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[] PhaseCalib(Projector projector, Camera camera, PointF[] cameraCorners, int steps = 7)
{
var color = Color.Green;
projector.DrawBackground(Color.Black);
var nolight = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
projector.DrawBackground(color);
var fulllight = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
Func<int, bool, Image<Gray, byte>[]> takePics = (step, vertical) =>
{
Image<Gray, byte>[] pics = new Image<Gray, byte>[3];
projector.DrawBackground(Color.Black);
camera.TakePicture(5).Dispose();
projector.DrawPhaseMod(step, (float)(-2.0f * Math.PI / 3.0f), vertical, color);
pics[0] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
projector.DrawBackground(Color.Black);
camera.TakePicture(5).Dispose();
projector.DrawPhaseMod(step, 0f, vertical, color);
pics[1] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
projector.DrawBackground(Color.Black);
camera.TakePicture(5).Dispose();
projector.DrawPhaseMod(step, (float)(2.0f * Math.PI / 3.0f), vertical, color);
pics[2] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1];
return pics;
};
Func<Image<Gray, byte>[], double[]> itop = (pics) =>
{
return cameraCorners.Select(c =>
{
var mini = nolight[(int)c.Y, (int)c.X].Intensity;
var maxi = fulllight[(int)c.Y, (int)c.X].Intensity;
var i1 = pics[0][(int)c.Y, (int)c.X].Intensity;
i1 = (i1 - mini) / (i1 - maxi);
var i2 = pics[1][(int)c.Y, (int)c.X].Intensity;
i2 = (i2 - mini) / (i2 - maxi);
var i3 = pics[2][(int)c.Y, (int)c.X].Intensity;
i3 = (i3 - mini) / (i3 - maxi);
return PhaseModulation.IntensityToPhase(i1, i2, i3);
}).ToArray();
};
int w = projector.Size.Width;
int h = projector.Size.Height;
int subdiv = 1;
int[] xs = new int[cameraCorners.Length];
int[] ys = new int[cameraCorners.Length];
int[] ids = Range.OfInts(cameraCorners.Length).ToArray();
for (int i = 0; i < steps; i++)
{
var xpics = takePics(subdiv, true);
var xphs = itop(xpics);
var xh = ids.Select(id => xphs[id] > 0.5).ToArray();
var qd = QuickDraw.Start(xpics[1].Bitmap);
var thrash = ids.Select(id =>
{
qd.Color(xh[id] ? Color.White : Color.Gray);
qd.DrawPoint(cameraCorners[id].X, cameraCorners[id].Y, 5);
return id;
}).ToArray();
qd.Finish();
DebugWindow.DrawBitmap(xpics[1].Bitmap);
var ypics = takePics(subdiv, false);
var yphs = itop(ypics);
var yh = ids.Select(id => yphs[id] > 0.5).ToArray();
qd = QuickDraw.Start(ypics[1].Bitmap);
thrash = ids.Select(id =>
{
qd.Color(yh[id] ? Color.White : Color.Gray);
qd.DrawPoint(cameraCorners[id].X, cameraCorners[id].Y, 5);
return id;
}).ToArray();
qd.Finish();
DebugWindow.DrawBitmap(ypics[1].Bitmap);
xs = ids.Select(id => (xs[id] << 1) | (xh[id] ? 1 : 0)).ToArray();
ys = ids.Select(id => (ys[id] << 1) | (yh[id] ? 1 : 0)).ToArray();
subdiv = subdiv << 1;
}
var fxs = ids.Select(id => ((double)xs[id] / (double)subdiv) * w).ToArray();
var fys = ids.Select(id => ((double)ys[id] / (double)subdiv) * h).ToArray();
return fxs.Zip(fys, (x,y) => new PointF((float)x,(float)y)).ToArray();
}
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 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);
}