人体骨骼坐标在彩色图像中显示

// YeNite2SimpleUsingOpenCV.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"
#include <iostream>

// OpenCV 头文件
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <OpenNI.h>
#include <NiTE.h>

using namespace std;
using namespace openni;
using namespace nite;

int main( int argc, char **argv )
{
// 初始化OpenNI
OpenNI::initialize();

// 打开Kinect设备
Device  mDevice;
mDevice.open( ANY_DEVICE );

// 创建深度数据流
VideoStream mDepthStream;
mDepthStream.create( mDevice, SENSOR_DEPTH );

// 设置VideoMode模式
VideoMode mDepthMode;
mDepthMode.setResolution( 640, 480 );
mDepthMode.setFps( 30 );
mDepthMode.setPixelFormat( PIXEL_FORMAT_DEPTH_1_MM );
mDepthStream.setVideoMode(mDepthMode);

// 同样的设置彩色数据流
VideoStream mColorStream;
mColorStream.create( mDevice, SENSOR_COLOR );
// 设置VideoMode模式
VideoMode mColorMode;
mColorMode.setResolution( 640, 480 );
mColorMode.setFps( 30 );
mColorMode.setPixelFormat( PIXEL_FORMAT_RGB888 );
mColorStream.setVideoMode( mColorMode);

// 设置深度图像映射到彩色图像
mDevice.setImageRegistrationMode( IMAGE_REGISTRATION_DEPTH_TO_COLOR );

// 为了得到骨骼数据，先初始化NiTE
NiTE::initialize();

// 创建用户跟踪器
UserTracker mUserTracker;
mUserTracker.create( &mDevice );

// Control the smoothing factor of the skeleton joints. Factor should be between 0 (no smoothing at all) and 1 (no movement at all)
mUserTracker.setSkeletonSmoothingFactor( 0.1f );

// 创建User彩色图像显示
cv::namedWindow( "User Image",  CV_WINDOW_AUTOSIZE );

// 环境初始化后，开始获取深度数据流和彩色数据流
mDepthStream.start();
mColorStream.start();

while( true )
{
// 创建OpenCV：：Mat，用于显示彩色数据图像
cv::Mat cImageBGR;

// 读取彩色图像数据帧信息流
VideoFrameRef mColorFrame;
mColorStream.readFrame( &mColorFrame );

// 将彩色数据流转换为OpenCV格式，记得格式是：CV_8UC3（含R\G\B）
const cv::Mat mImageRGB( mColorFrame.getHeight(), mColorFrame.getWidth(),
CV_8UC3, (void*)mColorFrame.getData() );

// RGB ==> BGR
cv::cvtColor( mImageRGB, cImageBGR, CV_RGB2BGR );

// 读取User用户数据帧信息流
UserTrackerFrameRef  mUserFrame;
mUserTracker.readFrame( &mUserFrame );

// 得到Users信息
const nite::Array<UserData>& aUsers = mUserFrame.getUsers();
for( int i = 0; i < aUsers.getSize(); ++ i )
{
const UserData& rUser = aUsers[i];

// 检查用户状态
if( rUser.isNew() )
{
// 开始对该用户的骨骼跟踪
mUserTracker.startSkeletonTracking( rUser.getId() );
}

if( rUser.isVisible() )
{
// 得到用户骨骼数据
const Skeleton& rSkeleton = rUser.getSkeleton();

// 检查骨骼状态是否为“跟踪状态”
if( rSkeleton.getState() == SKELETON_TRACKED )
{
// 得到15个骨骼数据
SkeletonJoint aJoints[15];
aJoints[ 0] = rSkeleton.getJoint( JOINT_HEAD );
aJoints[ 1] = rSkeleton.getJoint( JOINT_NECK );
aJoints[ 2] = rSkeleton.getJoint( JOINT_LEFT_SHOULDER );
aJoints[ 3] = rSkeleton.getJoint( JOINT_RIGHT_SHOULDER );
aJoints[ 4] = rSkeleton.getJoint( JOINT_LEFT_ELBOW );
aJoints[ 5] = rSkeleton.getJoint( JOINT_RIGHT_ELBOW );
aJoints[ 6] = rSkeleton.getJoint( JOINT_LEFT_HAND );
aJoints[ 7] = rSkeleton.getJoint( JOINT_RIGHT_HAND );
aJoints[ 8] = rSkeleton.getJoint( JOINT_TORSO );
aJoints[ 9] = rSkeleton.getJoint( JOINT_LEFT_HIP );
aJoints[10] = rSkeleton.getJoint( JOINT_RIGHT_HIP );
aJoints[11] = rSkeleton.getJoint( JOINT_LEFT_KNEE );
aJoints[12] = rSkeleton.getJoint( JOINT_RIGHT_KNEE );
aJoints[13] = rSkeleton.getJoint( JOINT_LEFT_FOOT );
aJoints[14] = rSkeleton.getJoint( JOINT_RIGHT_FOOT );

// 将骨骼3D坐标转换为深度坐标下骨骼位置坐标，并保存在数组中
cv::Point2f aPoint[15];
for( int  s = 0; s < 15; ++ s )
{
const Point3f& rPos = aJoints[s].getPosition();
mUserTracker.convertJointCoordinatesToDepth(
rPos.x, rPos.y, rPos.z,
&(aPoint[s].x), &(aPoint[s].y) );
}

// 在彩色图像中画出骨骼间的连接线
cv::line( cImageBGR, aPoint[ 0], aPoint[ 1], cv::Scalar( 255, 0, 0 ), 3 );
cv::line( cImageBGR, aPoint[ 1], aPoint[ 2], cv::Scalar( 255, 0, 0 ), 3 );
cv::line( cImageBGR, aPoint[ 1], aPoint[ 3], cv::Scalar( 255, 0, 0 ), 3 );
cv::line( cImageBGR, aPoint[ 2], aPoint[ 4], cv::Scalar( 255, 0, 0 ), 3 );
cv::line( cImageBGR, aPoint[ 3], aPoint[ 5], cv::Scalar( 255, 0, 0 ), 3 );
cv::line( cImageBGR, aPoint[ 4], aPoint[ 6], cv::Scalar( 255, 0, 0 ), 3 );
cv::line( cImageBGR, aPoint[ 5], aPoint[ 7], cv::Scalar( 255, 0, 0 ), 3 );
cv::line( cImageBGR, aPoint[ 1], aPoint[ 8], cv::Scalar( 255, 0, 0 ), 3 );
cv::line( cImageBGR, aPoint[ 8], aPoint[ 9], cv::Scalar( 255, 0, 0 ), 3 );
cv::line( cImageBGR, aPoint[ 8], aPoint[10], cv::Scalar( 255, 0, 0 ), 3 );
cv::line( cImageBGR, aPoint[ 9], aPoint[11], cv::Scalar( 255, 0, 0 ), 3 );
cv::line( cImageBGR, aPoint[10], aPoint[12], cv::Scalar( 255, 0, 0 ), 3 );
cv::line( cImageBGR, aPoint[11], aPoint[13], cv::Scalar( 255, 0, 0 ), 3 );
cv::line( cImageBGR, aPoint[12], aPoint[14], cv::Scalar( 255, 0, 0 ), 3 );

// 同样的在彩色图像中骨骼位置上画“圆”
for( int  s = 0; s < 15; ++ s )
{
if( aJoints[s].getPositionConfidence() > 0.5 )
cv::circle( cImageBGR, aPoint[s], 3, cv::Scalar( 0, 0, 255 ), 2 );
else
cv::circle( cImageBGR, aPoint[s], 3, cv::Scalar( 0, 255, 0 ), 2 );
}
}
}
}

// 显示图像
cv::imshow( "User Image", cImageBGR );

// 按键“q”退出循环
if( cv::waitKey( 1 ) == 'q' )
break;
}

// 先销毁User跟踪器
mUserTracker.destroy();

// 销毁彩色数据流和深度数据流
mColorStream.destroy();
mDepthStream.destroy();

// 关闭Kinect设备
mDevice.close();

// 关闭NITE和OpenNI环境
NiTE::shutdown();
OpenNI::shutdown();

return 0;
}