public static void Run(string[] args)
{
var intrinsicfile = args.FirstOrDefault() ?? Calibration.KinectDefaultFileName;
var intrinsic = Utils.DeSerializeObject<CalibrationResult>(intrinsicfile);
KinectSensor sensor = KinectSensor.KinectSensors.First();
var kinects = KinectSensor.KinectSensors.Where(s => s.Status == KinectStatus.Connected).ToArray();
var cameras = kinects.Select(s =>
{
s.Start();
return new Camera(s, ColorImageFormat.RgbResolution1280x960Fps12);
}).ToArray();
Projector projector = new Projector();
PointF[][] corners;
while (true)
{
Console.WriteLine("Place checkerboard at the origo position, make sure it is visible to all connected Kinects, and press enter.");
Console.ReadLine();
corners = cameras.Select(camera => StereoCalibration.GetCameraCorners(projector, camera, new Size(10, 7), false)).ToArray();
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 results = cameras.Zip(corners, (camera, cs) => StereoCalibration.CalibrateCamera(cs, new Size(10,7), 0.1f, intrinsic)).ToArray();
kinects.Zip(results, (kinect, result) =>
{
Utils.SerializeObject(result, kinect.UniqueKinectId.Substring(kinect.UniqueKinectId.Length - 16) + ".xml");
return true;
}).ToArray();
}
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();
}
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 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);
}
}
public Form1()
{
InitializeComponent();
var main = 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;
Projector proj = new Projector();
Camera camera = new Camera(KinectSensor.KinectSensors.First(row => row.Status == KinectStatus.Connected), ColorImageFormat.RgbResolution1280x960Fps12);
StereoCalibration.Test(proj, camera);
//proj.Renderer.RenderBitmap(bitm);
//BitmapWindow window = BitmapWindow.Make();
//window.RenderFrame();
//Projector proj = new Projector();
//var main = DisplayDevice.AvailableDisplays.First(row => row.IsPrimary);
//var window = new BitmapWindow(main.Bounds.Left + main.Width / 2 + 50, 50, 640, 480);
//window.Load();
//window.ResizeGraphics();
//DualCalibrator.DebugWindow = window;
////proj.DrawPoints((new float[] { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 }).Select(row => new PointF(row * 10, row * 10)).ToArray(), 10);
////proj.DrawPoints(new PointF[] { new PointF(50f, 50f) }, 10);
//Camera camera = new Camera(KinectSensor.KinectSensors.First(row => row.Status == KinectStatus.Connected), ColorImageFormat.RgbResolution640x480Fps30);
//bool res = false;
//Application.Idle += (o, e) =>
//{
// if (res)
// return;
// Bitmap map;
// res = DualCalibrator.DrawCorners(proj, camera, out map);
// //DualCalibrator.DrawNoFull(proj, camera, out map);
// //res = true;
// pictureBox1.Image = map;
//};
}
public static StereoCalibrationResult Calibrate(CalibrationData[] data, Camera camera, Projector projector, Size pattern, float checkerBoardSize)
{
var globals = GenerateCheckerBoard(pattern, checkerBoardSize);
var datas = data;
var globalCorners = datas.Select(row => globals).ToArray();
var cameraCorners = datas.Select(row => Properize(row.CameraCorners, pattern)
)
.ToArray();
var projectorCorners = datas.Select(row => Properize(row.ProjectorCorners, pattern)
).ToArray();
//for (var i = 0; i < datas.Length; i++)
//{
// var withCorners = camera.TakePicture(0);
// QuickDraw.Start(withCorners)
// .Color(Color.White)
// .DrawPoint(cameraCorners[i].Take(11).ToArray(), 5)
// .Finish();
// DebugWindow.DrawBitmap(withCorners);
// projector.DrawPoints(projectorCorners[i].Take(11).ToArray(), 4);
//}
IntrinsicCameraParameters cameraIntrinsics = new IntrinsicCameraParameters();
cameraIntrinsics.IntrinsicMatrix = new Matrix<double>(new double[,]
{
{ 4.884, 0, 0.032 },
{ 0, 4.884 * 3.0 / 4.0, -0.037 },
{ 0, 0, 1 } });
cameraIntrinsics.DistortionCoeffs = new Matrix<double>(new double[,] {
{-0.00575},
{0.000442},
{-0.000107},
{0},
{0},
{0},
{0},
{0},
});
ExtrinsicCameraParameters[] cameraExtrinsicsArray;
var cerr = Emgu.CV.CameraCalibration.CalibrateCamera(globalCorners, cameraCorners,
camera.Size, cameraIntrinsics, Emgu.CV.CvEnum.CALIB_TYPE.CV_CALIB_USE_INTRINSIC_GUESS,
out cameraExtrinsicsArray);
IntrinsicCameraParameters projectorIntrinsics = new IntrinsicCameraParameters();
//projectorIntrinsics.IntrinsicMatrix = new Matrix<double>(new double[,] { { 531.15f * 4f / 3f, 0, 1 }, { 0, 531.15f, 1 }, { 0, 0, 1 } });
projectorIntrinsics.IntrinsicMatrix = new Matrix<double>(new double[,]
{
{2151.0712684548571, 0, projector.Size.Width / 2},
{0, 1974.541465816948, projector.Size.Height / 2},
{0,0,1}
});
ExtrinsicCameraParameters[] projectorExtrinsicsArray;
var perr = Emgu.CV.CameraCalibration.CalibrateCamera(globalCorners, projectorCorners,
projector.Size, projectorIntrinsics,
Emgu.CV.CvEnum.CALIB_TYPE.CV_CALIB_RATIONAL_MODEL,
out projectorExtrinsicsArray);
//Matrix<double> foundamental, essential;
//ExtrinsicCameraParameters camera2projectorExtrinsics;
//Emgu.CV.CameraCalibration.StereoCalibrate(globalCorners,
// cameraCorners,
// projectorCorners,
// cameraIntrinsics,
// projectorIntrinsics,
// camera.Size,
// Emgu.CV.CvEnum.CALIB_TYPE.DEFAULT,
// new MCvTermCriteria(16, 0.01),
// out camera2projectorExtrinsics,
// out foundamental,
// out essential);
return new StereoCalibrationResult()
{
cameraIntrinsic = cameraIntrinsics,
cameraExtrinsic = cameraExtrinsicsArray.First(),
projectorIntrinsic = projectorIntrinsics,
projectorExtrinsic = projectorExtrinsicsArray.First(),
cameraToProjectorExtrinsic = null
};
}
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 void Run(string[] args)
{
//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();
//var window2 = new BitmapWindow(main.Bounds.Left + 50, 50, 640, 480);
//window2.Load();
//window2.ResizeGraphics();
CalibrationResult kinectcalib = null;
if (args.Length > 0)
{
var file = args.First();
kinectcalib = Utils.DeSerializeObject<CalibrationResult>(file);
}
Console.Write("Enter the folders you'd like to use: ");
var folders = Console.ReadLine().Split(' ').ToArray();
var maps = folders.SelectMany(f => PictureGrabber.GetBitmaps(f)).ToArray();
Size pattern = new Size(10, 7);
Size ppattern = new Size(10, 8);
float chsize = 0.1f;
var kcorners = maps.Select(ms => StereoCalibration.GetCameraCorners(ms.Camera, pattern)).ToArray();
var pcorners = maps.Select(ms => StereoCalibration.GetCameraCorners(ms.Projector, ppattern)).ToArray();
kcorners = kcorners.Zip(pcorners, (a, b) => a != null && b != null ? a : null).Where(a => a != null).ToArray();
pcorners = kcorners.Zip(pcorners, (a, b) => a != null && b != null ? b : null).Where(a => a != null).ToArray();
if (kcorners.Count() != pcorners.Count())
Console.WriteLine("Number of good shots did not match.");
if (kinectcalib == null)
{
if (kcorners.Length == 0)
{
Console.WriteLine("Could not find camera corners");
return;
}
kinectcalib = StereoCalibration.CalibrateCamera(kcorners, maps.First().Camera.Size, pattern, chsize);
}
var tkcorners = kcorners.Select(points => StereoCalibration.Undistort(kinectcalib, points)).ToArray();
var tpcorners = pcorners.Select(points => StereoCalibration.Undistort(kinectcalib, points)).ToArray();
var hgraphs = tpcorners.Zip(maps.Select(m => m.ProjCorners), (c, p) => StereoCalibration.FindHomography(c, p));
var ptkcorners = tkcorners.Zip(hgraphs, (ps, hg) => hg(ps)).ToArray();
//bool proceed = false;
//window.Keyboard.KeyDown += (o, e) =>
//{
// proceed = true;
//};
//window2.Keyboard.KeyDown += (o, e) =>
//{
// proceed = true;
//};
//for (int i = 0; i < maps.Length; i++)
//{
// Projector proj = new Projector();
// var map1 = (Bitmap)maps[i].Camera.Clone();
// QuickDraw.Start(map1).DrawPoint(kcorners[i], 5).Finish();
// var map2 = (Bitmap)maps[i].Projector.Clone();
// QuickDraw.Start(map2).DrawPoint(pcorners[i], 5).Finish();
// window.DrawBitmap(map1);
// window2.DrawBitmap(map2);
// proj.DrawPoints(ptkcorners[i], 5f);
// while (!proceed)
// {
// window.ProcessEvents();
// window2.ProcessEvents();
// }
// proceed = false;
//}
Projector proj = new Projector();
var projcalib = StereoCalibration.CalibrateCamera(ptkcorners, proj.Size, pattern, chsize);
proj.Close();
Console.WriteLine("Save result?");
Console.ReadLine();
Utils.SerializeObject(kinectcalib, Calibration.KinectDefaultFileName);
Utils.SerializeObject(projcalib, Calibration.ProjectorDefaultFileName);
}
//Manual pickings
public static void RunManual(string[] args)
{
//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();
//var window2 = new BitmapWindow(main.Bounds.Left + 50, 50, 640, 480);
//window2.Load();
//window2.ResizeGraphics();
CalibrationResult kinectcalib = null;
if (args.Length > 0)
{
var file = args.First();
kinectcalib = Utils.DeSerializeObject<CalibrationResult>(file);
}
Console.Write("Enter the folders you'd like to use: ");
var folders = Console.ReadLine().Split(' ').ToArray();
var maps = folders.SelectMany(f => PictureGrabber.GetManualBitmaps(f)).ToArray();
Size pattern = new Size(7, 4);
float chsize = 0.05f;
var kcorners = maps.Select(ms => StereoCalibration.GetCameraCorners(ms.Camera, pattern)).ToArray();
var pcorners = maps.Select(ms => ms.ProjCorners).ToArray();
if (kinectcalib == null)
{
if (kcorners.Length == 0)
{
Console.WriteLine("Could not find camera corners");
return;
}
kinectcalib = StereoCalibration.CalibrateCamera(kcorners, maps.First().Camera.Size, pattern, chsize);
}
Projector proj = new Projector();
var projcalib = StereoCalibration.CalibrateCamera(pcorners, proj.Size, pattern, chsize);
proj.Close();
Console.WriteLine("Save result?");
Console.ReadLine();
Utils.SerializeObject(kinectcalib, Calibration.KinectDefaultFileName);
Utils.SerializeObject(projcalib, Calibration.ProjectorDefaultFileName);
}
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 CalibrationResult CalibrateProjector(Projector projector, PointF[][] cacalibdata, PointF[][] cameraCorners, PointF[][] projectorCorners, Size cameraPattern, float checkerboardSize)
{
var hm = CreateHomography(cameraCorners, projectorCorners);
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_ZERO_TANGENT_DIST | CALIB_TYPE.CV_CALIB_FIX_K3, out projExtrins);
return new CalibrationResult() { Intrinsic = projIntrin, Extrinsic = projExtrins.First() };
}
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;
}
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 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;
}
static StereoCalibrationResult Calibrator(Camera camera, Projector projector)
{
while (true)
{
Console.WriteLine("Enter command: take (p)ics, (c)alculate results");
var command = Console.ReadLine();
if (command == "p")
TakePics(camera, projector);
else if (command == "c")
{
var result = Calculate(camera, projector);
if (result != null)
return result;
}
}
}
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 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 CalibrationData[] GatherData(Projector projector, Camera camera, Size pattern, int passes = 1, Func<int, bool> perPass = null)
{
var datas = new List<CalibrationData>();
PointF[] corners;
for (int i = 0; i < passes; i+= 0)
{
corners = GetCameraCorners(projector, camera, pattern);
var data = GetLocalCorners(corners, projector, camera, pattern);
if (perPass != null && perPass(i))
{
datas.Add(data);
i++;
}
}
return datas.ToArray();
}
static StereoCalibrationResult Calibrate(Camera camera, Projector projector
, bool reload = true, bool reloadData = true)
{
string filename = "calibrationresult.xml";
string datafile = "calibrationdata10p.xml";
while (true)
{
if (File.Exists(filename) && !reload)
return Utils.DeSerializeObject<StereoCalibrationResult>(filename);
CalibrationData[] data;
if (File.Exists(datafile) && !reloadData)
data = Utils.DeSerializeObject<CalibrationData[]>(datafile);
else
{
data = StereoCalibration.GatherData(projector, camera, new Size(7, 4), 10,
(pass) =>
{
Console.WriteLine("Pass: "******" done. Reject or Approve? (r/a).");
return Console.ReadLine() != "r";
});
Utils.SerializeObject(data, datafile);
}
var result = StereoCalibration.Calibrate(data, camera, projector, new Size(7, 4), 0.05f);
Utils.SerializeObject(result, filename);
return result;
Console.WriteLine("Proceed? (y/n)");
var command = Console.ReadLine();
if (command == "y")
return result;
}
}
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 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 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[] 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();
}
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() };
}
static void TakePics(Camera camera, Projector projector)
{
while (true)
{
var data = StereoCalibration.GatherData(projector, camera, new Size(7, 4), 1,
(pass) =>
{
Console.WriteLine("Reject or Approve? (r/a).");
return Console.ReadLine() != "r";
}).First();
Console.WriteLine("Enter filename: ");
var name = Console.ReadLine();
var refer = camera.TakePicture(0);
refer.Save(name + ".bmp");
Utils.SerializeObject(data, name + ".xml");
Console.WriteLine("Enter command: (n)ew pic, (r)eturn");
var command = Console.ReadLine();
if (command == "r") return;
}
}
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 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 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;
}
