/// <summary>
/// Draw a bone between 2 tracked joint.
/// <summary>
/// <param name="skeletonData">Skeleton coordinates</param>
/// <param name="imageRect">The rect which the color or depth image is streched to fit</param>
/// <param name="joint0">Index for the first joint</param>
/// <param name="joint1">Index for the second joint</param>
void UKinect::drawBone(const NUI_SKELETON_DATA& skeletonData, NUI_SKELETON_POSITION_INDEX joint0, NUI_SKELETON_POSITION_INDEX joint1) {
NUI_SKELETON_POSITION_TRACKING_STATE state0 = skeletonData.eSkeletonPositionTrackingState[joint0];
NUI_SKELETON_POSITION_TRACKING_STATE state1 = skeletonData.eSkeletonPositionTrackingState[joint1];
// Any is not tracked
if (NUI_SKELETON_POSITION_NOT_TRACKED == state0 || NUI_SKELETON_POSITION_NOT_TRACKED == state1) {
return;
}
// Both are inferred
if (NUI_SKELETON_POSITION_INFERRED == state0 && NUI_SKELETON_POSITION_INFERRED == state1) {
return;
}
LONG x1, y1, x2, y2;
LONG x1d, y1d, x2d, y2d;
USHORT depth;
NuiTransformSkeletonToDepthImage(skeletonData.SkeletonPositions[joint0], &x1d, &y1d, &depth, (NUI_IMAGE_RESOLUTION)depthResolution.as<int>());
NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution((NUI_IMAGE_RESOLUTION)colorResolution.as<int>(), (NUI_IMAGE_RESOLUTION)depthResolution.as<int>(), NULL, x1d, y1d, depth, &x1, &y1);
NuiTransformSkeletonToDepthImage(skeletonData.SkeletonPositions[joint1], &x2d, &y2d, &depth, (NUI_IMAGE_RESOLUTION)depthResolution.as<int>());
NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution((NUI_IMAGE_RESOLUTION)colorResolution.as<int>(), (NUI_IMAGE_RESOLUTION)depthResolution.as<int>(), NULL, x2d, y2d, depth, &x2, &y2);
// We assume all drawn bones are inferred unless BOTH joints are tracked
if (NUI_SKELETON_POSITION_TRACKED == state0 && NUI_SKELETON_POSITION_TRACKED == state1) {
line(skeletonCVMat, Point(x1, y1), Point(x2, y2), CV_RGB(0, 0, 255), 4);
} else {
line(skeletonCVMat, Point(x1, y1), Point(x2, y2), CV_RGB(160, 160, 180), 4);
}
}
void getDepthData(GLubyte* dest) {
float* fdest = (float*) dest;
long* depth2rgb = (long*) depthToRgbMap;
NUI_IMAGE_FRAME imageFrame;
NUI_LOCKED_RECT LockedRect;
if (sensor->NuiImageStreamGetNextFrame(depthStream, 0, &imageFrame) < 0) return;
INuiFrameTexture* texture = imageFrame.pFrameTexture;
texture->LockRect(0, &LockedRect, NULL, 0);
if (LockedRect.Pitch != 0) {
const USHORT* curr = (const USHORT*) LockedRect.pBits;
for (int j = 0; j < height; ++j) {
for (int i = 0; i < width; ++i) {
// Get depth of pixel in millimeters
USHORT depth = NuiDepthPixelToDepth(*curr++);
// Store coordinates of the point corresponding to this pixel
Vector4 pos = NuiTransformDepthImageToSkeleton(i, j, depth<<3, NUI_IMAGE_RESOLUTION_640x480);
*fdest++ = pos.x/pos.w;
*fdest++ = pos.y/pos.w;
*fdest++ = pos.z/pos.w;
// Store the index into the color array corresponding to this pixel
NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution(
NUI_IMAGE_RESOLUTION_640x480, NUI_IMAGE_RESOLUTION_640x480, NULL,
i, j, depth<<3, depth2rgb, depth2rgb+1);
depth2rgb += 2;
}
}
}
texture->UnlockRect(0);
sensor->NuiImageStreamReleaseFrame(depthStream, &imageFrame);
}
/// <summary>
/// Draw a circle to indicate a skeleton of which only position info is available
/// </summary>
/// <param name="skeletonData">Skeleton coordinates</param>
/// <param name="imageRect">The rect which the color or depth stream image is streched to fit</param>
void UKinect::drawPosition(const NUI_SKELETON_DATA& skeletonData) {
LONG xd, yd, x, y;
USHORT depth;
NuiTransformSkeletonToDepthImage(skeletonData.Position, &xd, &yd, &depth, (NUI_IMAGE_RESOLUTION)depthResolution.as<int>());
NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution((NUI_IMAGE_RESOLUTION)colorResolution.as<int>(), (NUI_IMAGE_RESOLUTION)depthResolution.as<int>(), NULL, xd, yd, depth, &x, &y);
circle(skeletonCVMat, Point(static_cast<int>(x), static_cast<int>(y)), 10, CV_RGB(0, 50, 200), 2);
putText(skeletonCVMat, lexical_cast<string>(skeletonData.dwTrackingID), Point(x, y), 1, 2, CV_RGB(255, 255, 255), 2);
}
bool KinectCapture::GetAlignedColorImage(cv::Mat & image)
{
if (m_alignedColorImagePixelBuffer == nullptr)
{
return false;
}
//Acquire unaligned color image
if (!GetColorImage(image))
{
return false;
}
//Align Color Image
for (int j = 0; j < m_DepthHeight; j++)
{
for (int i = 0; i < m_DepthWidth; i++)
{
// Determine color pixel for depth pixel i,j
long x;
long y;
NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution(
m_colorImageResolution, // color frame resolution
m_depthImageResolution, // depth frame resolution
NULL, // pan/zoom of color image (IGNORE THIS)
i, j, m_depthImagePixelBuffer[j*m_DepthWidth + i], // Column, row, and depth in depth image
&x, &y // Output: column and row (x,y) in the color image
);
// If out of bounds, then do not color this pixel
if (x < 0 || y < 0 || x > m_ColorWidth || y > m_ColorHeight)
{
for (int ch = 0; ch < 3; ch++)
{
m_alignedColorImagePixelBuffer[4 * (j*m_DepthWidth + i) + ch] = 0;
}
m_alignedColorImagePixelBuffer[4 * (j*m_DepthWidth + i) + 3] = 255;
}
else {
// Determine rgb color for depth pixel (i,j) from color pixel (x,y)
for (int ch = 0; ch < 3; ch++)
{
m_alignedColorImagePixelBuffer[4 * (j*m_DepthWidth + i) + ch] = m_colorImagePixelBuffer[4 * (y*m_ColorWidth + x) + ch];
}
m_alignedColorImagePixelBuffer[4 * (j*m_DepthWidth + i) + 3] = 255;
}
}
}
cv::Mat colorImg(m_DepthHeight, m_DepthWidth, CV_8UC4, m_alignedColorImagePixelBuffer);
image = colorImg;
return true;
}
QPointF MapToScreen(const Vector4 &point)
{
//@to-do: It's not fixed
const NUI_IMAGE_RESOLUTION resolution = NUI_IMAGE_RESOLUTION_640x480;
long x, y;
ushort depth;
NuiTransformSkeletonToDepthImage(point, &x, &y, &depth, resolution); // Returns coordinates in depth space
long backupX = x, backupY = y;
if (FAILED(NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution(resolution, resolution, NULL, x, y, depth, &x, &y))) {
x = backupX;
y = backupY;
}
return QPointF(x, y);
}
void UKinect::drawHand(DWORD ID, int joint, int _catch, bool pressed, bool active, double press)
{
LONG x, y, xd, yd;
USHORT depth;
Vector4 vPoint = {0,0,0,0};
vector<double> tmp = skeletonJointPosition(ID, joint);
if (tmp.size()==0) return;
vPoint.x = tmp[0];
vPoint.y = tmp[1];
vPoint.z = tmp[2];
NuiTransformSkeletonToDepthImage(vPoint, &xd, &yd, &depth, (NUI_IMAGE_RESOLUTION)depthResolution.as<int>());
hr = NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution((NUI_IMAGE_RESOLUTION)colorResolution.as<int>(), (NUI_IMAGE_RESOLUTION)depthResolution.as<int>(), NULL, xd, yd, depth, &x, &y);
if (FAILED(hr)) return;
if (press>2) press=2;
press=press*125;
circle(interCVMat,Point(static_cast<int>(x),static_cast<int>(y)),10,CV_RGB( static_cast<int>(press),static_cast<int>(press),static_cast<int>(press)),-1);
if (1 == _catch)
{
if (pressed){
circle(interCVMat,Point(static_cast<int>(x),static_cast<int>(y)),15,CV_RGB( 0, 0, 255),-1);
} else if (active) {
circle(interCVMat,Point(static_cast<int>(x),static_cast<int>(y)),15,CV_RGB( 0, 255 ,0),-1);
} else {
circle(interCVMat,Point(static_cast<int>(x),static_cast<int>(y)),15,CV_RGB( 255, 0, 0),-1);
}
}
else
{
if (pressed){
circle(interCVMat,Point(static_cast<int>(x),static_cast<int>(y)),15,CV_RGB( 0, 0, 255),3);
} else if (active) {
circle(interCVMat,Point(static_cast<int>(x),static_cast<int>(y)),15,CV_RGB( 0, 255, 0),3);
} else {
circle(interCVMat,Point(static_cast<int>(x),static_cast<int>(y)),15,CV_RGB( 255, 0, 0),3);
}
}
}
vector<double> UKinect::skeletonJointPositionOnImage(DWORD ID, int joint) {
vector<double> position;
if (&skeletonFrame != NULL) {
for (int i = 0; i < NUI_SKELETON_COUNT; i++) {
const NUI_SKELETON_DATA & skeletonData = skeletonFrame.SkeletonData[i];
if ((skeletonData.eTrackingState == NUI_SKELETON_TRACKED) && (skeletonData.dwTrackingID == ID) && (skeletonData.eSkeletonPositionTrackingState[joint] >= NUI_SKELETON_POSITION_INFERRED)) {
LONG x, y, xd, yd;
USHORT depth;
NuiTransformSkeletonToDepthImage(skeletonData.SkeletonPositions[joint], &xd, &yd, &depth, (NUI_IMAGE_RESOLUTION)depthResolution.as<int>());
NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution((NUI_IMAGE_RESOLUTION)colorResolution.as<int>(), (NUI_IMAGE_RESOLUTION)depthResolution.as<int>(), NULL, xd, yd, depth, &x, &y);
position.push_back(x);
position.push_back(y);
position.push_back(static_cast<double>(depth));
}
}
}
return position;
}
/// <summary>
/// Draw a joint of the skeleton
/// </summary>
/// <param name="skeletonData">Skeleton coordinates</param>
/// <param name="imageRect">The rect which the color or depth image is streched to fit</param>
/// <param name="joint">Index for the joint to be drawn</param>
void UKinect::drawJoint(const NUI_SKELETON_DATA& skeletonData, NUI_SKELETON_POSITION_INDEX joint) {
NUI_SKELETON_POSITION_TRACKING_STATE state = skeletonData.eSkeletonPositionTrackingState[joint];
// Not tracked
if (NUI_SKELETON_POSITION_NOT_TRACKED == state) {
return;
}
LONG x, y, xd, yd;
USHORT depth;
NuiTransformSkeletonToDepthImage(skeletonData.SkeletonPositions[joint], &xd, &yd, &depth, (NUI_IMAGE_RESOLUTION)depthResolution.as<int>());
NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution((NUI_IMAGE_RESOLUTION)colorResolution.as<int>(), (NUI_IMAGE_RESOLUTION)depthResolution.as<int>(), NULL, xd, yd, depth, &x, &y);
if (NUI_SKELETON_POSITION_TRACKED == state) {
circle(skeletonCVMat, Point(static_cast<int>(x), static_cast<int>(y)), 7, CV_RGB(0, 200, 0), -1);
} else {
circle(skeletonCVMat, Point(static_cast<int>(x), static_cast<int>(y)), 6, CV_RGB(0, 200, 0), 2);
}
// draw "L" and "R" informations near shoulders
if (joint == 4) putText(skeletonCVMat, lexical_cast<string>("L"), Point(static_cast<int>(x), static_cast<int>(y) - 10), 1, 2, CV_RGB(255, 255, 255), 2);
if (joint == 8) putText(skeletonCVMat, lexical_cast<string>("R"), Point(static_cast<int>(x) - 13, static_cast<int>(y) - 10), 1, 2, CV_RGB(255, 255, 255), 2);
}
