3D Baby Monitor

Imports Microsoft.Kinect
Imports OpenTK
Imports OpenTK.Graphics
Imports OpenTK.Graphics.OpenGL
Imports System.Threading

Public Class Form1

    Dim sw As Stopwatch = New Stopwatch

    'Kinect Declares
    Dim kinz As KinectSensor
    Dim depthz As DepthImageFrame
    Dim colorz As ColorImageFrame
    Dim vectordata As List(Of Vector3) = New List(Of Vector3)
    Dim colordata As List(Of Color) = New List(Of Color)

    'OpenTK Declares
    Dim rotateX As Integer = 0
    Dim rotatey As Integer = 1
    Dim rotatez As Integer = 0
    Dim positionx As Integer = 0 '250
    Dim positiony As Integer = 0 '-250
    Dim Scalez As Decimal = 1
    Dim zpos As Integer = 0 '-100
    Dim mousecontrol As Boolean = False
    Dim mousestartrotate As Point
    Dim mousestartposition As Point
    Dim mousestartZpos As Integer
    Dim mousecontroltype As Integer = 0
    Dim rotateshift As Boolean = False
    Dim translateshift As Boolean = False

    Private Sub Timer1_Tick(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Timer1.Tick

        sw.Restart()
        sw.Start()
        kinectread()

        GlControl1.Invalidate()

        Me.Text = sw.ElapsedMilliseconds
    End Sub

    Private Sub kinectread()
        If depthz IsNot Nothing And colorz IsNot Nothing Then
            Dim pixz(kinz.DepthStream.FramePixelDataLength - 1) As Short
            depthz.CopyPixelDataTo(pixz)
            Dim pixzC(kinz.ColorStream.FramePixelDataLength - 1) As Byte
            colorz.CopyPixelDataTo(pixzC)

            Dim tempx, tempy As Integer
            Dim tripoints As Vector3
            Dim colorpoints As Drawing.Color
            Dim CM As ColorImagePoint

            vectordata = New List(Of Vector3)
            colordata = New List(Of Color)

            For i = 0 To pixz.Length - 1

                tempy = i / 640
                tempx = i - (tempy * 640)
                If tempx < 0 Then
                    tempx = tempx + 640
                End If

                Dim depth As Single = (pixz(i) >> 5) 'Kinect gives 11 bits resolution, but it needs to be shifted down.
                Dim xscale As Single = 1 / 594.21434211923247 'These equations are for the Disparity correction
                Dim yscale As Single = 1 / 591.04053696870778
                Dim xoffset As Single = 339.30780975300314
                Dim yoffset As Single = 242.73913761751615
                Dim x As Single = ((tempx) - xoffset) * depth * xscale
                Dim y As Single = (((480 - tempy) - 240) - yoffset) * depth * yscale

                tripoints = New Vector3(x, y, depth) 'make the point

                'Get color
                CM = kinz.MapDepthToColorImagePoint(DepthImageFormat.Resolution640x480Fps30, tempx, tempy, pixz(i), ColorImageFormat.RgbResolution640x480Fps30)
                CM.X = CM.X - 1
                If CM.X >= 640 Then
                    CM.X = 639
                End If
                CM.Y = CM.Y - 1
                If CM.Y >= 480 Then
                    CM.Y = 479
                End If
                colorpoints = Color.FromArgb(pixzC((((CM.Y * 640) + CM.X) * 4) + 2), pixzC((((CM.Y * 640) + CM.X) * 4) + 1), pixzC((((CM.Y * 640) + CM.X) * 4) + 0))

                If tripoints.Z > 10 Then 'Throw out too small
                    vectordata.Add(tripoints)
                    colordata.Add(colorpoints)
                End If

            Next


        End If
    End Sub

    Private Sub Form1_Load(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles MyBase.Load
        For Each potentialSensor In KinectSensor.KinectSensors
            If potentialSensor.Status = KinectStatus.Connected Then
                kinz = potentialSensor
                Exit For
            End If
        Next potentialSensor

        If kinz Is Nothing Then
            Return
        End If

        kinz.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30)
        kinz.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30)
        AddHandler kinz.DepthFrameReady, AddressOf depthready
        AddHandler kinz.ColorFrameReady, AddressOf colorready
        kinz.Start()

        kinz.DepthStream.Range = DepthRange.Default
        kinz.ElevationAngle = kinz.MinElevationAngle
    End Sub

    Private Sub depthready(ByVal sender As Object, ByVal e As DepthImageFrameReadyEventArgs)

        depthz = e.OpenDepthImageFrame

    End Sub
    Private Sub colorready(ByVal sender As Object, ByVal e As ColorImageFrameReadyEventArgs)

        colorz = e.OpenColorImageFrame

    End Sub
    Private Sub GlControl1_Load(ByVal sender As Object, ByVal e As System.EventArgs) Handles GlControl1.Load
        GL.ClearColor(Color.Black)
    End Sub

    'GL mouse control
    Private Sub GlControl1_MouseDown(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles GlControl1.MouseDown
        mousecontrol = True
        If e.Button = MouseButtons.Right Then
            mousecontroltype = 2
            mousestartposition = New Point(MousePosition.X - positionx, MousePosition.Y - positiony)
        ElseIf e.Button = MouseButtons.Left Then
            mousestartrotate = New Point(MousePosition.X - rotatey, MousePosition.Y - rotateX)
            mousecontroltype = 1
        ElseIf e.Button = MouseButtons.Middle Then
            mousestartZpos = MousePosition.Y - zpos
            mousecontroltype = 3
        End If

    End Sub
    Private Sub GlControl1_MouseMove(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles GlControl1.MouseMove
        If mousecontrol = True Then

            If mousecontroltype = 1 Then
                rotatey = MousePosition.X - mousestartrotate.X
                rotateX = MousePosition.Y - mousestartrotate.Y
            ElseIf mousecontroltype = 2 Then
                positionx = MousePosition.X - mousestartposition.X
                positiony = MousePosition.Y - mousestartposition.Y
            ElseIf mousecontroltype = 3 Then
                zpos = MousePosition.Y - mousestartZpos
            End If

        End If
    End Sub
    Private Sub GlControl1_MouseUp(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles GlControl1.MouseUp
        mousecontrol = False
        mousecontroltype = 0
        GlControl1.Invalidate()
    End Sub
    Private Sub GlControl1_MouseWheel(ByVal sender As Object, ByVal e As System.Windows.Forms.MouseEventArgs) Handles GlControl1.MouseWheel
        Scalez = Scalez + e.Delta / 240
        If Scalez <= 0 Then
            Scalez = 0.1
        End If
        GlControl1.Invalidate()
    End Sub

    Private Sub GlControl1_Paint(ByVal sender As Object, ByVal e As System.Windows.Forms.PaintEventArgs) Handles GlControl1.Paint

        GL.Clear(ClearBufferMask.ColorBufferBit)
        GL.Clear(ClearBufferMask.DepthBufferBit)

        Dim perspective As OpenTK.Matrix4 = OpenTK.Matrix4.CreatePerspectiveFieldOfView(1.04, 4 / 3, 1, 10000)
        Dim lookat As OpenTK.Matrix4 = OpenTK.Matrix4.LookAt(0, 20, -200, 0, 0, 0, 0, 1, 0)

        GL.MatrixMode(MatrixMode.Projection)
        GL.LoadIdentity()
        GL.LoadMatrix(perspective)

        GL.MatrixMode(MatrixMode.Modelview)
        GL.LoadIdentity()
        GL.LoadMatrix(lookat)

        GL.Viewport(0, 0, GlControl1.Width, GlControl1.Height) 'Size of window
        GL.Rotate(-rotateX, 1, 0, 0)
        GL.Rotate(rotatey, 0, 1, 0)

        GL.Translate((-positionx * Scalez), (-positiony * Scalez), (zpos * Scalez))
        GL.Scale(Scalez, Scalez, Scalez)

        GL.Enable(EnableCap.DepthTest) 'Enable correct Z Drawings
        GL.DepthFunc(DepthFunction.Less) 'Enable correct Z Drawings
        GL.DepthMask(True)

        Dim vertz((vectordata.Count) * 3) As Single
        For i = 0 To vectordata.Count - 1
            vertz((i * 3) + 0) = vectordata(i).X
            vertz((i * 3) + 1) = vectordata(i).Y
            vertz((i * 3) + 2) = vectordata(i).Z
        Next
        Dim colorz((colordata.Count) * 3) As Single
        For i = 0 To colordata.Count - 1
            colorz((i * 3) + 0) = colordata(i).R / 255
            colorz((i * 3) + 1) = colordata(i).G / 255
            colorz((i * 3) + 2) = colordata(i).B / 255
        Next
        GL.PointSize(3)
        GL.EnableClientState(ArrayCap.VertexArray)
        GL.EnableClientState(ArrayCap.ColorArray)
        GL.ColorPointer(3, VertexPointerType.Float, 0, colorz)
        GL.VertexPointer(3, VertexPointerType.Float, 0, vertz)
        GL.DrawArrays(BeginMode.Points, 0, vectordata.Count)
        GL.DisableClientState(ArrayCap.VertexArray)
        GL.DisableClientState(ArrayCap.ColorArray)

        GraphicsContext.CurrentContext.VSync = True
        GlControl1.SwapBuffers()


    End Sub
End Class