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);
    }
}






参考
プログラミング色々


0 件のコメント:

コメントを投稿