2011年11月12日土曜日

Kinect OpenNIによるWindowsでのマルチキネクト - UDP

Kinectを複数使う場合、Macならマルチキネクト(Multi-Kinect)が可能です。

しかし、Windows環境ではまだドライバが対応しておらず、マルチキネクト(Multi-Kinect)を行うことができません。
できるみたいです。
USB端子の大元?が異なる端子で繋げば出来るみたい。
ex)デスクトップPCの前面パネルと後面パネルのUSB端子


ドライバが対応してないなら、違うPCで動かせばいいじゃない

っということで取得したKinectの画像を送受信してみます。


今回はUDPによって送受信を行います。

UDPはTCPと違い、相手に正しくデータが届くとは限りません。
届いたかどうかや誤りの確認をしていないからです。
そのかわり、高速でデータを送信することが可能となります。
よく、音声や画像のストリーム形式での配信に用いられるプロトコルです。


まず、Kinectから取得できるRGB画像と深度画像のサイズを計算してみます。

RGB画像
8bit (1byte)
3Channel    ->   921600byte   ->   900KB
480 * 640

深度画像
16bit (2byte)
1Channel    ->   614400byte   ->   600KB
480 * 640

合計 1500KB

RGB画像と深度画像のサイズは1500KB(約1.5MB)となります。


結構大きいですね・・・


では、このデータをUDPで送信することになりますが、UDPはこのような大きなサイズを一気に送ることはできません。
なので分割して送ります。

大体UDPで送ることができる最大サイズは60KBくらいなので、60KBで分割します。

RGB画像:900KB / 60KB = 15分割
深度画像:600KB / 60KB = 10分割


このことを頭において、プログラムを作ると



送信
/*
    送信
    ServerName    : IPアドレス
    PortNo        : ポート番号
    image        : 送信するRGBイメージ
    depth        : 送信する深度イメージ
*/
int sendKinectImage(char*ServerName,unsigned short PortNo,Mat &image,Mat &depth){
    HOSTENT*pHostent=gethostbyname(ServerName);
    if(pHostent==NULL){
        DWORD addr=inet_addr(ServerName);
        pHostent=gethostbyaddr((char*)&addr,4,AF_INET);
    }
    if(pHostent==NULL) return -1;//サーバーが見つかりません
  
    SOCKET s = socket(AF_INET,SOCK_DGRAM,0);
    if(s<0) return -2;//ソケットエラー
  
    SOCKADDR_IN sockaddrin;
    memset(&sockaddrin,0,sizeof(sockaddrin));
    memcpy(&(sockaddrin.sin_addr),pHostent->h_addr_list[0],pHostent->h_length);
    sockaddrin.sin_family = AF_INET;
    sockaddrin.sin_port = htons(PortNo);

    //RGBイメージ
    static const int sendSize = 61440;    //1度に送信する画像のバッファサイズ:60KB(61440byte)
    static char buff[sendSize + 2];            //送信するバッファ(ヘッダ部 + 画像データ)
    static const int divImageNum = (image.rows * image.step) / sendSize;    //RGB画像の分割数
    static const int divDepthNum = (depth.rows * depth.step) / sendSize;    //深度画像の分割数

    //RGBイメージ
    for(int i = 0;i < divImageNum;i++){
        //ヘッダ部の入力
        buff[0] = 'R';    //RGB画像だという識別記号
        buff[1] = i;    //分割された何番目か
        //画像データの入力
        memcpy(&buff[2],&image.data[sendSize * i],sendSize);
        sendto(s,buff,sendSize + 2,4,(LPSOCKADDR)&sockaddrin,sizeof(sockaddrin));

    }
    //Depthイメージ
    for(int i = 0;i < divDepthNum;i++){  
        //ヘッダ部の入力
        buff[0] = 'D';    //Depth画像だという識別記号
        buff[1] = i;    //分割された何番目か
        //画像データの入力
        memcpy(&buff[2],&depth.data[sendSize * i],sendSize);
        sendto(s,buff,sendSize + 2,4,(LPSOCKADDR)&sockaddrin,sizeof(sockaddrin));
    }
    closesocket(s);
    return 0;    //成功
}




送信するためにbuffという配列を用意します。

buffには画像に関する情報のヘッダ部と画像データを格納します。


ここでヘッダ部では
buff[0]にRGB画像か深度画像か判別する情報を入れます
つぎに
buff[1]にこのデータが何分割目のデータを格納します。
なぜこのようなことをするのかというと、UDPは送信したデータが相手に届くかわからず、また順番も送信した順になるとはかぎりません。
なので、受信側が分割されたデータを1つの画像に復元する際、必要になります。



データの送信はsendtoで行います


次に受信


受信

/*
    受信
    PortNo        : ポート番号
    image        : 受信したRGBイメージを格納する変数
    depth        : 受信した深度イメージを格納する変数
*/
int receiveKinectImage(unsigned short PortNo,Mat &image,Mat &depth){  
    SOCKET s = socket(AF_INET,SOCK_DGRAM,0);
    if(s<0) return -1;//ソケットエラー
  
    SOCKADDR_IN sockaddrin;
    memset(&sockaddrin,0,sizeof(sockaddrin));
    sockaddrin.sin_family = AF_INET;
    sockaddrin.sin_port = htons(PortNo);
    sockaddrin.sin_addr.S_un.S_addr = INADDR_ANY;
    if(SOCKET_ERROR == bind(s,(LPSOCKADDR)&sockaddrin,(int)sizeof(sockaddrin))){
        closesocket(s);
        return -2;//bindエラー
    }
    SOCKADDR_IN from;
    int fromlen=(int)sizeof(from);

    static const int receiveSize = 61440;    //1度に受信する画像のバッファサイズ        60KB(61440byte)
    static char buff[receiveSize + 2];            //受信するバッファ(ヘッダ部 + 画像データ)
    static const int divImageNum = (image.rows * image.step) / receiveSize;    //RGB画像の分割数
    static const int divDepthNum = (depth.rows * depth.step) / receiveSize;    //深度画像の分割数
    static int re;
    static char header;
    static char divNum;
    static int identificationNum;
    static char imageOrDepth;

    re = recvfrom(s,buff,receiveSize + 2,0,(SOCKADDR*)&from,&fromlen);//受信するまでここで停止
    if(re!=SOCKET_ERROR){//エラーで無ければ表示
        imageOrDepth = buff[0];    //RGB画像か深度画像か
        divNum = buff[1];        //分割番号

        if(imageOrDepth == 'R')        //RGBイメージ
            memcpy(&image.data[receiveSize * divNum],&buff[2],receiveSize);

        else if(imageOrDepth == 'D')    //Depthイメージ
            memcpy(&depth.data[receiveSize * divNum],&buff[2],receiveSize);
    }
  
    closesocket(s);
    return 0;
}


ヘッダ部の情報を元に、RGB画像か深度画像のどちらにデータを格納するか選び
分割番号を元に、格納するデータの位置を決定します。



これでKinectの画像データを他のPCに送ることができます。



データの受信はrecvfromで行います
ちなみにrecvfromはデータを受信するまで、待ち状態になります。
利用する際は、受信は別スレッドで動作させましょう。


ここではUDPのくわしいプログラムは説明しません。

各自調べてください。



これでWindows環境でもマルチキネクト(Multi-Kinect) が可能に!?

骨格情報に関しては紹介しませんでしたが、同じような感じでできるんじゃないでしょうか。




ということで、サンプルプログラムどん

#include <winsock2.h>
#pragma comment(lib, "wsock32.lib")
#include <opencv2/opencv.hpp>
#ifdef _DEBUG
    //Debugモードの場合
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_core220d.lib")            // opencv_core
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_imgproc220d.lib")        // opencv_imgproc
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_highgui220d.lib")        // opencv_highgui
#else
    //Releaseモードの場合
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_core220.lib")            // opencv_core
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_imgproc220.lib")        // opencv_imgproc
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_highgui220.lib")        // opencv_highgui
#endif
#include <XnCppWrapper.h>
#pragma comment(lib,"C:/Program files/OpenNI/Lib/openNI.lib")

#define SAMPLE_XML_PATH "C:/Program Files/OpenNI/Data/SamplesConfig.xml"
using namespace cv;
using namespace xn;

//受信スレッド
DWORD WINAPI ThreadFunc(LPVOID vdParam);
int sendKinectImage(char*ServerName,unsigned short PortNo,Mat &image,Mat &depth);    //送信
int receiveKinectImage(unsigned short PortNo,Mat &image,Mat &depth);                //受信

//ポート番号
#define PORT 1000
//IPアドレス(送信するPCのIPアドレスを指定してください)
#define SEND_IPADDRESS "192.168.12.55"
//送信するか受信するか
enum Type{SEND,RECEIVE};  


Mat receiveImage(480,640,CV_8UC3);        //受信RGB画像
Mat receiveDepth(480,640,CV_16UC1);        //受信深度画像

int main()
{
    //OpenNI
    DepthGenerator depthGenerator;
    ImageGenerator imageGenerator;
    DepthMetaData depthMD;
    ImageMetaData imageMD;
    Context context;

    //OpenCV
    Mat image(480,640,CV_8UC3);            //RGB画像
    Mat depth(480,640,CV_16UC1);        //深度画像
  
    //OpenNIの初期化
    context.InitFromXmlFile(SAMPLE_XML_PATH);
    context.FindExistingNode(XN_NODE_TYPE_DEPTH, depthGenerator);
    context.FindExistingNode(XN_NODE_TYPE_IMAGE, imageGenerator);
  
    //RGB画像と振動画像のズレを補正
    depthGenerator.GetAlternativeViewPointCap().SetViewPoint(imageGenerator);

    //送信するか受信するか
    //Type sendOrReceive = SEND;    //送信する
    Type sendOrReceive = RECEIVE;    //受信する
  
    DWORD dwID;    //スレッドのID
    if(sendOrReceive == RECEIVE){
        //受信スレッド起動
        CreateThread(NULL , 0 , ThreadFunc , 0 , 0 , &dwID);
        cvNamedWindow("receiveImage");    //ウィンドウの準備
        cvNamedWindow("receiveDepth");
    }
  
    //ウィンドウの準備
    cvNamedWindow("image");
    cvNamedWindow("depth");
    int key = 0;
    while (key!='q'){
        context.WaitAndUpdateAll();
       
        imageGenerator.GetMetaData(imageMD);
        depthGenerator.GetMetaData(depthMD);
       
        memcpy(image.data,imageMD.Data(),image.step * image.rows);    //イメージデータを格納
        memcpy(depth.data,depthMD.Data(),depth.step * depth.rows);    //深度データを格納
       
        //BGRをRGBへ
        cvtColor(image,image,CV_RGB2BGR);

        //画面に表示
        imshow("image",image);
        imshow("depth",depth);
       
        if(sendOrReceive == SEND){            //送信          
            sendKinectImage(SEND_IPADDRESS,PORT,image,depth);
        }
        else if(sendOrReceive == RECEIVE){    //受信
            //受信した画像を表示する
            imshow("receiveImage",receiveImage);
            imshow("receiveDepth",receiveDepth);
        }

        key = waitKey(33);
    }
    context.Shutdown();
    return 0;
}

/*
    送信
    ServerName    : IPアドレス
    PortNo        : ポート番号
    image        : 送信するRGBイメージ
    depth        : 送信する深度イメージ
*/
int sendKinectImage(char*ServerName,unsigned short PortNo,Mat &image,Mat &depth){
    HOSTENT*pHostent=gethostbyname(ServerName);
    if(pHostent==NULL){
        DWORD addr=inet_addr(ServerName);
        pHostent=gethostbyaddr((char*)&addr,4,AF_INET);
    }
    if(pHostent==NULL) return -1;//サーバーが見つかりません
  
    SOCKET s = socket(AF_INET,SOCK_DGRAM,0);
    if(s<0) return -2;//ソケットエラー
  
    SOCKADDR_IN sockaddrin;
    memset(&sockaddrin,0,sizeof(sockaddrin));
    memcpy(&(sockaddrin.sin_addr),pHostent->h_addr_list[0],pHostent->h_length);
    sockaddrin.sin_family = AF_INET;
    sockaddrin.sin_port = htons(PortNo);

    //RGBイメージ
    static const int sendSize = 61440;    //1度に送信する画像のバッファサイズ:60KB(61440byte)
    static char buff[sendSize + 2];            //送信するバッファ(ヘッダ部 + 画像データ)
    static const int divImageNum = (image.rows * image.step) / sendSize;    //RGB画像の分割数
    static const int divDepthNum = (depth.rows * depth.step) / sendSize;    //深度画像の分割数

    //RGBイメージ
    for(int i = 0;i < divImageNum;i++){
        //ヘッダ部の入力
        buff[0] = 'R';    //RGB画像だという識別記号
        buff[1] = i;    //分割された何番目か
        //画像データの入力
        memcpy(&buff[2],&image.data[sendSize * i],sendSize);
        sendto(s,buff,sendSize + 2,4,(LPSOCKADDR)&sockaddrin,sizeof(sockaddrin));

    }
    //Depthイメージ
    for(int i = 0;i < divDepthNum;i++){  
        //ヘッダ部の入力
        buff[0] = 'D';    //Depth画像だという識別記号
        buff[1] = i;    //分割された何番目か
        //画像データの入力
        memcpy(&buff[2],&depth.data[sendSize * i],sendSize);
        sendto(s,buff,sendSize + 2,4,(LPSOCKADDR)&sockaddrin,sizeof(sockaddrin));
    }
    closesocket(s);
    return 0;    //成功
}
/*
    受信
    PortNo        : ポート番号
    image        : 受信したRGBイメージを格納する変数
    depth        : 受信した深度イメージを格納する変数
*/
int receiveKinectImage(unsigned short PortNo,Mat &image,Mat &depth){  
    SOCKET s = socket(AF_INET,SOCK_DGRAM,0);
    if(s<0) return -1;//ソケットエラー
  
    SOCKADDR_IN sockaddrin;
    memset(&sockaddrin,0,sizeof(sockaddrin));
    sockaddrin.sin_family = AF_INET;
    sockaddrin.sin_port = htons(PortNo);
    sockaddrin.sin_addr.S_un.S_addr = INADDR_ANY;
    if(SOCKET_ERROR == bind(s,(LPSOCKADDR)&sockaddrin,(int)sizeof(sockaddrin))){
        closesocket(s);
        return -2;//bindエラー
    }
    SOCKADDR_IN from;
    int fromlen=(int)sizeof(from);

    static const int receiveSize = 61440;    //1度に受信する画像のバッファサイズ        60KB(61440byte)
    static char buff[receiveSize + 2];            //受信するバッファ(ヘッダ部 + 画像データ)
    static const int divImageNum = (image.rows * image.step) / receiveSize;    //RGB画像の分割数
    static const int divDepthNum = (depth.rows * depth.step) / receiveSize;    //深度画像の分割数
    static int re;
    static char header;
    static char divNum;
    static int identificationNum;
    static char imageOrDepth;

    re = recvfrom(s,buff,receiveSize + 2,0,(SOCKADDR*)&from,&fromlen);//受信するまでここで停止
    if(re!=SOCKET_ERROR){//エラーで無ければ表示
        imageOrDepth = buff[0];    //RGB画像か深度画像か
        divNum = buff[1];        //分割番号

        if(imageOrDepth == 'R')        //RGBイメージ
            memcpy(&image.data[receiveSize * divNum],&buff[2],receiveSize);

        else if(imageOrDepth == 'D')    //Depthイメージ
            memcpy(&depth.data[receiveSize * divNum],&buff[2],receiveSize);
    }
  
    closesocket(s);
    return 0;
}
//受信スレッド
DWORD WINAPI ThreadFunc(LPVOID vdParam) {
    while (TRUE) {
        receiveKinectImage(PORT,receiveImage,receiveDepth);
    }
}






参考
プログラミング色々


2011年11月11日金曜日

Kinect OpenNIによる人物領域の抽出 - OpenCV

OpenNIでは人物の領域をマスク画像として取得することができます。

そこで、OpenCVのMatを用いてマスク画像から人物のRGB画像、深度画像を取得してみます。

あと、ついでに背景も抽出してます。

まず、 「SamplesConfig.xml」を編集します。

「<Node type="User" name="User1"/>」を追加しましょう。



<OpenNI>
    <Licenses>
        <!-- Add licenses here
        <License vendor="vendor" key="key"/>
        -->
    </Licenses>
    <Log writeToConsole="false" writeToFile="false">
        <!-- 0 - Verbose, 1 - Info, 2 - Warning, 3 - Error (default) -->
        <LogLevel value="3"/>
        <Masks>
            <Mask name="ALL" on="true"/>
        </Masks>
        <Dumps>
        </Dumps>
    </Log>
    <ProductionNodes>
        <Node type="Depth" name="Depth1">
            <Configuration>
                <Mirror on="true"/>
            </Configuration>
        </Node>
        <Node type="Image" name="Image1" stopOnError="false">
            <Configuration>
                <Mirror on="true"/>
            </Configuration>
        </Node>
        <Node type="User" name="User1"/>
        <!--
        <Node type="Audio" name="Audio1"/>
        -->
    </ProductionNodes>
</OpenNI>

これでユーザ検出が可能となりました。


てことでサンプルどん

#include <opencv2/opencv.hpp>
#ifdef _DEBUG
    //Debugモードの場合
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_core220d.lib")            // opencv_core
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_imgproc220d.lib")        // opencv_imgproc
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_highgui220d.lib")        // opencv_highgui
#else
    //Releaseモードの場合
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_core220.lib")            // opencv_core
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_imgproc220.lib")        // opencv_imgproc
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_highgui220.lib")        // opencv_highgui
#endif
#include <XnCppWrapper.h>
#pragma comment(lib,"C:/Program files/OpenNI/Lib/openNI.lib")

#define SAMPLE_XML_PATH "C:/Program Files/OpenNI/Data/SamplesConfig.xml"
using namespace cv;
using namespace xn;

int main()
{
    //OpenNI
    DepthGenerator depthGenerator;
    ImageGenerator imageGenerator;
    UserGenerator userGenerator;
    DepthMetaData depthMD;
    ImageMetaData imageMD;
    SceneMetaData sceneMD;   
    Context context;

    //OpenCV
    Mat image(480,640,CV_8UC3);            //RGB画像
    Mat depth(480,640,CV_16UC1);        //深度画像
    Mat mask(480,640,CV_16UC1);            //プレイヤーマスク画像
    Mat player(480,640,CV_8UC3);        //人間画像
    Mat playerDepth(480,640,CV_16UC1);    //人間画像
    Mat background(480,640,CV_8UC3);    //背景画像
    Mat useMask(480,640,CV_16UC1);        //使用するマスク
   
    int maskSize = mask.step * mask.rows;    //マスク画像の配列数
   
    //OpenNIの初期化
    context.InitFromXmlFile(SAMPLE_XML_PATH);
    context.FindExistingNode(XN_NODE_TYPE_DEPTH, depthGenerator);
    context.FindExistingNode(XN_NODE_TYPE_IMAGE, imageGenerator);
    context.FindExistingNode(XN_NODE_TYPE_USER, userGenerator);
   
    //RGB画像と振動画像のズレを補正
    depthGenerator.GetAlternativeViewPointCap().SetViewPoint(imageGenerator);
   
    //ウィンドウの準備
    cvNamedWindow("image");
    cvNamedWindow("depth");
    cvNamedWindow("player");
    cvNamedWindow("playerDepth");
    cvNamedWindow("background");

    int key = 0;
    while (key!='q'){
        context.WaitAndUpdateAll();
       
        imageGenerator.GetMetaData(imageMD);
        depthGenerator.GetMetaData(depthMD);
        userGenerator.GetUserPixels(0,sceneMD);    //ユーザピクセル取得
       
        memcpy(image.data,imageMD.Data(),image.step * image.rows);    //イメージデータを格納
        memcpy(depth.data,depthMD.Data(),depth.step * depth.rows);    //深度データを格納
       
        //BGRをRGBへ
        cvtColor(image,image,CV_RGB2BGR);

        player = 0;                                //初期化
        playerDepth = 0;                        //初期化       
        memcpy(mask.data,sceneMD.Data(),maskSize); //マスクデータをコピー       
        mask.convertTo(useMask,CV_8UC1);        //マスクの変換
        image.copyTo(player,useMask);            //マスクを利用した人物抽出
        depth.copyTo(playerDepth,useMask);        //マスクを利用した人物奥行き抽出
        background = image - player;            //背景のみ取得

        //画面に表示
        imshow("image",image);
        imshow("depth",depth);
        imshow("player",player);
        imshow("playerDepth",playerDepth);
        imshow("background",background);

        key = waitKey(33);
    }
    context.Shutdown();
    return 0;
}


人物のRGBデータと深度データをマスクを用いて抽出しています。

マスク画像の取得は
・userGenerator
・sceneMD
この2つを用いて取得します。

取得したマスク画像を格納するOpenCVのMatクラスはMat mask(480,640,CV_16UC1);です。


では、OpenNIからマスク画像の取得方法です。


userGenerator.GetUserPixels(0,sceneMD);    //ユーザピクセル取得

これでsceneMDにマスク画像が格納されました。

次にsceneMDからmaskにデータを格納します。



memcpy(mask.data,sceneMD.Data(),maskSize);

sceneMD.Data()でマスクの配列が取得できるので、その値をそのままmaskにコピーしています。


もし、Matを使わずに取得したい場合は

g_UserGenerator.GetUserPixels(0,sceneMD);    //ピクセル取得
const XnLabel* pLabels = sceneMD.Data();
if(pLabels == NULL){
        return;
}
 これでpLabelにアクセスすることで可能です。

1ピクセルだけでいいなら
sceneMD(x,y)
 でいいのかな。




次にマスクから人物領域を取得する方法

player = 0;            //初期化
playerDepth = 0;    //初期化   
memcpy(mask.data,sceneMD.Data(),maskSize);    //マスクデータをコピー      mask.convertTo(useMask,CV_8UC1);            //マスクの変換
image.copyTo(player,useMask);                //マスクを利用した人物抽出
depth.copyTo(playerDepth,useMask);            //マスクを利用した人物奥行き抽出
background = image - player;                //背景のみ取得

まず最初に前回抽出した人物画像を初期化します。

次にマスクデータをコピー

そしてつぎ、コピーしたマスクデータのままではcopyToがうまくできないので変換します

そしてRGB画像と深度画像からそれぞれ人物領域のみを抽出し、コピーします。

で、あとはRGB画像から人物画像を引くと、背景のみ取得できます。



この方法ならピクセル一つ一つにアクセスしていないので高速に人物領域の画像を取得することができます。