小嶋秀樹 | 研究室
日本語 | English

このページは，諸般の事情で Mac のみに対応した新しい内容にしてあります．ご注意ください．

openFrameworks 応用編（４）

ここでは，Kinect のさらなる活用について，トピックごとにご紹介していきます．利用するアドオンは ofxOpenNI です．ofxOpenNI の基本的な使い方は「openFrameworks 応用編（３）」の後半を参照してください．

注意：準備作業が抜けている人をよく見かけます．Xcode を正しくインストールしてください．Apple の Developer サイトから無料で入手できます．openFrameworks は最新版をダウンロードしてください．ここでは Xcode 8.1 + openFrameworks 0.9.8 の利用を想定しています．

投影座標系からワールド座標系への変換（ofxOpenNI）
【２つの座標系】

Kinect の距離画像は，各ピクセル位置 {(x, y) | 0 ≤ x ≤ 639, 0 ≤ y ≤ 479} における対象までの距離をミリメートルを単位とする符号なし整数（unsigned short）で計測したものです．下図で，黄色の Screen 上のピクセル位置 (x, y) は，Kinect から見た「方向」を表し，距離 d (500 ≤ d ≤ 10000) は，その方向に沿った障害物までの Kinect からの「距離」となります．空間内の１点は (x, y, d) のように表現できます．これが投影座標系（projective coordinate system）です．

図

一方，ワールド座標系（world coordinate system）は，Kinect を原点としたとき，実空間の中での縦方向を X 軸，横方向を Y 軸，前方向を Z 軸とした直交座標系です．空間内の１点は (X, Y, Z) のように表現できます．各軸の向きは Kinect の姿勢（向き）によって決まりますが，簡単な座標変換によって任意の位置からどのように見えるのかを求めることができ，とても便利です．

図
【地平線レーダ（On-the-Horizon Radar）をつくる】

投影座標系からワールド座標系への変換方法を，地平線レーダ（OTH Radar: On-the-Horizon Radar）をつくることをとおして説明します．地平線レーダとは，Kinect 距離画像の中心を通る１本の水平線上の各点について障害物までの距離を計測し，それをワールド座標系の X-Z 平面として表示するというものです．プログラムはつぎのようになります． 

#pragma once
#include "ofMain.h"
#include "ofxOpenNI.h"
class ofApp : public ofBaseApp {
private:
    ofxOpenNI kinect;
    XnPoint3D XnProject[640], XnWorld[640];
    int horizonY;
public:
    （... 変更なし ...）
};

#include "ofApp.h"
void ofApp::setup() {
    //  window
    ofBackground(0, 0, 0);
    ofSetWindowShape(1280, 480);
    ofSetFrameRate(30);
    //  setup ofxOpenNI
    kinect.setup();
    kinect.setRegister(true);
    kinect.addDepthGenerator();
    //  start kinect
    kinect.start();
    //  set horizonY
    horizonY = 240;
}
void ofApp::update() {
    kinect.update();
    //  get profile on the horizon
    unsigned short *depthData = kinect.getDepthRawPixels().getData();
    for (int x = 0; x < 640; x++) {
        //  save projective coordinates
        XnProject[x].X = x;
        XnProject[x].Y = horizonY;
        XnProject[x].Z = depthData[horizonY * 640 + x];
    }
    //  convert projective position to real-world position
    kinect.getDepthGenerator().ConvertProjectiveToRealWorld(640, XnProject, XnWorld);
}
void ofApp::draw() {
    //  draw depth image
    ofSetColor(255, 255, 255);
    kinect.drawDepth(0, 0, 640, 480);
    ofDrawLine(0, horizonY, 639, horizonY);
    //  draw OTH radar
    ofSetColor(100, 200, 100);
    for (int x = 0; x < 640; x++) {
        XnPoint3D world = XnWorld[x];
        float scrX = world.X * 0.1 + 320, scrY = 479 - world.Z * 0.1;
        if (0 <= scrX && scrX < 640 && 0 <= scrY && scrY < 480)
            ofDrawCircle(640 + scrX, scrY, 3);
    }
}
（... 以下変更なし ...）

ofApp::update() の中では，白い水平線上に見える障害物の投影座標（640個）を XnProject[] という配列に格納し，それをワールド座標の配列 XnWorld[] に変換しています．処理効率を挙げるため，640個の座標を一度に変換しています．XnPoint3D は ofxOpenNI 内部における３次元座標のデータ型です．一方，ofApp::draw() の中では，変換された XnWorld の各点 (X, Z) を小さな丸で描画しています．

図

実行時のウィンドウは上のようになります．左半分は距離画像，右半分は，その下端中央を原点（Kinect の位置）とした X-Z 平面に，計測された深度を描いたもの，つまり，水平線で空間を切った断面を上から見たものです．２つの壁が直角に交わっている様子や，左側に物（ロボット２体）が置いてある様子がわかると思います．画面の 1 ピクセル当たり 1cm としてるので，見えている X-Z 平面は左右が 6.4m・奥行きが 4.8m となります．

【地平線レーダを改良する】

まず，少しはレーダらしく見えるように，X-Z 平面に 1m ごとのグリッド与えます．また，Kinect のセンシング範囲（水平方向で約 58 度）も表示します．ofApp::draw() をつぎのように変更してください． 

void ofApp::draw() {
    //  draw depth image
    ofSetColor(255, 255, 255);
    kinect.drawDepth(0, 0, 640, 480);
    ofDrawLine(0, horizonY, 639, horizonY);
    //  draw grid
    ofSetColor(150, 150, 150);
    for (int x = -300; x <= 600; x += 100)
        ofDrawLine(640 + x + 320, 0, 640 + x + 320, 479);
    for (int y = 0; y < 480; y += 100)
        ofDrawLine(640, 480 - y, 640 + 639, 480 - y);
    //  draw sensing area
    ofSetColor(200, 200, 0);
    float offset = 480 * tan(29.0 * M_PI / 180.0);
    ofDrawLine(640 + 320 - offset, 0, 640 + 320, 480);
    ofDrawLine(640 + 320 + offset, 0, 640 + 320, 480);
    //  draw OTH radar
    ofSetColor(100, 200, 100);
    for (int x = 0; x < 640; x++) {
        XnPoint3D world = XnWorld[x];
        float scrX = world.X * 0.1 + 320, scrY = 479 - world.Z * 0.1;
        if (0 <= scrX && scrX < 640 && 0 <= scrY && scrY < 480)
            ofDrawCircle(640 + scrX, scrY, 3);
    }
}
図

つぎに，背景（壁などのレーダ像）を除去することを試みます．キーボードの 's' (save) が押されたときの距離プロファイルを「背景」として記録し，その「背景」から 250mm 以上手前にあるものだけを表示対象とします．そのために，まず，ofApp.h に背景を記録する配列 backZ を追加します． 

#pragma once
#include "ofMain.h"
#include "ofxOpenNI.h"
class ofApp : public ofBaseApp {
private:
    ofxOpenNI kinect;
    XnPoint3D XnProject[640], XnWorld[640];
    int horizonY;
    unsigned short backZ[640];
public:
    （... 変更なし ...）
};

キーが押されたときに呼び出される ofApp::keyPressed() の中で，水平線上の距離プロファイルを backZ に記録しています．また，ofApp::update() の中では，backZ との差が250 [mm] 以上であれば距離データを採用し，そうでなければ距離 0 とするようにしています． 

void ofApp::setup() {
    （... 変更なし ...）
    //  set horizonY
    horizonY = 240;
    //  initialize background
    for (int x = 0; x < 640; x++)
        backZ[x] = 10000;
}
void ofApp::update() {
    kinect.update();
    //  get profile on the horizon
    unsigned short *depthData = kinect.getDepthRawPixels().getData();
    for (int x = 0; x < 640; x++) {
        //  save projective coordinates
        XnProject[x].X = x;
        XnProject[x].Y = horizonY;
        unsigned short depth = depthData[horizonY * 640 + x];
        XnProject[x].Z = (backZ[x] - depth >= 250)? depth: 0;
    }
    //  convert projective position to real-world position
    kinect.getDepthGenerator().ConvertProjectiveToRealWorld(640, XnProject, XnWorld);
}
void ofApp::draw() {
    （... 変更なし ...）
}
void ofApp::keyPressed(int key) {
    if (key == 's') {
        unsigned short *depthData = kinect.getDepthRawPixels().getData();
        for (int x = 0; x < 640; x++)
            backZ[x] = depthData[horizonY * 640 + x];
    }
}
（... 以下変更なし ...）
図
ワールド座標系から任意の座標系への変換（ofxOpenNI）

Kinect で捉えた対象物のワールド座標を，任意の座標系（たとえば天井プロジェクタから見た座標系）に変換する方法を説明します．ここでは例題として，下図のように，床から 50mm の高さのところを「地平」として上述の地平線レーダを動作させ，対象物が床と接するところにプロジェクタからスポットライトを当てることを考えます．

図

上図のように，Kinect (K) から見た対象物の位置は，地平線レーダによって，X-Z 平面（Y=0）上の点 Q = (QX, 0, QZ) として計測できます．床面上でスポットライトを当てたい位置 Q' は (QX, −50, QZ) となります．いずれも Kinect からみたワールド座標系における位置です．

【座標系の平行移動と回転】

この位置 Q' を任意の位置・姿勢をもつプロジェクタ P から見ると，どのように見えるでしょうか．言い換えれば，ワールド座標系の座標 Q' は，プロジェクタ座標系ではどのような座標となるでしょうか．

図

座標系の回転については後回しとし，ここでは平行移動だけを考えることにします．ワールド座標系におけるプロジェクタの位置を P = (PX, PY, PZ) とするとき，プロジェクタ座標系から見た Q' は，Qp = Q' − P = (QX − PX, −50 − PY, QZ − PZ) となります．

図

上図のように，さらに回転を加えるとどうでしょうか．ここでは説明を簡単にするために，Xp 軸まわりの回転（いわゆるピッチ＝上下傾動）だけを考えます．プロジェクタを Xp 軸まわりに（右ネジ方向がプラス）θX だけ回転させた場合，このプロジェクタから見た対象物の位置 Qp' は，元の Qp を Xp 軸（= Xp' 軸）まわりに −θX だけ回転させた位置に来るはずです．これは Zp-Yp 平面上の原点 P まわりの回転となり，以下の式により求めることができます．

図
【プロジェクタ画像の生成】

プロジェクタ P から見た対象物の座標 (Qp'X, Qp'Y, Qp'Z) が得られれば，その位置にスポットライトを当てるのは比較的簡単です．つぎのプログラムを実行してみてください． 

#pragma once
#include "ofMain.h"
#include "ofxOpenNI.h"
class ofApp : public ofBaseApp{
private:
    ofxOpenNI kinect;
    XnPoint3D XnProject[640], XnWorld[640];
    int horizonY;
    unsigned short backZ[640];
    int drawMode;
    XnPoint3D move;
    float degX, sinX, cosX;
    float focalLength;
    ofPoint changeView(XnPoint3D Q);
public:
    （... 変更なし ...）
};

#include "ofApp.h"
void ofApp::setup() {
    //  window
    ofBackground(0, 0, 0);
    ofSetWindowShape(640, 480);
    ofSetFrameRate(30);
    （... 変更なし ...）
    //  background
    for (int x = 0; x < 640; x++)
        backZ[x] = 10000;
    //  display mode (0:depth, 1:OTH, 2:projection)
    drawMode = 0;
    //  set view parameters
    move.X =    0.0;
    move.Y = 1500.0;
    move.Z = -500.0;
    degX = -35.0;
    sinX = sin(-degX * M_PI / 180.0);
    cosX = cos(-degX * M_PI / 180.0);
    focalLength = 500.0;     //  tweak for your projector
}
void ofApp::update() {
    （... 変更なし ...）
}
ofPoint ofApp::changeView(XnPoint3D Q) {
    //  translate
    XnPoint3D Q1;
    Q1.X = Q.X - move.X;
    Q1.Y = Q.Y - move.Y;
    Q1.Z = Q.Z - move.Z;
    //  rotation:X
    XnPoint3D Q2;
    Q2.X = Q1.X;
    Q2.Y = Q1.Z * sinX + Q1.Y * cosX;
    Q2.Z = Q1.Z * cosX - Q1.Y * sinX;
    //  projection
    ofPoint S;
    S.x = 320.0 + (focalLength / Q2.Z) * Q2.X;
    S.y = 240.0 - (focalLength / Q2.Z) * Q2.Y;
    return S;
}
void ofApp::draw() {
    if (drawMode == 0) {
        //  depth image
        ofSetColor(255, 255, 255);
        kinect.drawDepth(0, 0, 640, 480);
        ofDrawLine(0, horizonY, 639, horizonY);
    }
    else if (drawMode == 1) {
        //  OTH radar
        //  draw grid
        ofSetColor(150, 150, 150);
        for (int x = -300; x <= 600; x += 100)
            ofDrawLine(x + 320, 0, x + 320, 479);
        for (int y = 0; y < 480; y += 100)
            ofDrawLine(0, 480 - y, 639, 480 - y);
        //  draw sensing area
        ofSetColor(200, 200, 0);
        float offset = 480 * tan(29.0 * M_PI / 180.0);
        ofDrawLine(320 - offset, 0, 320, 480);
        ofDrawLine(320 + offset, 0, 320, 480);
        //  draw radar
        ofSetColor(100, 200, 100);
        for (int x = 0; x < 640; x++) {
            XnPoint3D world = XnWorld[x];
            float scrX = world.X * 0.1 + 320, scrY = 479 - world.Z * 0.1;
            ofDrawCircle(scrX, scrY, 3);
        }
    }
    else if (drawMode == 2) {
        //  spotlights
        //  draw grid
        ofSetColor(150, 150, 150);
        for (int x = -3000; x <= 3000; x += 1000) {
            XnPoint3D A = {x, 0.0, 0.0};
            ofPoint Sa = changeView(A);
            XnPoint3D B = {x, 0.0, 6000.0};
            ofPoint Sb = changeView(B);
            ofDrawLine(Sa, Sb);
        }
        for (int z = 0; z <= 6000; z += 1000) {
            XnPoint3D A = {-3000.0, 0.0, z};
            ofPoint Sa = changeView(A);
            XnPoint3D B = { 3000.0, 0.0, z};
            ofPoint Sb = changeView(B);
            ofDrawLine(Sa, Sb);
        }
        //  draw spotlights
        ofSetColor(255, 255, 0);
        for (int x = 0; x < 640; x++) {
            XnPoint3D Q = XnWorld[x];
            Q.Y -= 50.0;
            ofPoint S = changeView(Q);
            ofDrawCircle(S, 3);
        }
    }
}
void ofApp::keyPressed(int key){
    if (key == 's') {
        unsigned short *depthData = kinect.getDepthRawPixels().getData();
        for (int x = 0; x < 640; x++)
            backZ[x] = depthData[horizonY * 640 + x];
    }
    else if (key == ' ') {
        //  0-1-2-0-1-2-0-1-...
        drawMode = (drawMode + 1) % 3;
    }
}

座標変換を担うのは ofApp::changeView() です．あらかじめ K から P への平行移動量（move.X, Y, Z）や回転角度 degX，そしてプロジェクタ画像を生成するスクリーンまでの距離 focalLength (f) を，ofApp::setup() の中で設定しておきます．focalLength の値は，プロジェクタの投影画角に応じて，個別に調整する必要があります．

図

これらのパラメタにもとづいて，対象物のワールド座標を引数として与えられた ofApp::changeView() は，平行移動（translation）や Xp 軸まわりの回転（rotation:X），そして仮想スクリーンへの投影（projection）を実行し，得られたスクリーン座標を返します．ofApp::draw() では，スペースキーで切り替えられる描画モードに応じて，距離画像（下図左上）や地平線レーダ画像（下図右上）のほか，対象物にスポットライトを当てるためのプロジェクタ画像（下図下段）を表示します．

図 図
図 図
【さらに改造】

まず X 軸まわりの回転角度（degX：初期値 −35度＝下向き35度）カーソルキーで変えられるように改造します．つぎのように ofApp::keyPressed() を書き換えてください．上下方向のカーソルキーを押すことで，degX を増減させ，同時に sinX, cosX を再計算しています． 

void ofApp::keyPressed(int key){
    if (key == 's') {
        unsigned short *depthData = kinect.getDepthRawPixels().getData();
        for (int x = 0; x < 640; x++)
            backZ[x] = depthData[horizonY * 640 + x];
    }
    else if (key == ' ') {
        drawMode = (drawMode + 1) % 3;
    }
    else if (key == OF_KEY_UP) {
        degX += 0.5;
        sinX = sin(-degX * M_PI / 180.0);
        cosX = cos(-degX * M_PI / 180.0);
    }
    else if (key == OF_KEY_DOWN) {
        degX -= 0.5;
        sinX = sin(-degX * M_PI / 180.0);
        cosX = cos(-degX * M_PI / 180.0);
    }
}
図 図

さらに Y 軸まわり（つまり水平旋回）の動きもプロジェクタに与えてみましょう．degY, sinY, cosY を変数とし，左右方向のカーソルキーに応答するようにします．そして，ofApp::changeView() に Y 軸まわりの回転変換を加えます．水平旋回をしてから上下傾動するようにします．いわゆる艦砲（gun-turret）方式です． 

#pragma once
#include "ofMain.h"
#include "ofxOpenNI.h"
class ofApp : public ofBaseApp{
private:
    （... 変更なし ...）
    float degX, sinX, cosX;
    float degY, sinY, cosY;
    float focalLength;
    ofPoint changeView(XnPoint3D Q);
public:
    （... 変更なし ...）
};

#include "ofApp.h"
void ofApp::setup() {
    （... 変更なし ...）
    degX = -35.0;
    sinX = sin(-degX * M_PI / 180.0);
    cosX = cos(-degX * M_PI / 180.0);
    degY = 0.0;
    sinY = sin(-degY * M_PI / 180.0);
    cosY = cos(-degY * M_PI / 180.0);
    focalLength = 500.0;     //  tweak for your projector
}
ofPoint ofApp::changeView(XnPoint3D Q) {
    //  translate
    XnPoint3D Q1;
    Q1.X = Q.X - move.X;
    Q1.Y = Q.Y - move.Y;
    Q1.Z = Q.Z - move.Z;
    //  rotation:Y
    XnPoint3D Q2;
    Q2.X = Q1.X * cosY - Q1.Z * sinY;
    Q2.Y = Q1.Y;
    Q2.Z = Q1.X * sinY + Q1.Z * cosY;
    //  rotation:X
    XnPoint3D Q3;
    Q3.X = Q2.X;
    Q3.Y = Q2.Z * sinX + Q2.Y * cosX;
    Q3.Z = Q2.Z * cosX - Q2.Y * sinX;
    //  projection
    ofPoint S;
    S.x = 320.0 + (focalLength / Q3.Z) * Q3.X;
    S.y = 240.0 - (focalLength / Q3.Z) * Q3.Y;
    return S;
}
void ofApp::update() {
    （... 変更なし ...）
}
void ofApp::draw() {
    （... 変更なし ...）
}
void ofApp::keyPressed(int key){
    （... 変更なし ...）
    else if (key == OF_KEY_DOWN) {
        degX -= 0.5;
        sinX = sin(-degX * M_PI / 180.0);
        cosX = cos(-degX * M_PI / 180.0);
    }
    else if (key == OF_KEY_LEFT) {
        degY += 0.5;
        sinY = sin(-degY * M_PI / 180.0);
        cosY = cos(-degY * M_PI / 180.0);
    }
    else if (key == OF_KEY_RIGHT) {
        degY -= 0.5;
        sinY = sin(-degY * M_PI / 180.0);
        cosY = cos(-degY * M_PI / 180.0);
    }
}
図 図

ちなみに，sinX, cosX, sinY, cosY は ofApp::setup() や ofApp::keyPressed() の中で１回だけ計算しています．ofApp::draw() の中で ofApp::changeView() を繰り返し呼び出していますが，その内部で毎回 sin(), cos() を計算しなおす必要はありません．これにより，高速な座標変換を実現しています．