【Kinect V2】JointTypeのpositionをスムーズに取得する

昔作った資産をまたサルベージした

BodyFrameのPositionをColorFrameに変換して画像とかを動かす時に
Positionがガタガタになる問題を解決するために実装した。

ガタガタするのはなぜか

もはやかなりうろ覚えだがKinectV2はToF方式で計測されており
対象へ赤外線を照射し、それが反射してKinectに戻ってくる時間を元に位置情報を取得している。

このとき対象物に動きがあった場合、射出した赤外線がデバイスに戻ってこないことがあり
それが測定不能値としてPositionが欠損するためにガタガタしてしまう（だった気がする）

どうやって解消したか

生の値を使用すると欠損があった場合にPosition.x,y,zはNULLないし0となってしまうので
ここを補完する必要がある

なので位置情報のバッファを取っておいて前後の値で穴埋めすれば解決するはず、
という結論に達した。

以下ソース。
Queueを使用して位置データを保持するDictionaryに、指定した数の履歴を追加しておき
Dictionary内に保持されたPositionを平滑化する（平均値で動かす）
Smoothing関数にJointTypeとそのフレームにおけるPositionを渡せば
平滑化されたPositionが返却される。
今気づいたがz軸が考慮されてねぇ。

using System;
using System.Windows;
using System.Collections.Generic;
using Microsoft.Kinect;

namespace Microsoft.Samples.Kinect.BodyBasics
{
    class SmoothingPoint
    {
        private static Dictionary<JointType, Queue<Point>> pointBufferS = new Dictionary<JointType, Queue<Point>>();
        private static Dictionary<JointType, Queue<Point>> pointBufferE = new Dictionary<JointType, Queue<Point>>();

        public SmoothingPoint()
        {
            foreach (JointType jt in Enum.GetValues(typeof(JointType)))
            {
                pointBufferS.Add(jt, new Queue<Point>());
                pointBufferE.Add(jt, new Queue<Point>());
            }
        }

        public Point Smoothing(JointType jType, Point p)
        {
            return DoubleMovingAverage(jType, p);
        }

        private Point SimpleAverageFilter(JointType jt, Point newPoint, int parameter)
        {
            pointBufferS[jt].Enqueue(newPoint);
            if (pointBufferS[jt].Count <= parameter)
            {
                return newPoint;
            }
            pointBufferS[jt].Dequeue();
            Point[] list = pointBufferS[jt].ToArray();
            Point point = new Point();
            double x = 0;
            double y = 0;

            int n = pointBufferS[jt].Count;
            for (int i = 0; i < pointBufferS[jt].Count; i++)
            {
                Point p = list[i];
                x += p.X;
                y += p.Y;
            }
            point.X = x / n;
            point.Y = y / n;

            return point;
        }

        private Point DoubleMovingAverage(JointType jt, Point newPoint, int parameter = 5)
        {
            Point newSimpleAverage = SimpleAverageFilter(jt, newPoint, parameter);
            pointBufferE[jt].Enqueue(newSimpleAverage);
            if (pointBufferE[jt].Count <= parameter)
            {
                return newSimpleAverage;
            }
            pointBufferE[jt].Dequeue();
            Point[] list = pointBufferE[jt].ToArray();
            Point point = new Point();
            double x = 0;
            double y = 0;

            int n = pointBufferE[jt].Count;
            for (int i = 0; i < pointBufferE[jt].Count; i++)
            {
                Point p = list[i];
                x += p.X;
                y += p.Y;
            }
            point.X = x / n;
            point.Y = y / n;

            return point;
        }
    }
}