NtKinect: Kinect V2 C++ Programming with OpenCV on Windows10

[応用] Kinect V2 で呼吸周期を計測する

前提として理解しておくべき知識

• NtKinect: Kinect V2 でRGBカメラ画像を取得する(基本設定)
• NtKinect: Kinect V2 で Depth 画像を取得する

離散フーリエ変換

データを波の重ね合せで表現する方法が「フーリエ変換」で、 連続した時間ではなくとびとびの時刻にサンプリングしたデータを フーリエ変換する方法を「離散フーリエ変換」といいます。 データの個数が2の累乗の場合には「高速フーリエ変換」という方法が使えますが、 ここでは簡単のために「離散フーリエ変換」について説明します。

データの計測時間を $T$, サンプリングデータ数を $N_s$, サンプリング周期を $f_s$, サンプリング周期の分解能を $\Delta f$ とすると 次の式が成り立ちます。 $$\displaystyle \Delta f = \frac{1}{T} = \frac{f_s}{N_s} $$

「サンプリングしたデータを離散フーリエ変換する」ということは、 元のデータを 「 $\Delta f$, $2 \Delta f$, $\cdots$, $\displaystyle \frac{N_s}{2} \Delta f$ という $\displaystyle \frac{N_s}{2}$ 個の周波数の波の重ね合わせで表現する」 ということです。

離散フーリエ変換を行う c++ の関数 DFT() を次に示します。 この関数は、サンプリングデータを第1引数として受け取り、 離散フーリエ変換した結果を第2引数に指定されたvectorに返します。

#define _USE_MATH_DEFINES
#include <cmath>

void DFT(vector<double>& data,vector<double>&ret) {
  int n = data.size();
  vector<double> re(n), im(n);
  for (int i=0; i<n; i++) {
    re[i] = 0.0;
    im[i] = 0.0;
    double d = 2 * M_PI * i / n;
    for (int j=0; j<n; j++) {
      re[i] += data[j] * cos(d * j);
      im[i] -= data[j] * sin(d * j);
    }
  }
  ret.resize(n);
  for (int i=0; i<n; i++) ret[i] = sqrt(re[i]*re[i] + im[i]*im[i]);
}

[例1]

$N_s = 1024, T=4$ とすると、サンプリング周波数は $\displaystyle f_s = \frac{N_s}{T} = \frac{1024}{4} = 256 $ Hz であり 1秒間に256回の割合でサンプリングすることになり、 また、サンプリング周波数の分解は $\displaystyle \Delta f = \frac{1}{T} = \frac{1}{4} = 0.25$ Hz となります。 このデータに対して離散フーリエ変換を行うと $\Delta f$, $2 \Delta f$, $\cdots$, $\displaystyle \frac{N_s}{2} \Delta f$, すなわち $0.25$, $0.5$, $0.75$, $\cdots$, $128.0$ Hz という512個の周波数の波の重ね合せで表現することになります。

さて、サンプリングによって次のデータが得られたとしましょう。 横軸は時刻で、左端が0秒, 右端が時刻 4 秒です。

この1024点のデータ $d_i$ を離散フーリエ変換すると、 1024個の複素数のデータが得られ、 ちょうど半分のところで折り返した対照的な結果となります。 複素数の大きさ $r_i$ をグラフにすると次の図になります。 インデックスが 0〜511までの間で、両隣より大きい （ $r_{i} > r_{i-1} \bigcap r_{i} > r_{i+1}$ を満たす) データのインデックス 5, 8, 32 が表示されています。 すなわち、このサンプリングされたデータは $5 \Delta f = 1.25$ Hz, $8 \Delta f = 2.0$ Hz, $32 \Delta f = 8.0$ Hz の波の重ね合わせでてきていると解析されたことになります。

実は、上記のサンプリングデータは次の式から作成したものでした。

一般に、1秒間に $a$ 個のデータがある場合は、 $\displaystyle \sin 2 \pi \frac{x}{b}$ という波は 1秒間に $\displaystyle \frac{a}{b}$ 個分の波があるので 周波数が $\displaystyle f = \frac{a}{b}$ となり、 また、フーリエ変換したときのインデックスは $\frac{f}{\Delta f}$ となります。 この例の場合は $a=256, \Delta f = 0.25$ ですから、 $\displaystyle f=\frac{256}{b}, \frac{f}{\Delta f} = \frac{256/b}{0.25}=\frac{1024}{b}$ となります。 各波について周波数と$\Delta f$のインデックスを次の表に示します。 離散フーリエ変換によって、データから近い周波数が正しく求められていることがわかります。

この離散フーリエ変換のVisual Studio 2017 のプロジェクト例は こちら OpenCV_dft.zip。

Depth画像

512x424の解像度でDepth(距離、深度)画像を取得することができます。 測定可能な距離の範囲は 500mm 〜 8000mm ですが、 人間を認識できる範囲は 500mm 〜 4500mm です。

Kinect20.lib において IDepthFrameSource は get_DepthMinReliableDistance()メソッドと get_DepthMaxReliableDistance()メソッドを持っていますが、 それぞれが返す値は 500 と 4500 です。

得られたDepth画像はピクセル毎に UINT16 (16bit 符号なし整数) で表現されます。

NtKinect の Depth 画像に関するメソッド

NtKinect の Depth 画像に関するメンバ変数

Kinect V2 の3種類の座標系

データの種類によって、それを計測するセンサーの位置や解像度が異なります。 そのため、実世界での同じ位置の状態が、センサーによってそれぞれの座標系で表現された値として得られます。 異なるセンサーから得られたデータを同時に利用する場合は、 座標系の変換を行なってどちらかの座標系に合わせる必要があります。

Kinect V2 では、ColorSpace, DepthSpace, CameraSpace という3つの座標系があって、 それぞれの座標を表すデータ型 ColorSpacePoint, DepthSpacePoint, CameraSpacePoint が存在します。

typedef struct _ColorSpacePoint {
    float X;
    float Y;
} ColorSpacePoint;

typedef struct _DepthSpacePoint {
    float X;
    float Y;
} DepthSpacePoint;

typedef struct _CameraSpacePoint {
    float X;
    float Y;
    float Z;
} CameraSpacePoint;

Kinect V2 の座標系とデータ型

RGB画像, Depth画像, 関節情報ではそれぞれ使っている座標系が異なります。 RGB画像の座標系は ColorSpace で、Depth画像の座標系は DepthSpace, 関節情報の座標系は CameraSpace です。

座標系の変換に関するKinect V2のメソッド

Kinect V2 の ICoordinateMapper クラス が保持する「座標系の変換用メソッド」は次の通りです。

座標系の変換に関する NtKinect のメンバ変数

Kinect V2 で座標系の変換に使う ICoordinateMapper クラス のインスタンスは、 NtKinect のメンバ変数 coordinateMapper に保持されています。

「呼吸を計測する」プログラム作成の手順

1. 「NtKinect: Kinect V2 でRGBカメラ画像を取得する(基本設定)」 の Visual Studio 2017 のプロジェクト KinectV2.zipを用いて作成します。
2. main.cppの内容を以下のように変更します。

kinect.setDepth()メソッドを呼び出して kinect.depthImage に距離データを設定します。 引数を指定していないので、距離データそのものを、すなわち、 対象までの距離をmm単位で取得できます。

画面中心よりもやや下の領域の1点の距離と、n_ave (デフォルト値は 8)個のデータの移動平均、 および時刻を記録していきます。 少なくとも min_period (デフォルト値は30秒) 以上計測し、それまでの計測回数を n_dft に設定します。 最新からn_dft個遡ったデータの間で移動平均データに対してフーリエ変換を行って表示します。

main.cpp

/* * Copyright (c) 2017 Yoshihisa Nitta * Released under the MIT license * http://opensource.org/licenses/mit-license.php */ #include <iostream> #include <sstream> #define _USE_MATH_DEFINES #include <cmath> #include "NtKinect.h" using namespace std; void draw(cv::Mat& img, const vector<double>& v, int start = 0, int n = 1024) { stringstream ss; if (start < 0) start = v.size() + start; if (start < 0) start = 0; if (start >= v.size()) return; int end = start + n; if (end > v.size()) end = v.size(); int m = end - start; // real data number if (m <= 0) return; int padding = 30; double wstep = ((double) img.cols - 2 * padding) / n; auto Dmin = *min_element(v.begin()+start,v.begin()+end); auto Dmax = *max_element(v.begin()+start,v.begin()+end); if (Dmin == Dmax) Dmax = Dmin + 1; cv::rectangle(img,cv::Rect(0,0,img.cols,img.rows),cv::Scalar(255,255,255),-1); for (int i=0; i<m; i++) { int x = (int) (padding + i * wstep); int y = (int) (padding + (img.rows - 2 * padding) * (v[start+i] - Dmin) / (Dmax-Dmin)); y = img.rows - 1 - y; cv::rectangle(img,cv::Rect(x-2,y-2,4,4),cv::Scalar(255,0,0),-1); } ss.str(""); ss << (int)Dmin; cv::putText(img,ss.str(),cv::Point(0,img.rows-padding),cv::FONT_HERSHEY_SIMPLEX,0.4,cv::Scalar(0,0,0),1,CV_AA); ss.str(""); ss << (int)Dmax; cv::putText(img,ss.str(),cv::Point(0,padding),cv::FONT_HERSHEY_SIMPLEX,0.4,cv::Scalar(0,0,0),1,CV_AA); } void DFT(vector<double>& data,vector<double>&ret,int start, int n) { if (start < 0) start = data.size() + start; if (start < 0) start = 0; if (start >= data.size()) start = data.size(); if (start + n > data.size()) n = data.size() - start; ret.resize(n); if (n <= 0) return; vector<double> re(n), im(n); for (int i=0; i<n; i++) { re[i] = 0.0; im[i] = 0.0; double d = 2 * M_PI * i / n; for (int j=0; j<n; j++) { re[i] += data[start+j] * cos(d * j); im[i] -= data[start+j] * sin(d * j); } } for (int i=0; i<n; i++) ret[i] = sqrt(re[i]*re[i] + im[i]*im[i]); } void drawTarget(NtKinect& kinect,cv::Mat& img,int dx,int dy,UINT16 depth) { int scale = 4; kinect.rgbImage.copyTo(img); cv::resize(img,img,cv::Size(img.cols/scale,img.rows/scale),0,0); DepthSpacePoint dp; dp.X = (float)dx; dp.Y = (float)dy; ColorSpacePoint cp; kinect.coordinateMapper->MapDepthPointToColorSpace(dp,depth,&cp); cv::rectangle(img,cv::Rect((int)cp.X/scale-10,(int)cp.Y/scale-10,20,20),cv::Scalar(0,0,255),2); stringstream ss; ss << dx << " " << dy << " " << depth << " " << (int)cp.X << " " << (int)cp.Y; cv::putText(img,ss.str(),cv::Point(10,50),cv::FONT_HERSHEY_SIMPLEX,0.7,cv::Scalar(0,0,255),2,CV_AA); } void drawMsg(cv::Mat& img, vector<double>& ret,long dt) { stringstream ss; double df = 1000.0 / dt; // frequency resolution (sec) int y = 100; ss.str(""); ss << "resolution = " << df ; cv::putText(img,ss.str(),cv::Point(100,y),cv::FONT_HERSHEY_SIMPLEX,1.0,cv::Scalar(0,0,0),2,CV_AA); y += 40; for (int i=1; i<ret.size()/2 -1; i++) { if (ret[i] > ret[i-1] && ret[i] > ret[i+1]) { double freq = i * df; if (freq > 1) continue; ss.str(""); ss << i << ": period = " << (1.0/freq) << " " << ret[i]; cv::putText(img,ss.str(),cv::Point(100,y),cv::FONT_HERSHEY_SIMPLEX,1.0,cv::Scalar(0,0,0),2,CV_AA); y += 40; } } } void doJob() { const int n_ave = 8; // running average const long min_period = 32 * 1000; // milliseconds int n_dft = 512; // sampling number (changed) NtKinect kinect; cv::Mat rgb, dImg(480,1280,CV_8UC3);; vector<double> depth, depth_ave; double sum = 0; vector<long> vtime; vector<double> result(n_dft); long t0 = GetTickCount(); bool init_flag = false; for (int count=0; ; count++) { kinect.setDepth(); int dx = kinect.depthImage.cols / 2; int dy = kinect.depthImage.rows * 2 / 3; UINT16 dz = kinect.depthImage.at<UINT16>(dy,dx); depth.push_back((double)dz); long t = GetTickCount(); vtime.push_back(t); sum += (double)dz; if (depth.size() > n_ave) sum -= depth[depth.size()-1-n_ave]; if (depth.size() >= n_ave) { depth_ave.push_back(sum/n_ave); } kinect.setRGB(); drawTarget(kinect,rgb,dx,dy,dz); cv::imshow("rgb", rgb); draw(dImg, depth_ave, -n_dft, n_dft); if (init_flag) { stringstream ss; ss << "n_dft = " << n_dft; cv::putText(dImg,ss.str(),cv::Point(50,200),cv::FONT_HERSHEY_SIMPLEX,1.2,cv::Scalar(0,0,0),2,CV_AA); } cv::imshow("moving average",dImg); if (t - t0 >= min_period) { if (init_flag == false) { //n_dft = (int) pow(2.0, (int)ceil(log2((double) depth_ave.size()))); n_dft = depth_ave.size(); init_flag = true; } } else { if (n_dft < depth_ave.size()) n_dft *= 2; } if (init_flag) { auto Dmin = *min_element(depth_ave.end()-n_dft,depth_ave.end()); auto Dmax = *max_element(depth_ave.end()-n_dft,depth_ave.end()); auto Dmid = (Dmin + Dmax) / 2.0; for (int i=0; i<n_dft; i++) { result[i] = depth_ave[depth_ave.size()-n_dft+i] - Dmid; } DFT(result,result,0,n_dft); draw(dImg,result,0,result.size()); drawMsg(dImg,result,t - vtime[vtime.size()-n_dft]); cv::imshow("DFT",dImg); } if (depth_ave.size() > 10 * n_dft) { depth.erase(depth.begin(), depth.end()-n_dft*2); depth_ave.erase(depth_ave.begin(), depth_ave.end()-n_dft*2); vtime.erase(vtime.begin(), vtime.end()-n_dft*2); } auto key = cv::waitKey(1); if (key == 'q') break; } cv::destroyAllWindows(); } int main(int argc, char** argv) { try { doJob(); } catch (exception &ex) { cout << ex.what() << endl; string s; cin >> s; } return 0; }

3. プログラムを実行するとRGB画像が表示され、距離を測定している場所が赤い正方形で示されます。 別のウィンドウには、移動平均データがn_dft (初期値は512で、30秒経過時点での計測回数に変更される)個表示されます。 30秒以上経過すると、距離の移動平均(n_ave = 8 平均)データに対してフーリエ変換した結果が刻々と表示されます。 'q'キーで終了します。

[注意] このプログラムの実行はVisual Studio 2017の Debug モードで行って下さい。 Releaseモードだとなぜか途中で落ちることがあります。 Debugモードでの実行は OpenCVのライブラリとして opencv_world330d.lib をリンクする必要があります。











4. このトピックスは、Kinect V2のDepthセンサで呼吸のような小さい変化の身体データもそれなりに取得できる という応用例を示すためのものです。 フーリエ変換で呼吸の周期を算出することを勧めているものではありません。

実行例を見ると、この例では$T = 30$ 秒で$N_s = 165$個の計測データが得られているので、 サンプリング周波数は $\displaystyle f_s = \frac{N_s}{T} = \frac{165}{30} = 5.5 Hz $ であり、1秒間に 5.5 回サンプリングしています(MacBook Proで実行したのですが、かなり遅いですね)。 サンプリング周期の分解能力は $\displaystyle \Delta f = \frac{1}{T} = \frac{1}{30} = 0.0333... $となります。このデータに対して離散フーリエ変換を行うと $\Delta f$, $2 \Delta f$, $\cdots$, $\displaystyle \frac{N_s}{2} \Delta f$, すなわち $\displaystyle \frac{1}{30}, \frac{2}{30}, \frac{3}{30}, \frac{4}{30}, \cdots$ Hz という周波数の波に分解され、周期は周波数の逆数ですから、それぞれ $\displaystyle 30, 15, 7.5, 3.25, 1.125, 0.5625, \cdots$ 秒 という周期の波を表しています。呼吸が2秒程度の周期だとするとこの辺りの波は非常にまばらですので、 意味のある値を出すためにはもっと長期間のサンプリングが必要となります。

したがって、周期の算出はフーリエ変換以外の別の方法が適切だと思われます。 適切と思われる方法についてはここでは触れません。

5. サンプルのプロジェクトはこちら KinectV2_depth2.zip。

上記のzipファイルには必ずしも最新の NtKinect.h が含まれていない場合があるので、 こちらから最新版をダウンロードして 差し替えてお使い下さい。