Friday, September 11, 2015

การตรวจจับตำแหน่งศีรษะด้วย Kinect

นอกจาก Kinect จะสามารถอ่านภาพวิดีโอ มันยังสามารถตรวจจับข้อต่อต่างๆของมนุษย์ด้วยกล้อง infrared ซึ่งจะสามารถอ่านได้ถึง 20 ข้อต่อ (Joints) ได้แก่ หัว คอ ไหล่ ข้อศอก ข้อมือ มือ เอว สะโพก ท่อนขา หัวเข่า ข้อเท้า และ เท้า

ในตอนนี้เราจะมาลองตรวจจับตำแหน่งของศีรษะกันครับ

พิกัดในการอ่านตำแหน่งของ Kinect จะมีแกนดังนี้ ถ้าวาง Kinect ไว้หน้าเรา จุดกำเนิดจะอยู่ที่กล้อง แกน X จะมีทิศไปทางขวา แกน Y จะชี้ขึ้น และแกน Z จะพุ่งจากกล้องเข้าหาเรา

นั่นคือถ้าเราขยับไปทางขวามือของเรา เมื่ออ่านค่าข้อต่อ ค่า X ก็จะเพิ่ม หรือ ถ้าเรากระโดด ค่า Y ก็จะเพิ่ม และถ้าเราถอยห่างออกจากกล้อง ค่า Z ก็จะเพิ่ม

สำหรับการตรวจจับศีรษะ เราจะสร้าง interface ของโปรแกรมให้อ่านค่าได้ประมาณนี้

ในกรณีที่กล้องจับตำแหน่งศีรษะไม่ได้

ในกรณีที่กล้องจับตำแหน่งศีรษะได้

ซึ่งค่าพิกัดที่ได้คือตำแหน่งของศีรษะเมื่อเทียบกับตัวกล้อง ในหน่วยเมตร

โค้ดของส่วน UI
<Window x:Class="HeadDetection.MainWindow"
        xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
        xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
        Title="Head Detection" SizeToContent="Height" Width="800" Loaded="Window_Loaded">
    <StackPanel>
        <TextBlock Name="TextHead" Text="Head Position" FontSize="72" HorizontalAlignment="Center"/>
    </StackPanel>
</Window>
โค้ดที่เหลือก็จะประมาณนี้ครับ ลองสังเกตคำอธิบายที่หมายเหตุที่แทรกไว้ได้ครับ

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;

using Microsoft.Kinect;

namespace HeadTracking
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        //variables
        KinectSensor myKinect;

        public MainWindow()
        {
            InitializeComponent();
        }

        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            //Check if there is any connecting Kinect
            if (KinectSensor.KinectSensors.Count == 0)
            {
                MessageBox.Show("No Kinect detected!", "Error");
                //end this app
                Application.Current.Shutdown();
                return;
            }

            //Try to initialize Kinect
            try
            {
                //Get the first Kinect connected to this computer
                myKinect = KinectSensor.KinectSensors[0];
                //Enable the skeleton stream
                myKinect.SkeletonStream.Enable();
                //Start the sensor
                myKinect.Start();
            }
            catch
            {
                MessageBox.Show("Initialize Kinect failed!", "Error");
                //end this app
                Application.Current.Shutdown();
            }

            //Skeleton event handler
            myKinect.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(myKinect_SkeletonFrameReady);
        }

        private void myKinect_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            //array of skeleton data
            Skeleton[] skeletons = null;

            using (SkeletonFrame frame = e.OpenSkeletonFrame())
            {
                string message = "No Skeleton Data";
                //if there is skeleton data
                if (frame != null)
                {
                    //create skeleton array
                    skeletons = new Skeleton[frame.SkeletonArrayLength];
                    //copy skeleton data to array
                    frame.CopySkeletonDataTo(skeletons);
                }

                //if no skeleton data
                if(skeletons == null)
                    return;

                //for each skeleton data in the skeleton array, there will be upto 6 possible skeletons (men)
                //only 2 skeletons are fully tracked, other 4 are for positions only (no joint info)
                foreach(Skeleton skeleton in skeletons)
                {
                    //focus only the trackable skeletons
                    if (skeleton.TrackingState == SkeletonTrackingState.Tracked)
                    {
                        //get the head joint
                        Joint headJoint = skeleton.Joints[JointType.Head];
                        //get the head joint's position
                        SkeletonPoint headPosition = headJoint.Position;

                        //show the head position with one decimal or 0.1 meter
                        message = string.Format("Head: X:{0:0.0} Y:{1:0.0} Z:{2:0.0}",
                            headPosition.X,
                            headPosition.Y,
                            headPosition.Z);
                    }
                }

                //show message on textblock
                TextHead.Text = message;
            }
        }
    }
}

Sunday, September 6, 2015

เพิ่มประสิทธิภาพการแสดงและประมวลผลภาพจาก Kinect ด้วย OpenNextFrame และ Thread

จนถึงตอนนี้ เราสามารถเชื่อมต่อ อ่านค่า และประมวลผลภาพจาก Kinect ได้ แต่มีข้อจำกัดเรื่องประสิทธิภาพอยู่บ้าง สำหรับคอมพิวเตอร์ที่มี CPU ไม่สูงพอ นั่นคือ การได้ภาพจาก Kinect จะมาจาก Event ชื่อ ColorImageFrameReadyEventArgs ซึ่งจะมาอย่างต่อเนื่องเมื่อกล้องของ Kinect ทำงาน

ปัญหาจะเกิดในกรณีที่ CPU ประมวลผลภาพในเฟรมไม่ทัน ในขณะที่เฟรมใหม่ก็เข้ามาแล้ว

เพื่อจะแก้ปัญหานี้ เราสามารถบังคับให้รับเฟรมใหม่เข้ามา ตามเวลาที่ระบุไว้ (ไม่ต้องตามกล้องของ Kinect อย่างเดียว) โดยใช้ method ชื่อ ColorStream.OpenNextFrame(time) ซึ่งจะมี argument เป็น เวลาในหน่วยมิลลิวินาที

นอกจากนี้แล้ว เรายังสามารถเพิ่มประสิทธิภาพการทำงาน โดยกำหนดให้การแสดงและประมวลผลภาพเป็น thread ใหม่ แยกออกจาก thread ในการจัดการ User Interface ได้ ซึ่งทั้งสอง thread สามารถทำงานควบคู่กันไปได้

โค้ดใหม่ที่ปรับปรุงก็จะประมาณนี้ครับ ดูคำอธิบายที่ comment ในโค้ดได้เลยครับ

ปล. โค้ดนี้ยังมีส่วนผิดพลาดในการบันทึกภาพ เนื่องจากการใช้งาน thread อยู่ ถ้าผมเจอวิธีแก้จะกลับมาปรับปรุงอีกทีครับ

ส่วนของ UI
<Window x:Class="KinectPerformance.MainWindow"
        xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
        xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
        Title="MainWindow" SizeToContent="WidthAndHeight" Loaded="Window_Loaded" Closing="Window_Closing">
    <StackPanel Orientation="Horizontal" >
        <Image Name="kinectVideo" Height="480" Width="640" />
        <StackPanel Width="80">
            <CheckBox Content="Reflect" Name="cbReflect" Margin="10,20" Click="cbReflect_Click"/>
            <Button Name="bttSave" Content="Save" Margin="10,20" Click="onSave" HorizontalAlignment="Center" VerticalAlignment="Center"></Button>
        </StackPanel>
    </StackPanel>
</Window>

ส่วนโค้ด
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;

//import additional references
using Microsoft.Kinect;     //for Kinect
using System.IO;            //for saving image
using System.Threading;     //for separating video display process

namespace KinectPerformance
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        //Variables
        KinectSensor myKinect;
        Thread updateVideoThread;
        bool displayActive = true;      //flag to show/not show video
        byte[] colorData = null;        //array to keep image pixel data
        bool reflect = false;           //flag to reflect image
        bool takePicture = false;       //flag to capture a video frame
        BitmapSource capBitmap = null;      //bitmap for a captured frame
        WriteableBitmap imageBitmap = null; //bitmap for keeping processed image
        int frameWidth, frameHeight, frameBytePerPixel;

        public MainWindow()
        {
            InitializeComponent();
        }

        //===================== when window is loaded =======================
        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            //if Kinect is not found
            if (KinectSensor.KinectSensors.Count == 0)
            {
                MessageBox.Show("No Kinects detected", "Error");
                //end this app
                Application.Current.Shutdown();
                return;
            }

            //Try to initialize Kinect
            try
            {
                //Get the first Kinect connected to this computer
                myKinect = KinectSensor.KinectSensors[0];
                //Enable the color video stream
                myKinect.ColorStream.Enable();
                //Start the sensor
                myKinect.Start();
            }
            catch
            {
                MessageBox.Show("Initialize Kinect failed!", "Error");
                //end this app
                Application.Current.Shutdown();
                return;
            }

            //create a thread to display video
            updateVideoThread = new Thread(new ThreadStart(videoDisplay));
            //updateVideoThread.IsBackground = true;

            //start the video display thread
            updateVideoThread.Start();
        }
        //----------------- End window loaded --------------------
        
        //========================= Method to display video ===========================
        void videoDisplay()
        {
            while (displayActive)
            {
                //request next video frame in 10 millisec
                using (ColorImageFrame colorFrame = myKinect.ColorStream.OpenNextFrame(10))
                {
                    //if the next video frame is not ready or not completely processed
                    if (colorFrame == null) 
                        continue;
                    //if not exists, create an array of pixel data and get frame's properties
                    if (colorData == null)
                    {
                        colorData = new byte[colorFrame.PixelDataLength];
                        frameWidth = colorFrame.Width;
                        frameHeight = colorFrame.Height;
                        frameBytePerPixel = colorFrame.BytesPerPixel;
                    }

                    //extract pixel data from the frame to the array
                    colorFrame.CopyPixelDataTo(colorData);

                    //call unsafe method to modify the pixel data
                    if (reflect)
                    {
                        //reflect a half left image to a half right
                        reflectImage(colorData);
                    }

                    //prepare a saved frame
                    if (takePicture)
                    {
                        //create a bitmap from the current frame
                        capBitmap = BitmapSource.Create(
                            colorFrame.Width, colorFrame.Height, 96, 96, PixelFormats.Bgr32, null,
                            colorData, colorFrame.Width * colorFrame.BytesPerPixel);
                        //switch a photo captured flag
                        takePicture = false;
                    }

                    //This thread is not a main thread. It cannot update the UI, must use this technique:
                    Dispatcher.Invoke(new Action(() => updateDisplay()));
                }
            }
            // when we get here the program is ending – stop the sensor
            myKinect.Stop();
        } 
        //-------------- end videoDisplay() method ------------------------------

        //================= unsafe method to modify pixel data ===================
        //Reflect the half left of the image to the half right
        unsafe void reflectImage(byte[] colorData)
        {
            //fixed pixel data array in memory, otherwise it can be moved and be inaccessible by a pointer
            fixed (byte* pImage = colorData)
            {
                //in this block the data array in memory will be fixed                
                //we use integer pointer to move by one pixel, faster than by one byte
                //however, we cannot get color data, OK for reflection                

                //loop through each row
                for (int row = 0; row < frameHeight; row++)
                {
                    //set a start pointer to the beginning of each row
                    int* pStart = (int*)pImage + (row * frameWidth);
                    //set an end pointer to the end of each row
                    int* pEnd = pStart + frameWidth - 1;
                    //loop through each column of the row
                    while (pStart < pEnd)
                    {
                        //assign right pixel to left pixel
                        *pEnd = *pStart;
                        //move start pointer to right
                        pStart++;
                        //move end pointer to left
                        pEnd--;
                    }
                }//end for                               
            }
        }
        //-------------- end unsafe image processing method ---------------------

        //==================== Method to update display ======================
        void updateDisplay()
        {
            //For the first video frame, no bitmap, create a writable bitmap of video size
            if (imageBitmap == null)
            {
                imageBitmap = new WriteableBitmap(
                    frameWidth,
                    frameHeight,
                    96,     //dpiX
                    96,     //dpiY
                    PixelFormats.Bgr32,
                    null    //palette:none
                    );
            }
            //For other video frames, write video data to bitmap
            imageBitmap.WritePixels(
                new Int32Rect(0, 0, frameWidth, frameHeight),
                colorData,  //video data
                frameWidth * frameBytePerPixel,      //stride
                0   //offset to the array
                );
            //set bitmap to image view
            kinectVideo.Source = imageBitmap;
        }
        //---------------------- end update display ---------------------------

        //================= Method to save a captured frame ========================
        private void onSave(object sender, RoutedEventArgs e)
        {
            //set a flag to capture image in video frame event
            takePicture = true;
            // Configure save file dialog box
            Microsoft.Win32.SaveFileDialog dlg = new Microsoft.Win32.SaveFileDialog();
            dlg.FileName = "capture"; // Default file name
            dlg.DefaultExt = ".jpg"; // Default file extension
            dlg.Filter = "Pictures (.jpg)|*.jpg"; // Filter files by extension

            //if select Dialog OK
            if (dlg.ShowDialog() == true)
            {
                // Save document
                string filename = dlg.FileName;
                using (FileStream stream = new FileStream(filename, FileMode.Create))
                {
                    //encode and save to JPG format
                    JpegBitmapEncoder encoder = new JpegBitmapEncoder();
                    encoder.Frames.Add(BitmapFrame.Create(capBitmap));
                    encoder.Save(stream);
                }
            }            
        }
        //------------------------ end save image -----------------

        //================== When window is closed ================
        private void Window_Closing(object sender, System.ComponentModel.CancelEventArgs e)
        {
            //set flag to end display loop in video thread
            displayActive = false;
        }

        //------------------- end closing window -------------------

        //======================== Checkbox click ==================
        private void cbReflect_Click(object sender, RoutedEventArgs e)
        {
            if (cbReflect.IsChecked == true)
                reflect = true;
            else
                reflect = false;
            //We can replace the if...else with
            //reflect = (cbReflect.IsChecked == true);
        }
        
    }//end class
}//end namespace

การประมวลผลภาพโดยการสะท้อน (Mirror) และการจับภาพจากวิดีโอเฟรมของ Kinect ตอนที่ 2

ตอนที่แล้ว เราเขียนโค้ดเพื่อทำ mirror effect ให้กับวิดีโอ ดังนี้

มาดูคำอธิบายคร่าวๆกันครับ

Method หลักก็จะมี 4 Method ด้วยกันดังนี้

1. Window_Loaded()
อันนี้ก็ตามชื่อ เมื่อหน้าต่างถูกโหลดขึ้นมาเรียบร้อย ก็ไปเรียก Video Event ของ Kinect เพื่อตรวจสอบว่าวิดีโอพร้อมหรือไม่ ถ้าพร้อมก็เรียก Method ในการแสดงผลเพื่อเอาข้อมูลภาพมาแสดงบน Image UI ที่เตรียมไว้

2. myKinect_ColorFrameReady()
ซึ่งจะถูกเรียกจาก Window_Loaded()
ใน Method นี้ก็ทำการอ่านค่าวิดีโอจาก Kinect รวมถึงประมวลผลภาพต่างๆ แล้วแสดงผลบน Image UI

นอกจากนี้ ยังมีการตรวจสอบว่า Checkbox ถูกเลือกหรือไม่ รวมถึงมีการกดปุ่มบันทึกภาพหรือไม่

3. reflectImage()
Method นี้เป็นกระบวนการประมวลผลภาพแบบ Mirror โดยมีหลักการดังนี้
3.1 ได้ข้อมูลภาพจาก Kinect มาเป็นแบบ byte array
3.2 เอาพอยน์เตอร์ pImage ไปชี้ไว้ที่ตำแหน่งเริ่มต้นของ array
3.3 วนลูปสำหรับทุกแถว
   3.3.1 ให้พอยน์เตอร์ pStart ชี้ที่หัวแถว และ pEnd ชี้ที่ท้ายแถว
   3.3.2 เปลี่ยนค่าใน array ณ ตำแหน่งที่ pEnd ชี้อยู่ ให้มีค่าเท่ากับค่าในตำแหน่งที่ pStart ชี้อยู่ (ทำ Mirror นั่นเอง)
   3.3.3 ขยับ pStart ไปคอลัมน์ถัดไป และ ถอย pEnd มายังคอลัมน์ก่อนหน้า
   3.3.4 ทำจนกระทั่ง pStart ชี้ที่เดียวกับ pEnd จึงไปเริ่มต้นใหม่ในแถวถัดไป


4. onSave()
อันนี้ก็สร้าง dialog มาถามว่าจะบันทึกไฟล์ชื่ออะไร ที่ไหน แล้ว encode array ให้เป็นรูป รวมถึงเขียนไฟล์

โดยรวมก็ประมาณนี้ครับ