openni2_tracker and rgb data

edit

Using openni::Videostream. Find below some example relevant source code (header and footer to see where it goes in the main of tracker.cpp). I am also copying the complete file but it is a mess and read the bold text as you need to change two variables.

Relevant snippet

// Get Tracker Parameters
if(!pnh.getParam("tf_prefix", tf_prefix)){
    ROS_ERROR("tf_prefix not found on Param Server! Check your launch file!");
    return -1;
}
if(!pnh.getParam("relative_frame", relative_frame)){
    ROS_ERROR("relative_frame not found on Param Server! Check your launch file!");
    return -1;
}

// Initial OpenNI
if( openni::OpenNI::initialize() != openni::STATUS_OK )
{
    ROS_ERROR("openni initial error: \n");
    //cerr << "OpenNI Initial Error: " << OpenNI::getExtendedError() << endl;
    return -1;
}

// Open Device
openni::Device  devDevice;
if( devDevice.open( openni::ANY_DEVICE ) != openni::STATUS_OK )
{
    ROS_ERROR("Can't Open Device: \n");
    return -1;
}
ROS_INFO("device opened\n");

// NITE Stuff
nite::UserTracker userTracker;
nite::Status niteRc;
nite::NiTE::initialize();
niteRc = userTracker.create(&devDevice);
if (niteRc != nite::STATUS_OK){
    printf("Couldn't create user tracker\n");
    return 3;
}


// Create color stream
openni::VideoStream vsColorStream;

if( vsColorStream.create( devDevice, openni::SENSOR_COLOR ) == openni::STATUS_OK )
{
    // set video mode
    openni::VideoMode mMode;
    //mMode.setResolution( 640, 480 );
    mMode.setResolution( 640, 480 );
    mMode.setFps( 30 );
    mMode.setPixelFormat( openni::PIXEL_FORMAT_RGB888 );

    if( vsColorStream.setVideoMode( mMode) != openni::STATUS_OK )
    {
        ROS_INFO("Can't apply videomode\n");
        //cout << "Can't apply VideoMode: " << OpenNI::getExtendedError() << endl;
    }

    // image registration
    if( devDevice.isImageRegistrationModeSupported( openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR ) )
    {
        devDevice.setImageRegistrationMode( openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR );
    }
    vsColorStream.setMirroringEnabled(false);
}
else
{
    ROS_ERROR("Can't create color stream on device: ");// << OpenNI::getExtendedError() << endl;
    //cerr <<  "Can't create color stream on device: " << OpenNI::getExtendedError() << endl;
    return -1;
}


// Loop
printf("\nStart moving around to get detected...\n(PSI pose may be required for skeleton calibration, depending on the configuration)\n");

nite::UserTrackerFrameRef userTrackerFrame;

// create OpenCV Window
cv::namedWindow( "User Image",  CV_WINDOW_AUTOSIZE );

// start
vsColorStream.start();

// while (!wasKeyboardHit()){
ros::Rate rate(100.0);
while (nh.ok()){
    niteRc = userTracker.readFrame(&userTrackerFrame);
    if (niteRc != nite::STATUS_OK){
        printf("Get next frame failed\n");
        continue;
    }

    // get depth frame
    openni::VideoFrameRef depthFrame;
    depthFrame = userTrackerFrame.getDepthFrame();

    // get color frame
    openni::VideoFrameRef vfColorFrame;
    cv::Mat mImageBGR;
    if( vsColorStream.readFrame( &vfColorFrame ) == openni::STATUS_OK )
    {
        // convert data to OpenCV format
        const cv::Mat mImageRGB( vfColorFrame.getHeight(), vfColorFrame.getWidth(), CV_8UC3, const_cast<void*>( vfColorFrame.getData() ) );
        // convert form RGB to BGR
        cv::cvtColor( mImageRGB, mImageBGR, CV_RGB2BGR );
        vfColorFrame.release();
    }

    const nite::Array<nite::UserData>& users = userTrackerFrame.getUsers();
    for (int i = 0; i < users.getSize(); ++i) {

Complete file and fairly messy but to compare with yours:

You need to change in main the following two variables as I am saving the images and the skeleton: std::string path("/home/yiannis/datasets/recorded/scrap/"); std::string filename_skeleton("/home/yiannis/datasets/recorded/scrap/skeleton.csv");

Full file

/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2013, Johns Hopkins University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice ...