private static TexVertex[] BuildVertexCircle(Vector2 center, Vector2 texCenter, float radius, float scale, int complexity = 24) { TexVertex[] vertexData = new TexVertex[complexity + 2]; vertexData[0].Position = new Vector3(center.X, center.Y, 0.0f); vertexData[0].Tu = texCenter.X; vertexData[0].Tv = texCenter.Y; double stepAngle = (Math.PI * 2) / complexity; for (int i = 0; i <= complexity; i++) { double angle = (Math.PI * 2) - (i * stepAngle); float x = (float)Math.Cos(angle); float y = (float)Math.Sin(angle); vertexData[i + 1].Position = new Vector3(center.X + x * radius, center.Y + y * radius, 0.0f); vertexData[i + 1].Tu = texCenter.X + scale * x; vertexData[i + 1].Tv = texCenter.Y - scale * y; } return vertexData; }
public override IObservable <Mat> Process(IObservable <DeviceEvents> source) { return(source.SelectMany(evts => { var device = evts.Device; Extrinsics depthToColor; Intrinsics colorIntrinsics, depthIntrinsics; var colorStream = Observable.FromEvent <FrameCallback, Frame>(handler => evts.ColorFrame += handler, handler => evts.ColorFrame -= handler); var depthStream = Observable.FromEvent <FrameCallback, Frame>(handler => evts.DepthFrame += handler, handler => evts.DepthFrame -= handler); device.GetStreamIntrinsics(Stream.Color, out colorIntrinsics); device.GetStreamIntrinsics(Stream.Depth, out depthIntrinsics); device.GetExtrinsics(Stream.Depth, Stream.Color, out depthToColor); var pixelScale = new Vector2(colorIntrinsics.Width, colorIntrinsics.Height); var depthScale = device.DepthScale; int depthBufferWidth = 0; ushort[] depthBuffer = null; TexVertex[] depthPoints = null; var depthBufferStream = depthStream.Select(frame => { if (depthBuffer == null) { depthBufferWidth = frame.Width; depthBuffer = new ushort[frame.Width * frame.Height]; depthPoints = new TexVertex[depthBuffer.Length]; } var depthFrameHeader = new Mat(frame.Height, frame.Width, Depth.U16, 1, frame.FrameData, frame.Stride); using (var bufferHeader = Mat.CreateMatHeader(depthBuffer, depthFrameHeader.Rows, depthFrameHeader.Cols, depthFrameHeader.Depth, depthFrameHeader.Channels)) { CV.Copy(depthFrameHeader, bufferHeader); } return depthBuffer; }); return depthBufferStream.Select(depth => { int pindex = 0; for (int i = 0; i < depthBuffer.Length; i++) { Vector2 depthPixel, colorPixel; depthPixel.X = i % depthBufferWidth; depthPixel.Y = i / depthBufferWidth; var depthValue = depth[i] * depthScale; if (depthValue == 0) { continue; } Vector3 depthPoint, colorPoint; Intrinsics.DeprojectPoint(ref depthPixel, ref depthIntrinsics, depthValue, out depthPoint); Extrinsics.TransformPoint(ref depthPoint, ref depthToColor, out colorPoint); Intrinsics.ProjectPoint(ref colorPoint, ref colorIntrinsics, out colorPixel); Vector2.Divide(ref colorPixel, ref pixelScale, out depthPoints[pindex].TexCoord); depthPoints[pindex].Position = depthPoint; pindex++; } return Mat.FromArray(depthPoints, pindex, 5, Depth.F32, 1); }); })); }