public static void Run(string[] args) { var camfile = args.FirstOrDefault() ?? Calibration.KinectDefaultFileName; if (!File.Exists(camfile)) { Console.WriteLine("Calib file could not be found."); return; } var cc = Utils.DeSerializeObject<CalibrationResult>(camfile); var overview = new ProgramWindow(750, 50, 640, 480); overview.Load(); overview.ResizeGraphics(); OverviewProgram program = new OverviewProgram(); overview.SetProgram(program); RemoteKinect kinect = new RemoteKinect("localhost", 10500); IDepthCamera depth = new RemoteDepthCamera(kinect); ISkeletonCamera skeleton = new RemoteSkeletonCamera(kinect); kinect.Start(Commands.Depth80 | Commands.Skeleton); TriplexCamera triplex = new TriplexCamera(depth, skeleton); KinectCalibrator kc = new KinectCalibrator(cc); var coordinateMapper = KinectSensor.KinectSensors.First().CoordinateMapper; //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(sensor, cc); //sensor.Start(); while (true) { var players = triplex.Trigger(1000); if (players.Length > 0) { for (int i = 0; i < players.Length; i++) { program.SetPointCloud(i, players[i].DepthPoints.Select(dp => { var sp = coordinateMapper.MapDepthPointToSkeletonPoint(DepthImageFormat.Resolution80x60Fps30, dp); var gp = kc.ToGlobal(sp); return new DynamicVertex(new Vector3(gp[0], gp[1], gp[2])); }).ToArray()); } } overview.ProcessEvents(); overview.RenderFrame(); } }
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 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); } }