FaceToCam unity3d

BodySourceManager.cs

using UnityEngine;
using System.Collections;
using Windows.Kinect;

public class BodySourceManager : MonoBehaviour 
{
    private KinectSensor _Sensor;
    private BodyFrameReader _Reader;
    private Body[] _Data = null;
    
    public Body[] GetData()
    {
        return _Data;
    }
    

    void Start () 
    {
        _Sensor = KinectSensor.GetDefault();

        if (_Sensor != null)
        {
            _Reader = _Sensor.BodyFrameSource.OpenReader();
            
            if (!_Sensor.IsOpen)
            {
                _Sensor.Open();
            }
        }   
    }
    
    void Update () 
    {
        if (_Reader != null)
        {
            var frame = _Reader.AcquireLatestFrame();
            if (frame != null)
            {
                if (_Data == null)
                {
                    _Data = new Body[_Sensor.BodyFrameSource.BodyCount];
                }
                
                frame.GetAndRefreshBodyData(_Data);
                
                frame.Dispose();
                frame = null;
            }
        }    
    }
    
    void OnApplicationQuit()
    {
        if (_Reader != null)
        {
            _Reader.Dispose();
            _Reader = null;
        }
        
        if (_Sensor != null)
        {
            if (_Sensor.IsOpen)
            {
                _Sensor.Close();
            }
            
            _Sensor = null;
        }
    }
}

FaceToCam.cs

using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using Windows.Kinect;
using Microsoft.Kinect.Face;
using System.IO;

public class FaceToCam : MonoBehaviour
{
    private KinectSensor kinectSensor;
    private int bodyCount;
    private Body[] bodies;
    private FaceFrameSource[] faceFrameSources;
    private FaceFrameReader[] faceFrameReaders;
    public GameObject bodyManager;
    
    //this is our game camera
    public GameObject face;
    
    StreamWriter sx;
    StreamWriter sy;
    StreamWriter kx;
    
    //my value 0.005
    public float qq;
    
    //my value 0.01
    public float rr;
    
    //my value 0.0001
    public float init_state;
    
    //bigger values faster rotations and more noise. my value 10
    public float headSmooth;

    private float last_x = 0;
    private float[] last;
    private float last_x3 = 0;

    private float last_y = 0;

    private float last_mod = 0;


    private int updateFrame;

    KalmanFilterSimple1D kalman_X;
    KalmanFilterSimple1D kalman_Y;
    KalmanFilterSimple1D kalman_mod;

    void Start()
    {
        updateFrame = 0;
        kalman_X = new KalmanFilterSimple1D(f: 1, h: 1, q: qq, r: rr);
        kalman_Y = new KalmanFilterSimple1D(f: 1, h: 1, q: qq, r: rr);
        kalman_mod = new KalmanFilterSimple1D(f: 1, h: 1, q: qq, r: rr);

        sx = new StreamWriter("coords_X.txt");
        kx = new StreamWriter("coords_KX.txt");


        // one sensor is currently supported
        kinectSensor = KinectSensor.GetDefault();


        // set the maximum number of bodies that would be tracked by Kinect
        bodyCount = kinectSensor.BodyFrameSource.BodyCount;

        // allocate storage to store body objects
        bodies = new Body[bodyCount];

        // specify the required face frame results
        FaceFrameFeatures faceFrameFeatures =
            FaceFrameFeatures.BoundingBoxInColorSpace
                | FaceFrameFeatures.PointsInColorSpace
                | FaceFrameFeatures.BoundingBoxInInfraredSpace
                | FaceFrameFeatures.PointsInInfraredSpace
                | FaceFrameFeatures.RotationOrientation
                | FaceFrameFeatures.FaceEngagement
                | FaceFrameFeatures.Glasses
                | FaceFrameFeatures.Happy
                | FaceFrameFeatures.LeftEyeClosed
                | FaceFrameFeatures.RightEyeClosed
                | FaceFrameFeatures.LookingAway
                | FaceFrameFeatures.MouthMoved
                | FaceFrameFeatures.MouthOpen;

        // create a face frame source + reader to track each face in the FOV
        faceFrameSources = new FaceFrameSource[bodyCount];
        faceFrameReaders = new FaceFrameReader[bodyCount];
        for (int i = 0; i < bodyCount; i++)
        {
            // create the face frame source with the required face frame features and an initial tracking Id of 0
            faceFrameSources[i] = FaceFrameSource.Create(kinectSensor, 0, faceFrameFeatures);

            // open the corresponding reader
            faceFrameReaders[i] = faceFrameSources[i].OpenReader();
        }
    }

    void LateUpdate()
    {
        if(updateFrame < 1)
        {
            updateFrame++;
            return;
        }
        updateFrame = 0;
        // get bodies either from BodySourceManager object get them from a BodyReader
        var bodySourceManager = bodyManager.GetComponent<BodySourceManager>();
        bodies = bodySourceManager.GetData();
        if (bodies == null)
        {
            return;
        }

        // iterate through each body and update face source
        for (int i = 0; i < bodyCount; i++)
        {
            // check if a valid face is tracked in this face source				
            if (faceFrameSources[i].IsTrackingIdValid)
            {
                using (FaceFrame frame = faceFrameReaders[i].AcquireLatestFrame())
                {
                    if (frame != null)
                    {
                        if (frame.TrackingId == 0)
                        {
                            continue;
                        }

                        // do something with result
                        var result = frame.FaceFrameResult.FaceRotationQuaternion;


                        if(last_x == 0)
                        {
                            last_x = face.transform.rotation.x;
                        }
                        if (last_y == 0)
                        {
                            last_y = face.transform.rotation.y;
                        }
                        
                        sx.WriteLine((-result.X).ToString("0.000000"), true);
                        sx.Flush();

                        kalman_Y.SetState(last_y, init_state);
                        kalman_Y.Correct(-result.Y);

                        kalman_X.SetState(last_x, init_state);
                        kalman_X.Correct(-result.X);
                        float mod_x = last_x % -result.X;
                        
                        kalman_mod.SetState(last_mod, init_state);
                        kalman_mod.Correct(mod_x);
                        
                        //small shakes
                        if(mod_x > 0.01f && mod_x < 0.08f)
                        {

                            mod_x = mod_x / 2;
                        }


                        face.transform.localRotation = Quaternion.Lerp(face.transform.localRotation, new Quaternion((float)kalman_X.State, (float)kalman_Y.State, face.transform.localRotation.z, face.transform.localRotation.w), Time.deltaTime * headSmooth * mod_x + 0.3f);
                        kx.WriteLine(kalman_X.State.ToString("0.000000"), true);
                        kx.Flush();
                        last_y = (float)kalman_Y.State;
                        last_x = (float)kalman_X.State;
                        last_mod = (float)kalman_mod.State;

                        
                        //updateFrame = !updateFrame;
                    }
                }
            }
            else
            {
                // check if the corresponding body is tracked 
                if (bodies[i].IsTracked)
                {
                    // update the face frame source to track this body
                    faceFrameSources[i].TrackingId = bodies[i].TrackingId;
                }
            }
        }

    }
}

KalmanFilterSimple1D

//http://habrahabr.ru/post/140274/
class KalmanFilterSimple1D
{
    public double X0 { get; private set; } // predicted state
    public double P0 { get; private set; } // predicted covariance

    public double F { get; private set; } // factor of real value to previous real value
    public double Q { get; private set; } // measurement noise
    public double H { get; private set; } // factor of measured value to real value
    public double R { get; private set; } // environment noise

    public double State { get; private set; }
    public double Covariance { get; private set; }

    public KalmanFilterSimple1D(double q, double r, double f = 1, double h = 1)
    {
        Q = q;
        R = r;
        F = f;
        H = h;
    }

    public void SetState(double state, double covariance)
    {
        State = state;
        Covariance = covariance;
    }

    public void Correct(double data)
    {
        //time update - prediction
        X0 = F * State;
        P0 = F * Covariance * F + Q;

        //measurement update - correction
        var K = H * P0 / (H * P0 * H + R);
        State = X0 + K * (data - H * X0);
        Covariance = (1 - K * H) * P0;
    }
}

Video demonstration: https://youtu.be/LvOnxGkoW7w

Kalman filter class got from here: http://habrahabr.ru/post/140274/