調べたこと、作ったことをメモしています。
こちらに移行中: https://blog.shimazu.me/

Xtionでの人検知

regular term6 eledev xtion OpenNI OpenCV

OpenNIのライブラリ使っただけですが、座標を取ることができました。

あとは、中心からの変位によってカメラの角度をずらせば簡単に追従できそうです。

後ろとか向いていてもわりと正しく検知してくれるみたいでちょっと驚きました。

imageimage

ソースはこんな感じ。

#include <iostream>
#include <stdexcept>
#include <vector>

#include <opencv2/opencv.hpp>

#include <XnCppWrapper.h>

using namespace std;

// ユーザー検出
void XN_CALLBACK_TYPE UserDetected( xn::UserGenerator& generator, XnUserID nId, void* pCookie )
{
    std::cout << "ユーザー検出:" << nId << " " << generator.GetNumberOfUsers() << "人目" << std::endl;

    generator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}

// キャリブレーションの終了
void XN_CALLBACK_TYPE CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie)
{
    // キャリブレーション成功
    if ( bSuccess ) {
        std::cout << "キャリブレーション成功。ユーザー:" << nId << std::endl;
        capability.StartTracking(nId);
    }
    // キャリブレーション失敗
    else {
        std::cout << "キャリブレーション失敗。ユーザー:" << nId << std::endl;
    }
}

typedef struct {
    // char *title;
    // IplImage *view;
    xn::Context *context;
    xn::ImageGenerator ℑ
    xn::DepthGenerator &depth;
    xn::UserGenerator  &user;
    xn::ImageMetaData  &imageMD;
    xn::DepthMetaData  &depthMD;
    xn::SceneMetaData  &sceneMD;
} capthread_t;

void *capThread( void *tmp )
{
    capthread_t *cap = ( capthread_t * ) tmp;
    // ::cvShowImage( cap->title, cap->view );
    cap->context->WaitAnyUpdateAll();
    cap->image.GetMetaData( cap->imageMD );
    cap->depth.GetMetaData( cap->depthMD );
    cap->user.GetUserPixels( 0, cap->sceneMD );
}


int main (int argc, char * argv[])
{
    try {
        cv::Ptr< IplImage > cameraCap = 0;
        cv::Ptr< IplImage > depthCap = 0;
        cv::Ptr< IplImage > cameraProc = 0;
        cv::Ptr< IplImage > depthProc = 0;
        cout << "SkeletonDetector is started!" << endl;
        // コンテキストの初期化
        xn::Context context;
        XnStatus rc = context.InitFromXmlFile("Config.xml");
        if (rc != XN_STATUS_OK) {
            throw std::runtime_error(xnGetStatusString(rc));
        }
        cout << "Initialized from XML is finished." << endl;
        
        // イメージジェネレータの作成
        xn::ImageGenerator image;
        rc = context.FindExistingNode(XN_NODE_TYPE_IMAGE, image);
        if (rc != XN_STATUS_OK) {
            throw std::runtime_error(xnGetStatusString(rc));
        }
        cout << "ImageGenerator is created." << endl;

        // デプスジェネレータの作成
        xn::DepthGenerator depth;
        rc = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
        if (rc != XN_STATUS_OK) {
            throw std::runtime_error(xnGetStatusString(rc));
        }
        cout << "DepthGenerator is created." << endl;

        // デプスの座標をイメージに合わせる
        depth.GetAlternativeViewPointCap().SetViewPoint(image);

        // ユーザーの作成
        xn::UserGenerator user;
        rc = context.FindExistingNode( XN_NODE_TYPE_USER, user );
        if ( rc != XN_STATUS_OK ) {
            throw std::runtime_error( xnGetStatusString( rc ) );
        }
        cout << "UserGenerator is created." << endl;

        // スケルトン・トラッキングをサポートしているか確認
        if (!user.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {
            throw std::runtime_error("ユーザー検出をサポートしてません");
        }
        cout << "Skeleton Tracking is Supported." << endl;

        // キャリブレーションにポーズが必要
        xn::SkeletonCapability skeleton = user.GetSkeletonCap();
        if ( skeleton.NeedPoseForCalibration() ) {
            throw std::runtime_error("最新のOpenNIをインストールしてください");
        }
        cout << "Pose isn't needed for the calibration." << endl;

        // ユーザー認識のコールバックを登録
        // キャリブレーションのコールバックを登録
        XnCallbackHandle userCallbacks, calibrationCallbacks;
        user.RegisterUserCallbacks(&::UserDetected, 0, 0, userCallbacks);
        skeleton.RegisterCalibrationCallbacks( 0, &::CalibrationEnd, 0, calibrationCallbacks );
        cout << "Calibration Callback is set." << endl;

        // ユーザートラッキングで、すべてをトラッキングする
        //XN_SKEL_PROFILE_ALL           すべてをトラッキングする
        //XN_SKEL_PROFILE_UPPER         上半身をトラッキングする
        //XN_SKEL_PROFILE_LOWER         下半身をトラッキングする
        //XN_SKEL_PROFILE_HEAD_HANDS    頭と手をトラッキングする
        // skeleton.SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
        skeleton.SetSkeletonProfile(XN_SKEL_PROFILE_UPPER);
        cout << "Skeleton Profile is set." << endl;

        // カメラサイズのイメージを作成(8bitのRGB)
        XnMapOutputMode outputMode;
        image.GetMapOutputMode(outputMode);
        cameraProc = ::cvCreateImage(cvSize(outputMode.nXRes, outputMode.nYRes), IPL_DEPTH_8U, 3);
        cout << "xres:: " << outputMode.nXRes << "  yres:: " << outputMode.nYRes << endl;
        if (!cameraProc) {
            throw std::runtime_error("error : cvCreateImage");
        }
        cout << "Camera Image is created" << endl;

        depthProc = ::cvCreateImage(cvSize(outputMode.nXRes, outputMode.nYRes), IPL_DEPTH_8U, 1);
        if (!depthProc) {
            throw std::runtime_error("error : cvCreateImage depthProc");
        }
        
        
        cout << "All initialization is finished" << endl;
        
        // はじめのデータ
        // cameraCap = cvCloneImage( cameraProc );
        // depthCap = cvCloneImage( depthProc );
        // 構造体初期化
        // capthread_t camThreadParam = { .title = "Xtion Camera", .view = cameraCap };
        // capthread_t depThreadParam = { .title = "Xtion Depth",  .view = depthCap  };
        
        
        context.WaitAnyUpdateAll();
        
        // 画像データの取得
        xn::ImageMetaData imageMD;
        image.GetMetaData( imageMD );
        // 深度データの取得
        xn::DepthMetaData depthMD;
        depth.GetMetaData( depthMD );
        // ユーザーデータの取得
        xn::SceneMetaData sceneMD;
        user.GetUserPixels( 0, sceneMD );

        // カメラ画像の保存
        memcpy( cameraProc->imageData, imageMD.Data(), cameraProc->imageSize );
        // 深度画像の保存
        int xsize = depthProc->width;
        int ysize = depthProc->height;
        int step  = depthProc->widthStep / xsize;
        for ( int i = 0; i < ysize; i++ ) {
            for ( int j = 0; j < xsize; j++ ) {
                int cur =  i*depthProc->widthStep + j ;
                if ( cur * step > depthProc->imageSize ) {
                    throw std::runtime_error( "error: buffer overrun" );
                }
                uint16_t tmp = depthMD[ cur * step ];
                tmp >>= 8;
                // cout << tmp << endl;
                depthProc->imageData[ cur * step ] = ( uint8_t ) tmp * 10;
            }
        }

        capthread_t capThreadParam = {
            .context = &context,
            .image = image,
            .depth = depth,
            .user  = user,
            .imageMD = imageMD,
            .depthMD = depthMD,
            .sceneMD = sceneMD
        };
        
        
        // メインループ
        while ( cvWaitKey(10) != 'q' ) {
            


            // カメラ画像の保存
            memcpy( cameraProc->imageData, imageMD.Data(), cameraProc->imageSize );
            // 深度画像の保存
            for ( int i = 0; i < ysize; i++ ) {
                for ( int j = 0; j < xsize; j++ ) {
                    int cur =  i*depthProc->widthStep + j ;
                    if ( cur * step > depthProc->imageSize ) {
                        throw std::runtime_error( "error: buffer overrun" );
                    }
                    uint16_t tmp = depthMD[ cur * step ];
                    tmp >>= 8;
                    // cout << tmp << endl;
                    depthProc->imageData[ cur * step ] = ( uint8_t ) tmp * 10;
                }
            }

            // 待ちスレッド立ち上げ
            pthread_t camThread;
            pthread_create( &camThread, NULL, capThread, &capThreadParam );
            
            
            
            // スケルトンの描画
            string skelname[] = {
                "HEAD",
                "NECK",
                "TORSO",
                "WAIST",
                "LEFT_COLLAR",
                "LEFT_SHOULDER",
                "LEFT_ELBOW",
                "LEFT_WRIST",
                "LEFT_HAND",
                "LEFT_FINGERTIP",
                "RIGHT_COLLAR",
                "RIGHT_SHOULDER",
                "RIGHT_ELBOW",
                "RIGHT_WRIST",
                "RIGHT_HAND",
                "RIGHT_FINGERTIP",
                "LEFT_HIP",
                "LEFT_KNEE",
                "LEFT_ANKLE",
                "LEFT_FOOT",
                "RIGHT_HIP",
                "RIGHT_KNEE",
                "RIGHT_ANKLE",
                "RIGHT_FOOT"
            };
            XnUserID users[15];
            XnUInt16 userCount = 15;
            user.GetUsers(users, userCount);
            for (int i = 0; i < userCount; ++i) {
                if ( !skeleton.IsTracking( users[i] ) ) {
                    continue;
                }
                if ( !skeleton.IsJointAvailable( XN_SKEL_NECK ) ) {
                    cerr << "user[" << i << "]'s Neck is unavailable!" << endl;
                    continue;
                }
                // 各箇所の座標を取得する
                XnSkeletonJointPosition joint;
                skeleton.GetSkeletonJointPosition( users[i], XN_SKEL_NECK, joint);
                // 確かさチェック
                if ( joint.fConfidence < 0.5 ) {
                    cerr << "user[" << i << "]'s Neck is unconfidence!" << endl;
                    continue;
                }
                // 座標を変換する
                XnPoint3D pt = joint.position;
                depth.ConvertRealWorldToProjective( 1, &pt, &pt );
                cvCircle( cameraProc, cvPoint(pt.X, pt.Y), 10, cvScalar( 255, 0, 0 ), -1 );
                // 座標表示
                cout << "user[ " << i << " ]: x= " << pt.X << " y= " << pt.Y << " ";
                int diffx = pt.X - xsize/2;
                cout << "diff= " << diffx << endl;
                // for ( int j = (int)XN_SKEL_HEAD; j <= (int)XN_SKEL_RIGHT_FOOT; ++j ) {
                //     if ( !skeleton.IsJointAvailable( (XnSkeletonJoint)j ) ) {
                //         continue;
                //     }

                //     // 各箇所の座標を取得する
                //     XnSkeletonJointPosition joint;
                //     skeleton.GetSkeletonJointPosition(users[i], (XnSkeletonJoint)j, joint);
                //     if ( joint.fConfidence < 0.5 ) {
                //         continue;
                //     }

                //     // 座標を変換する
                //     XnPoint3D pt = joint.position;
                //     depth.ConvertRealWorldToProjective( 1, &pt, &pt );
                //     cvCircle( cameraProc, cvPoint(pt.X, pt.Y), 10, cvScalar( 255, 0, 0 ), -1 );
                //     cout << "user[" << i << "]'s " << skelname[ j ] << " is (" << pt.X << ", " << pt.Y << ") " << endl;
                // }
            }

            ::cvCvtColor(cameraProc, cameraProc, CV_RGB2BGR);
            ::cvShowImage("Xtion Camera", cameraProc);
            ::cvShowImage("Xtion Depth", depthProc);

            pthread_join( camThread, NULL );

        }
    }
    catch (std::exception& ex) {
        std::cout << "Exception:: " << ex.what() << std::endl;
    }

    return 0;
}