nuitrack_gl_sample/src/main.cpp

OpenGL 視覚化のサンプルです。

サンプルの説明:Nuitrack OpenGL サンプル

nuitrack_gl_sample/src/NuitrackGLSample.h:

#ifndef NUITRACKGLSAMPLE_H_
#define NUITRACKGLSAMPLE_H_

#include <nuitrack/Nuitrack.h>

#ifdef _WIN32
#include <Windows.h>
#endif

#ifndef ANDROID
#include <GL/gl.h>
#else
#include <GLES/gl.h>
#endif

#include <string>


typedef enum
{
    DEPTH_SEGMENT_MODE = 0,
    RGB_MODE = 1,
    MODES_MAX_COUNT
} ViewMode;



// Main class of the sample
class NuitrackGLSample final
{
public:
    NuitrackGLSample();
    ~NuitrackGLSample();
    
    // Initialize sample: initialize Nuitrack, create all required modules,
    // register callbacks and start Nuitrack
    void init(const std::string& config = "");
    
    // Update the depth map, tracking and gesture recognition data,
    // then redraw the view
    bool update();
    
    // Release all sample resources
    void release();

    void nextViewMode()
    {
        _viewMode = (ViewMode)(((uint32_t)_viewMode + 1) % _modesNumber);
    }

    tdv::nuitrack::OutputMode getOutputMode() const
    {
        return _outputMode;
    }

private:
    int _width, _height;
    
    // GL data
    GLuint _textureID;
    uint8_t* _textureBuffer;
    GLfloat _textureCoords[8];
    GLfloat _vertexes[8];
    std::vector<GLfloat> _lines;
    std::vector<GLfloat> _leftHandPointers;
    std::vector<GLfloat> _rightHandPointers;
    std::vector<tdv::nuitrack::Gesture> _userGestures;
    
    tdv::nuitrack::OutputMode _outputMode;
    
    tdv::nuitrack::DepthSensor::Ptr _depthSensor;
    tdv::nuitrack::ColorSensor::Ptr _colorSensor;
    tdv::nuitrack::UserTracker::Ptr _userTracker;
    tdv::nuitrack::SkeletonTracker::Ptr _skeletonTracker;
    tdv::nuitrack::HandTracker::Ptr _handTracker;
    tdv::nuitrack::GestureRecognizer::Ptr _gestureRecognizer;
    
    tdv::nuitrack::IssuesData::Ptr _issuesData;
    uint64_t _onIssuesUpdateHandler;
    
    ViewMode _viewMode;
    uint32_t _modesNumber;
    bool _isInitialized;

    void onNewDepthFrame(tdv::nuitrack::DepthFrame::Ptr frame);
    void onNewRGBFrame(tdv::nuitrack::RGBFrame::Ptr frame);
    void onUserUpdateCallback(tdv::nuitrack::UserFrame::Ptr frame);
    void onLostUserCallback(int id);
    void onNewUserCallback(int id);
    void onSkeletonUpdate(tdv::nuitrack::SkeletonData::Ptr userSkeletons);
    void onHandUpdate(tdv::nuitrack::HandTrackerData::Ptr handData);
    void onNewGesture(tdv::nuitrack::GestureData::Ptr gestureData);
    void onIssuesUpdate(tdv::nuitrack::IssuesData::Ptr issuesData);
    
    void drawSkeleton(const std::vector<tdv::nuitrack::Joint>& joints);
    void drawBone(const tdv::nuitrack::Joint& j1, const tdv::nuitrack::Joint& j2);
    void renderTexture();
    void renderLines();
    
    int power2(int n);
    void initTexture(int width, int height);
};

#endif /* NUITRACKGLSAMPLE_H_ */

nuitrack_gl_sample/src/NuitrackGLSample.cpp:

#include "NuitrackGLSample.h"

#include <cstdlib>
#include <cstring>
#include <iostream>

using namespace tdv::nuitrack;

NuitrackGLSample::NuitrackGLSample() :
    _textureID(0),
    _textureBuffer(0),
    _width(640),
    _height(480),
    _viewMode(DEPTH_SEGMENT_MODE),
    _modesNumber(2),
    _isInitialized(false)
{}

NuitrackGLSample::~NuitrackGLSample()
{
    try
    {
        Nuitrack::release();
    }
    catch (const Exception& e)
    {
        // Do nothing
    }
}

void NuitrackGLSample::init(const std::string& config)
{
    // Initialize Nuitrack first, then create Nuitrack modules
    try
    {
        Nuitrack::init(config);
    }
    catch (const Exception& e)
    {
        std::cerr << "Can not initialize Nuitrack (ExceptionType:" << e.type() << ")" << std::endl;
        exit(EXIT_FAILURE);
    }
    
    // Create all required Nuitrack modules

    _depthSensor = DepthSensor::create();
    // Bind to event new frame
    _depthSensor->connectOnNewFrame(std::bind(&NuitrackGLSample::onNewDepthFrame, this, std::placeholders::_1));

    _colorSensor = ColorSensor::create();
    // Bind to event new frame
    _colorSensor->connectOnNewFrame(std::bind(&NuitrackGLSample::onNewRGBFrame, this, std::placeholders::_1));

    _outputMode = _depthSensor->getOutputMode();
    OutputMode colorOutputMode = _colorSensor->getOutputMode();
    if (colorOutputMode.xres > _outputMode.xres)
        _outputMode.xres = colorOutputMode.xres;
    if (colorOutputMode.yres > _outputMode.yres)
        _outputMode.yres = colorOutputMode.yres;

    _width = _outputMode.xres;
    _height = _outputMode.yres;

    _userTracker = UserTracker::create();
    // Binds to user tracker events
    _userTracker->connectOnUpdate(std::bind(&NuitrackGLSample::onUserUpdateCallback, this, std::placeholders::_1));
    _userTracker->connectOnNewUser(std::bind(&NuitrackGLSample::onNewUserCallback, this, std::placeholders::_1));
    _userTracker->connectOnLostUser(std::bind(&NuitrackGLSample::onLostUserCallback, this, std::placeholders::_1));

    _skeletonTracker = SkeletonTracker::create();
    // Bind to event update skeleton tracker
    _skeletonTracker->connectOnUpdate(std::bind(&NuitrackGLSample::onSkeletonUpdate, this, std::placeholders::_1));

    _handTracker = HandTracker::create();
    // Bind to event update Hand tracker
    _handTracker->connectOnUpdate(std::bind(&NuitrackGLSample::onHandUpdate, this, std::placeholders::_1));

    _gestureRecognizer = GestureRecognizer::create();
    _gestureRecognizer->connectOnNewGestures(std::bind(&NuitrackGLSample::onNewGesture, this, std::placeholders::_1));

    _onIssuesUpdateHandler = Nuitrack::connectOnIssuesUpdate(std::bind(&NuitrackGLSample::onIssuesUpdate,
                                                                      this, std::placeholders::_1));
}

bool NuitrackGLSample::update()
{
    if (!_isInitialized)
    {
        // Create texture by DepthSensor output mode
        initTexture(_width, _height);

        // When Nuitrack modules are created, we need to call Nuitrack::run() to start processing all modules
        try
        {
            Nuitrack::run();
        }
        catch (const Exception& e)
        {
            std::cerr << "Can not start Nuitrack (ExceptionType:" << e.type() << ")" << std::endl;
            release();
            exit(EXIT_FAILURE);
        }
        _isInitialized = true;
    }
    try
    {
        // Wait and update Nuitrack data
        Nuitrack::waitUpdate(_skeletonTracker);
        
        renderTexture();
        renderLines();
    }
    catch (const LicenseNotAcquiredException& e)
    {
        // Update failed, negative result
        std::cerr << "LicenseNotAcquired exception (ExceptionType:" << e.type() << ")" << std::endl;
        return false;
    }
    catch (const Exception& e)
    {
        // Update failed, negative result
        std::cerr << "Nuitrack update failed (ExceptionType:" << e.type() << ")" << std::endl;
        return false;
    }
    
    return true;
}

void NuitrackGLSample::release()
{
    if (_onIssuesUpdateHandler)
        Nuitrack::disconnectOnIssuesUpdate(_onIssuesUpdateHandler);

    // Release Nuitrack and remove all modules
    try
    {
        Nuitrack::release();
    }
    catch (const Exception& e)
    {
        std::cerr << "Nuitrack release failed (ExceptionType:" << e.type() << ")" << std::endl;
    }

    _isInitialized = false;
    // Free texture buffer
    if (_textureBuffer)
    {
        delete[] _textureBuffer;
        _textureBuffer = 0;
    }
}

// Copy depth frame data, received from Nuitrack, to texture to visualize
void NuitrackGLSample::onNewDepthFrame(DepthFrame::Ptr frame)
{
    if(_viewMode != DEPTH_SEGMENT_MODE)
        return;

    uint8_t* texturePtr = _textureBuffer;
    const uint16_t* depthPtr = frame->getData();
    
    float wStep = (float)_width / frame->getCols();
    float hStep = (float)_height / frame->getRows();
    
    float nextVerticalBorder = hStep;
    
    for (size_t i = 0; i < _height; ++i)
    {
        if (i == (int)nextVerticalBorder)
        {
            nextVerticalBorder += hStep;
            depthPtr += frame->getCols();
        }
        
        int col = 0;
        float nextHorizontalBorder = wStep;
        uint16_t depthValue = *depthPtr >> 5;
        
        for (size_t j = 0; j < _width; ++j, texturePtr += 3)
        {
            if (j == (int)nextHorizontalBorder)
            {
                ++col;
                nextHorizontalBorder += wStep;
                depthValue = *(depthPtr + col) >> 5;
            }
            
            texturePtr[0] = depthValue;
            texturePtr[1] = depthValue;
            texturePtr[2] = depthValue;
        }
    }
}

// Copy color frame data, received from Nuitrack, to texture to visualize
void NuitrackGLSample::onNewRGBFrame(RGBFrame::Ptr frame)
{
    if(_viewMode != RGB_MODE)
        return;

    uint8_t* texturePtr = _textureBuffer;
    const tdv::nuitrack::Color3* colorPtr = frame->getData();

    float wStep = (float)_width / frame->getCols();
    float hStep = (float)_height / frame->getRows();

    float nextVerticalBorder = hStep;

    for (size_t i = 0; i < _height; ++i)
    {
        if (i == (int)nextVerticalBorder)
        {
            nextVerticalBorder += hStep;
            colorPtr += frame->getCols();
        }

        int col = 0;
        float nextHorizontalBorder = wStep;

        for (size_t j = 0; j < _width; ++j, texturePtr += 3)
        {
            if (j == (int)nextHorizontalBorder)
            {
                ++col;
                nextHorizontalBorder += wStep;
            }

            texturePtr[0] = (colorPtr + col)->red;
            texturePtr[1] = (colorPtr + col)->green;
            texturePtr[2] = (colorPtr + col)->blue;
        }
    }
}
// Colorize user segments using Nuitrack User Tracker data
void NuitrackGLSample::onUserUpdateCallback(UserFrame::Ptr frame)
{
    if(_viewMode != DEPTH_SEGMENT_MODE)
        return;

    const int MAX_LABELS = 8;
    static uint8_t colors[MAX_LABELS][3] =
    {
        {0, 0, 0},
        {0, 255, 0},
        {0, 0, 255},
        {255, 255, 0},
        
        {0, 255, 255},
        {255, 0, 255},
        {127, 255, 0},
        {255, 255, 255}
    };
    
    std::vector<uint8_t> labelIssueState(MAX_LABELS, 0);
    for (uint16_t label = 0; label < MAX_LABELS; ++label)
    {
        labelIssueState[label] = 0;
        if (_issuesData)
        {
            FrameBorderIssue::Ptr frameBorderIssue = _issuesData->getUserIssue<FrameBorderIssue>(label);
            labelIssueState[label] = (frameBorderIssue != 0);
        }
    }
    
    uint8_t* texturePtr = _textureBuffer;
    const uint16_t* labelPtr = frame->getData();
    
    float wStep = (float)_width / frame->getCols();
    float hStep = (float)_height / frame->getRows();
    
    float nextVerticalBorder = hStep;
    
    for (size_t i = 0; i < _height; ++i)
    {
        if (i == (int)nextVerticalBorder)
        {
            nextVerticalBorder += hStep;
            labelPtr += frame->getCols();
        }
        
        int col = 0;
        float nextHorizontalBorder = wStep;
        uint16_t label = *labelPtr;
        
        for (size_t j = 0; j < _width; ++j, texturePtr += 3)
        {
            if (j == (int)nextHorizontalBorder)
            {
                ++col;
                nextHorizontalBorder += wStep;
                label = *(labelPtr + col);
            }
            
            if (!label)
                continue;
            
            for (int i = 0; i < 3; ++i)
            {
                texturePtr[i] = colors[label & 7][i];
                if (labelIssueState[label])
                    texturePtr[i] /= 2;
            }
        }
    }
}

// Callback for onLostUser event
void NuitrackGLSample::onLostUserCallback(int id)
{
    std::cout << "Lost User " << id << std::endl;
}

// Callback for onNewUser event
void NuitrackGLSample::onNewUserCallback(int id)
{
    std::cout << "New User " << id << std::endl;
}

// Prepare visualization of skeletons, received from Nuitrack
void NuitrackGLSample::onSkeletonUpdate(SkeletonData::Ptr userSkeletons)
{
    _lines.clear();
    
    auto skeletons = userSkeletons->getSkeletons();
    for (auto skeleton: skeletons)
    {
        drawSkeleton(skeleton.joints);
    }
}

// Prepare visualization of tracked hands
void NuitrackGLSample::onHandUpdate(HandTrackerData::Ptr handData)
{
    _leftHandPointers.clear();
    _rightHandPointers.clear();
    
    for (auto userHands : handData->getUsersHands())
    {
        if (userHands.leftHand)
        {
            _leftHandPointers.push_back(_width * userHands.leftHand->x);
            _leftHandPointers.push_back(_height * userHands.leftHand->y);
            if (userHands.leftHand->click)
            {
                _leftHandPointers.push_back(_width * userHands.leftHand->x);
                _leftHandPointers.push_back(_height * userHands.leftHand->y);
            }
        }
        
        if (userHands.rightHand)
        {
            _rightHandPointers.push_back(_width * userHands.rightHand->x);
            _rightHandPointers.push_back(_height * userHands.rightHand->y);
            if (userHands.rightHand->click)
            {
                _rightHandPointers.push_back(_width * userHands.rightHand->x);
                _rightHandPointers.push_back(_height * userHands.rightHand->y);
            }
        }
    }
}

// Display information about gestures in the console
void NuitrackGLSample::onNewGesture(GestureData::Ptr gestureData)
{
    _userGestures = gestureData->getGestures();
    for (int i = 0; i < _userGestures.size(); ++i)
    {
        printf("Recognized %d from %d\n", _userGestures[i].type, _userGestures[i].userId);
    }
}

void NuitrackGLSample::onIssuesUpdate(IssuesData::Ptr issuesData)
{
    _issuesData = issuesData;
}

// Helper function to draw a skeleton bone
void NuitrackGLSample::drawBone(const Joint& j1, const Joint& j2)
{
    // Prepare line data for confident enough bones only
    if (j1.confidence > 0.15 && j2.confidence > 0.15)
    {
        _lines.push_back(_width * j1.proj.x);
        _lines.push_back(_height * j1.proj.y);
        _lines.push_back(_width * j2.proj.x);
        _lines.push_back(_height * j2.proj.y);
    }
}

// Helper function to draw skeleton from Nuitrack data
void NuitrackGLSample::drawSkeleton(const std::vector<Joint>& joints)
{
    // We need to draw a bone for every pair of neighbour joints
    drawBone(joints[JOINT_HEAD], joints[JOINT_NECK]);
    drawBone(joints[JOINT_NECK], joints[JOINT_LEFT_COLLAR]);
    drawBone(joints[JOINT_LEFT_COLLAR], joints[JOINT_TORSO]);
    drawBone(joints[JOINT_LEFT_COLLAR], joints[JOINT_LEFT_SHOULDER]);
    drawBone(joints[JOINT_LEFT_COLLAR], joints[JOINT_RIGHT_SHOULDER]);
    drawBone(joints[JOINT_WAIST], joints[JOINT_LEFT_HIP]);
    drawBone(joints[JOINT_WAIST], joints[JOINT_RIGHT_HIP]);
    drawBone(joints[JOINT_TORSO], joints[JOINT_WAIST]);
    drawBone(joints[JOINT_LEFT_SHOULDER], joints[JOINT_LEFT_ELBOW]);
    drawBone(joints[JOINT_LEFT_ELBOW], joints[JOINT_LEFT_WRIST]);
    drawBone(joints[JOINT_LEFT_WRIST], joints[JOINT_LEFT_HAND]);
    drawBone(joints[JOINT_RIGHT_SHOULDER], joints[JOINT_RIGHT_ELBOW]);
    drawBone(joints[JOINT_RIGHT_ELBOW], joints[JOINT_RIGHT_WRIST]);
    drawBone(joints[JOINT_RIGHT_WRIST], joints[JOINT_RIGHT_HAND]);
    drawBone(joints[JOINT_RIGHT_HIP], joints[JOINT_RIGHT_KNEE]);
    drawBone(joints[JOINT_LEFT_HIP], joints[JOINT_LEFT_KNEE]);
    drawBone(joints[JOINT_RIGHT_KNEE], joints[JOINT_RIGHT_ANKLE]);
    drawBone(joints[JOINT_LEFT_KNEE], joints[JOINT_LEFT_ANKLE]);
}

// Render prepared background texture
void NuitrackGLSample::renderTexture()
{
    glClearColor(1, 1, 1, 1);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    
    glEnable(GL_TEXTURE_2D);
    glColor4f(1, 1, 1, 1);
    
    glBindTexture(GL_TEXTURE_2D, _textureID);
    glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, _width, _height, GL_RGB, GL_UNSIGNED_BYTE, _textureBuffer);
    
    glEnableClientState(GL_VERTEX_ARRAY);
    glEnableClientState(GL_TEXTURE_COORD_ARRAY);
    
    glVertexPointer(2, GL_FLOAT, 0, _vertexes);
    glTexCoordPointer(2, GL_FLOAT, 0, _textureCoords);
    
    glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
    
    glDisableClientState(GL_VERTEX_ARRAY);
    glDisableClientState(GL_TEXTURE_COORD_ARRAY);
    
    glDisable(GL_TEXTURE_2D);
}

int NuitrackGLSample::power2(int n)
{
    unsigned int m = 2;
    while (m < n)
        m <<= 1;
    
    return m;
}

// Visualize bones, joints and hand positions
void NuitrackGLSample::renderLines()
{
    if (_lines.empty())
        return;
    
    glEnableClientState(GL_VERTEX_ARRAY);
    
    glColor4f(1, 1, 1, 1);
    
    glLineWidth(6);
    
    glVertexPointer(2, GL_FLOAT, 0, _lines.data());
    glDrawArrays(GL_LINES, 0, _lines.size() / 2);
    
    glLineWidth(1);
    
    glEnable(GL_POINT_SMOOTH);
    glPointSize(16);
    
    glVertexPointer(2, GL_FLOAT, 0, _lines.data());
    glDrawArrays(GL_POINTS, 0, _lines.size() / 2);
    
    if (!_leftHandPointers.empty())
    {
        glColor4f(1, 0, 0, 1);
        glPointSize(16);
        glVertexPointer(2, GL_FLOAT, 0, _leftHandPointers.data());
        glDrawArrays(GL_POINTS, 0, 1);
        if (_leftHandPointers.size() > 2)
        {
            glPointSize(24);
            glVertexPointer(2, GL_FLOAT, 0, _leftHandPointers.data() + 2);
            glDrawArrays(GL_POINTS, 0, 1);
        }
    }
    
    if (!_rightHandPointers.empty())
    {
        glColor4f(0, 0, 1, 1);
        glPointSize(16);
        glVertexPointer(2, GL_FLOAT, 0, _rightHandPointers.data());
        glDrawArrays(GL_POINTS, 0, 1);
        if (_rightHandPointers.size() > 2)
        {
            glPointSize(24);
            glVertexPointer(2, GL_FLOAT, 0, _rightHandPointers.data() + 2);
            glDrawArrays(GL_POINTS, 0, 1);
        }
    }
    
    glColor4f(1, 1, 1, 1);
    glPointSize(1);
    glDisable(GL_POINT_SMOOTH);
    
    glDisableClientState(GL_VERTEX_ARRAY);
}

void NuitrackGLSample::initTexture(int width, int height)
{
    glGenTextures(1, &_textureID);
    
    width = power2(width);
    height = power2(height);
    
    if (_textureBuffer != 0)
        delete[] _textureBuffer;
    
    _textureBuffer = new uint8_t[width * height * 3];
    memset(_textureBuffer, 0, sizeof(uint8_t) * width * height * 3);
    
    glBindTexture(GL_TEXTURE_2D, _textureID);
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, NULL);
    
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
    
    // Set texture coordinates [0, 1] and vertexes position
    {
        _textureCoords[0] = (float) _width / width;
        _textureCoords[1] = (float) _height / height;
        _textureCoords[2] = (float) _width / width;
        _textureCoords[3] = 0.0;
        _textureCoords[4] = 0.0;
        _textureCoords[5] = 0.0;
        _textureCoords[6] = 0.0;
        _textureCoords[7] = (float) _height / height;
        
        _vertexes[0] = _width;
        _vertexes[1] = _height;
        _vertexes[2] = _width;
        _vertexes[3] = 0.0;
        _vertexes[4] = 0.0;
        _vertexes[5] = 0.0;
        _vertexes[6] = 0.0;
        _vertexes[7] = _height;
    }
}

nuitrack_gl_sample/src/main.cpp:

#include "NuitrackGLSample.h"

#include <GL/glut.h>

#include <iostream>

NuitrackGLSample sample;

// Keyboard handler
void keyboard(unsigned char key, int x, int y)
{
    switch (key)
    {
    // On Esc key press
    case 27:
    {
        sample.release();
        glutDestroyWindow(glutGetWindow());
        exit(EXIT_FAILURE);
    }

    default:
    {
        // Do nothing otherwise
        break;
    }
    }
}

void mouseClick(int button, int state, int x, int y)
{
    if(button == GLUT_LEFT_BUTTON && state == GLUT_DOWN)
    {
        sample.nextViewMode();
    }
}

// Update tracking data and visualize it
void display()
{
    // Delegate this action to example's main class
    bool update = sample.update();

    if (!update)
    {
        // End the work if update failed
        sample.release();
        glutDestroyWindow(glutGetWindow());
        exit(EXIT_FAILURE);
    }

    // Do flush and swap buffers to update viewport
    glFlush();
    glutSwapBuffers();
}

void idle()
{
    glutPostRedisplay();
}

void showHelpInfo()
{
    std::cout << "Usage: nuitrack_gl_sample [path/to/nuitrack.config]\n"
                 "Press Esc to close window."<< std::endl;
}



int main(int argc, char* argv[])
{
    showHelpInfo();
    
    // Prepare sample to work
    if (argc < 2)
        sample.init();
    else
        sample.init(argv[1]);

    auto outputMode = sample.getOutputMode();

    // Initialize GLUT window
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
    glutInitWindowSize(outputMode.xres, outputMode.yres);
    glutCreateWindow("Nuitrack GL Sample (Nuitrack API)");
    //glutSetCursor(GLUT_CURSOR_NONE);

    // Connect GLUT callbacks
    glutKeyboardFunc(keyboard);
    glutDisplayFunc(display);
    glutIdleFunc(idle);
    glutMouseFunc(mouseClick);

    // Setup OpenGL
    glDisable(GL_DEPTH_TEST);
    glEnable(GL_TEXTURE_2D);

    glEnableClientState(GL_VERTEX_ARRAY);
    glDisableClientState(GL_COLOR_ARRAY);


    glOrtho(0, outputMode.xres, outputMode.yres, 0, -1.0, 1.0);
    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();
    
    // Start main loop
    glutMainLoop();

    return 0;
}