public static void RunOld(string[] args)
{
var camfile = args.FirstOrDefault() ?? Calibration.KinectDefaultFileName;
var projfile = args.Skip(1).FirstOrDefault() ?? Calibration.ProjectorDefaultFileName;
if (!File.Exists(camfile) || !File.Exists(projfile))
{
Console.WriteLine("Either calib file could not be found.");
return;
}
var cc = Utils.DeSerializeObject<CalibrationResult>(camfile);
var pc = Utils.DeSerializeObject<CalibrationResult>(projfile);
var window = ProgramWindow.OpenOnSecondary();
var program = new LightStudioProgram(0.08f);
window.SetProgram(program);
var overviewWindow = new ProgramWindow(750, 50, 1280, 960);
overviewWindow.Load();
overviewWindow.ResizeGraphics();
OverviewProgram overview = new OverviewProgram(program);
overviewWindow.SetProgram(overview);
KinectSensor sensor = KinectSensor.KinectSensors.First();
var format = DepthImageFormat.Resolution80x60Fps30;
DepthCamera depthCam = new DepthCamera(sensor, format);
KinectCalibrator kc = new KinectCalibrator(cc);
SkeletonCamera scam = new SkeletonCamera(sensor);
sensor.Start();
float[] data = Utils.DeSerializeObject<float[]>(LightningFastApp.IR2RGBFILE) ?? MathNet.Numerics.LinearAlgebra.Single.DenseMatrix.Identity(4).ToColumnWiseArray();
MathNet.Numerics.LinearAlgebra.Generic.Matrix<float> D2C = MathNet.Numerics.LinearAlgebra.Single.DenseMatrix.OfColumnMajor(4, 4, data);
//var adjustment = OpenTK.Matrix4.CreateTranslation(0f, 0.14f, 0.00f);
//program.SetProjection(pc, kc.GetModelView(D2C), adjustment);
//program.SetProjection(pc); //, kc.GetModelView(D2C), OpenTK.Matrix4.CreateTranslation(0f, 0.14f, 0.06f));
//var rs = Range.OfDoubles(0.5, -0.5, 0.04);
//program.SetPositions(rs.SelectMany(x => rs.Select(y => new Vector3((float)x, (float)y, 1.7f))).ToArray());
//while (true)
//{
// window.RenderFrame();
// window.ProcessEvents();
//}
var keyl = new KeyboardListener(window.Keyboard);
SkeletonPoint[] skeletons = null;
// Action H hide background:
bool hideBackground = false;
float zCutoff = 3.4f;
float xadj = 0, yadj = 0.0f, zadj = 0;
float incr = 0.01f;
bool adjust = true;
program.SetProjection(pc, kc.GetModelView(D2C), OpenTK.Matrix4.CreateTranslation(xadj, yadj, zadj));
Action<float> xfoo = (f) =>
{
if (!adjust)
return;
xadj += f;
program.SetProjection(pc, kc.GetModelView(D2C), OpenTK.Matrix4.CreateTranslation(xadj, yadj, zadj));
};
Action<float> yfoo = (f) =>
{
if (!adjust)
return;
yadj += f;
program.SetProjection(pc, kc.GetModelView(D2C), OpenTK.Matrix4.CreateTranslation(xadj, yadj, zadj));
};
Action<float> zfoo = (f) =>
{
if (!adjust)
return;
zadj += f;
program.SetProjection(pc, kc.GetModelView(D2C), OpenTK.Matrix4.CreateTranslation(xadj, yadj, zadj));
};
keyl.AddAction(() => adjust = !adjust, Key.A);
keyl.AddBinaryAction(1, -1, Key.Up, Key.Down, null, (i) => yfoo(i * incr));
keyl.AddBinaryAction(1, -1, Key.Left, Key.Right, null, (i) => xfoo(i * incr));
keyl.AddBinaryAction(1, -1, Key.Up, Key.Down, new Key[] { Key.ShiftLeft }, (i) => zfoo(i * incr));
keyl.AddBinaryAction(1, -1, Key.Up, Key.Down, new Key[] { Key.ControlLeft }, (i) => yfoo(i * incr * 3));
keyl.AddBinaryAction(1, -1, Key.Left, Key.Right, new Key[] { Key.ControlLeft }, (i) => xfoo(i * incr * 3));
keyl.AddBinaryAction(1, -1, Key.Up, Key.Down, new Key[] { Key.ShiftLeft, Key.ControlLeft }, (i) => zfoo(i * incr * 3));
keyl.AddAction(() =>
{
hideBackground = !hideBackground;
if (skeletons != null && skeletons.Length > 0)
{
zCutoff = skeletons.Min(sp => sp.Z);
}
else
zCutoff = 0;
}, OpenTK.Input.Key.H);
while (true)
{
var points = depthCam.Get(1000).Where(p => p.HasValue && p.Value.Index > 0).Select(p => p.Value);
skeletons = points.Select(p => sensor.CoordinateMapper.MapDepthPointToSkeletonPoint(format, p.Point)).ToArray();
program.SetPositions(skeletons.Where(sp => !hideBackground || sp.Z < zCutoff).Select(sp => new Vector3(sp.X, sp.Y, sp.Z)).ToArray());
overview.SetPointCloud(0, skeletons.Select(sp =>
{
var gp = kc.ToGlobal(sp);
return new DynamicVertex(new Vector3(gp[0], gp[1], gp[2]));
}).ToArray());
window.RenderFrame();
overviewWindow.RenderFrame();
window.ProcessEvents();
overviewWindow.ProcessEvents();
}
}
public static void Run(string[] args)
{
var camfile = args.FirstOrDefault() ?? Calibration.KinectDefaultFileName;
var projfile = args.Skip(1).FirstOrDefault() ?? Calibration.ProjectorDefaultFileName;
if (!File.Exists(camfile) || !File.Exists(projfile))
{
Console.WriteLine("Either calib file could not be found.");
return;
}
var cc = Utils.DeSerializeObject<CalibrationResult>(camfile);
var pc = Utils.DeSerializeObject<CalibrationResult>(projfile);
var window = ProgramWindow.OpenOnSecondary();
var program = new FastPointCloudProgram(15f);
window.SetProgram(program);
KinectSensor sensor = KinectSensor.KinectSensors.First();
Camera cam = new Camera(sensor, ColorImageFormat.RgbResolution640x480Fps30);
var format = DepthImageFormat.Resolution80x60Fps30;
DepthCamera depthCam = new DepthCamera(sensor, format);
SkeletonCamera skeletonCam = new SkeletonCamera(sensor);
TriplexCamera triplex = new TriplexCamera(depthCam, skeletonCam);
KinectCalibrator kc = new KinectCalibrator(cc);
sensor.Start();
sensor.SkeletonStream.Enable();
float[] data = Utils.DeSerializeObject<float[]>(IR2RGBFILE) ?? MathNet.Numerics.LinearAlgebra.Single.DenseMatrix.Identity(4).ToColumnWiseArray();
MathNet.Numerics.LinearAlgebra.Generic.Matrix<float> D2C = MathNet.Numerics.LinearAlgebra.Single.DenseMatrix.OfColumnMajor(4,4, data);
program.SetProjection(pc, kc.GetModelView(D2C), OpenTK.Matrix4.CreateTranslation(0f, 0.14f, 0.06f));
TimedBlockRecorder rec = new TimedBlockRecorder();
float leastError = float.MaxValue;
MathNet.Numerics.LinearAlgebra.Generic.Matrix<float> leastErrD2C;
while (true)
{
CompositePlayer[] players;
DepthImagePoint[] depths;
using (var block = rec.GetBlock("Triplex trigger"))
players = triplex.Trigger(1000);
if (players == null)
continue;
using (var block = rec.GetBlock("Player sorting into depths"))
depths = players.Where(p => p.Skeleton != null).SelectMany(s => s.DepthPoints).ToArray();
if (depths.Count() > 0)
{
if (true)
{
using (var block = rec.GetBlock("Find Rigid transform"))
{
var points = kc.ToColorSpace(sensor.CoordinateMapper, depths, format);
var B = points.Select(p => new float[] { p.X, p.Y, p.Z }).ToArray();
var A = depths.Select(d => sensor.CoordinateMapper.MapDepthPointToSkeletonPoint(format, d)).Select(sp => new float[] { sp.X, sp.Y, sp.Z }).ToArray();
float error;
var res = Dynamight.ImageProcessing.CameraCalibration.Maths.RigidTransformation.FindTransform(A, B, out error);
if (error < leastError)
{
D2C = leastErrD2C = res;
Utils.SerializeObject(leastErrD2C.ToColumnWiseArray(), IR2RGBFILE);
leastError = error;
program.SetProjection(pc, kc.GetModelView(D2C));
}
}
}
Vector3[] ps;
using (var block = rec.GetBlock("Map to skeleton space"))
ps = depths.Select(d => sensor.CoordinateMapper.MapDepthPointToSkeletonPoint(format, d)).Select(d => new Vector3(d.X, d.Y, d.Z)).ToArray();
using (var block = rec.GetBlock("Set positions"))
program.SetPositions(ps);
}
else
program.SetPositions(new Vector3[0]);
using (var block = rec.GetBlock("Render frame"))
window.RenderFrame();
window.ProcessEvents();
Console.Clear();
Console.WriteLine(rec.ToString());
Console.WriteLine(rec.AverageAll());
Console.WriteLine("Error: " + leastError);
}
}
public static void Run(string[] args)
{
var camfile = args.FirstOrDefault() ?? Calibration.KinectDefaultFileName;
var projfile = args.Skip(1).FirstOrDefault() ?? Calibration.ProjectorDefaultFileName;
if (!File.Exists(camfile) || !File.Exists(projfile))
{
Console.WriteLine("Either calib file could not be found.");
return;
}
var cc = Utils.DeSerializeObject<CalibrationResult>(camfile);
var pc = Utils.DeSerializeObject<CalibrationResult>(projfile);
var window = ProgramWindow.OpenOnSecondary();
var program = new PointCloudProgram(29f);
window.SetProgram(program);
program.Draw().All((xp, yp) =>
{
var x = 0.5 - xp;
var y = 0.5 - yp;
var i = Math.Sqrt(x * x + y * y) * 2.5;
if (i > 1)
i = 1;
i = Math.Pow(1 - i, 3);
byte ii = (byte)(i * 255);
return Color.FromArgb(ii, 255, 255, 255);
}).Finish();
KinectSensor sensor = KinectSensor.KinectSensors.First();
var format = DepthImageFormat.Resolution80x60Fps30;
DepthCamera depthCam = new DepthCamera(sensor, format);
SkeletonCamera skeletonCam = new SkeletonCamera(sensor);
TriplexCamera triplex = new TriplexCamera(depthCam, skeletonCam);
KinectCalibrator kc = new KinectCalibrator(cc);
sensor.Start();
sensor.SkeletonStream.Enable();
program.SetProjection(pc, kc.GetModelView());
Rendering rend = new Rendering(program);
float dt = 0;
{
float[] hand = new float[] { 0.124f, -0.9f, 1.7f, 1f };
{
var t = program.SetLight0Pos(hand);
Console.Write("({0}, {1}, {2}) ", hand[0], hand[1], hand[2]);
Console.WriteLine("({0}, {1}, {2})", t.X, t.Y, t.Z);
}
window.KeyPress += (o, e) =>
{
if (e.KeyChar == 'w')
hand[1] += 0.1f;
else if (e.KeyChar == 's')
hand[1] -= 0.1f;
if (e.KeyChar == 'a')
hand[0] += 0.1f;
else if (e.KeyChar == 'd')
hand[0] -= 0.1f;
if (e.KeyChar == 'r')
hand[2] += 0.1f;
else if (e.KeyChar == 'f')
hand[2] -= 0.1f;
var t = program.SetLight0Pos(hand);
Console.Write("({0}, {1}, {2}) ", hand[0], hand[1], hand[2]);
Console.WriteLine("({0}, {1}, {2})", t.X, t.Y, t.Z);
};
}
while (true)
{
var players = triplex.Trigger(1000);
if (players == null)
continue;
rend.Render(players, sensor, format);
window.RenderFrame();
window.ProcessEvents();
}
//program.SetPositions(new Vector3[] { new Vector3(0.0f, -0.1f, 0) });
//window.RenderFrame();
Func<float[]> getHand = () =>
{
using (var frame = sensor.SkeletonStream.OpenNextFrame(100))
{
if (frame == null)
return null;
Skeleton[] skeletons = new Skeleton[frame.SkeletonArrayLength];
frame.CopySkeletonDataTo(skeletons);
var rhand = skeletons.Where(sk => sk.TrackingState == SkeletonTrackingState.Tracked)
.SelectMany(sk => sk.Joints).Where(j => j.TrackingState != JointTrackingState.NotTracked)
.Where(j => j.JointType == JointType.HandRight)
.Select(j => j.Position).ToArray();
if (rhand.Length > 0)
return kc.ToGlobal(rhand.First());
else
return null;
}
};
while (true)
{
var test = depthCam.GetDepth(10000);
if (test == null)
continue;
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
sw.Start();
var points = test.Select(f => sensor.CoordinateMapper.MapDepthPointToSkeletonPoint(format, f))
.Select(p => new Vector3(p.X, p.Y, p.Z)).ToArray();
program.SetPositions(points);
window.RenderFrame();
sw.Stop();
Console.WriteLine("Time: " + sw.ElapsedMilliseconds);
window.ProcessEvents();
//System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
//sw.Start();
//var sp = test.Select(f => sensor.CoordinateMapper.MapDepthPointToSkeletonPoint(format, f));
//var globals = sp.Select(p => new float[] { p.X, p.Y, p.Z, 1 }).ToArray();
////var globals = kc.ToGlobal(sp).Where(g => g[2] > 0.1f).ToArray();
//sw.Stop();
//Console.WriteLine("Time: " + sw.ElapsedMilliseconds);
//program.SetLight0Pos(new float[] { 0.16f, -0.14f, 0.24f, 1 });
//var hand = getHand();
//if (hand != null)
//{
// var t = program.SetLight0Pos(hand);
// Console.Write("({0}, {1}, {2}) ", hand[0], hand[1], hand[2]);
// Console.WriteLine("({0}, {1}, {2})", t.X, t.Y, t.Z);
// //Console.Write("Done ");
//}
//program.SetPositions(globals);
//window.RenderFrame();
//window.ProcessEvents();
//{
// var projected = pc.Transform(globals.Where(g => g[2] > -0.0f).ToArray());
// projector.DrawPoints(projected,2, Color.Gray);
//}
}
}
public static void Run(string[] args)
{
//var debug = new BitmapWindow(550, 50, 640, 480);
//debug.Load();
//debug.ResizeGraphics();
//debug.DrawBitmap(test);
//while (true)
// debug.ToString();
var camfile = args.FirstOrDefault() ?? Calibration.KinectDefaultFileName;
var projfile = args.Skip(1).FirstOrDefault() ?? Calibration.ProjectorDefaultFileName;
if (!File.Exists(camfile) || !File.Exists(projfile))
{
Console.WriteLine("Either calib file could not be found.");
return;
}
var cc = Utils.DeSerializeObject<CalibrationResult>(camfile);
var pc = Utils.DeSerializeObject<CalibrationResult>(projfile);
var window = ProgramWindow.OpenOnSecondary();
KinectSensor sensor = KinectSensor.KinectSensors.First();
SkeletonCamera skeletonCam = new SkeletonCamera(sensor);
KinectCalibrator kc = new KinectCalibrator(cc);
sensor.Start();
var parameters = new TransformSmoothParameters
{
Smoothing = 0.5f,
Correction = 0.1f,
Prediction = 1.1f,
JitterRadius = 0.05f,
MaxDeviationRadius = 0.05f
};
sensor.SkeletonStream.Enable(parameters);
var program = new MovingHeadsProgram();
window.SetProgram(program);
program.SetProjection(pc);
var heads = program.CreateRenderables(3);
//program.Draw().All((xp, yp) =>
//{
// var x = 0.5 - xp;
// var y = 0.5 - yp;
// var i = Math.Sqrt(x * x + y * y) * 2.5;
// if (i > 1)
// i = 1;
// i = Math.Pow(1 - i, 3);
// byte ii = (byte)(i * 255);
// return Color.FromArgb(ii, ii, ii, ii);
//}).Finish();
while (true)
{
var skeletons = skeletonCam.GetSkeletons(1000);
if (skeletons != null && skeletons.Length > 0)
{
var joints = skeletons.Where(sk => sk.TrackingState == SkeletonTrackingState.Tracked).SelectMany(sk => sk.Joints)
.Where(j => j.TrackingState == JointTrackingState.Tracked).ToArray();
var rh = joints.Where(j => j.JointType == JointType.HandRight).ToArray();
var lh = joints.Where(j => j.JointType == JointType.HandLeft).ToArray();
var h = joints.Where(j => j.JointType == JointType.Head).ToArray();
Func<SkeletonPoint, Vector3> transform = (sp) =>
{
var v = kc.ToGlobal(sp);
return new Vector3(v[0], v[1], v[2]);
};
if (rh.Length > 0)
heads[0](transform(rh.First().Position), true);
else
heads[0](new Vector3(), false);
if (lh.Length > 0)
heads[1](transform(lh.First().Position), true);
else
heads[1](new Vector3(), false);
if (h.Length > 0)
heads[2](transform(h.First().Position), true);
else
heads[2](new Vector3(), false);
}
window.RenderFrame();
window.ProcessEvents();
}
}
