// Update is called once per frame
void Update()
{
if (sm != null)
{
/* Wait until any frame data is available */
if (sm.AcquireFrame(false, 0) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
if (hand_data.QueryNumberOfHands() == 0)
{
calibText.text = "";
}
/* Retrieve latest hand data, only update slingshot if ready */
if (hand_data.Update() == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
;
}
{
TrackJoints(hand_data);
pDrag.UpdateDrag(isPinching);
}
/* Now, release the current frame so we can process the next frame */
sm.ReleaseFrame();
}
}
}
/// <summary> 手のデータを更新する </summary>
private void UpdateHandFrame()
{
// 手のデータを更新する
handData.Update();
// データを初期化する
CanvasFaceParts.Children.Clear();
// 検出した手の数を取得する
var numOfHands = handData.QueryNumberOfHands();
for (int i = 0; i < numOfHands; i++)
{
// 手を取得する
PXCMHandData.IHand hand;
var sts = handData.QueryHandData(
PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_ID, i, out hand);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
GetFingerData(hand, PXCMHandData.JointType.JOINT_MIDDLE_TIP);
DetectTap(hand);
}
}
/* Displaying current frames hand joints */
private void DisplayJoints(PXCMHandData handOutput, long timeStamp = 0)
{
if (form.GetJointsState() || form.GetSkeletonState())
{
//Iterate hands
PXCMHandData.JointData[][] nodes = new PXCMHandData.JointData[][] { new PXCMHandData.JointData[0x20], new PXCMHandData.JointData[0x20] };
int numOfHands = handOutput.QueryNumberOfHands();
for (int i = 0; i < numOfHands; i++)
{
//Get hand by time of appearence
//PXCMHandAnalysis.HandData handData = new PXCMHandAnalysis.HandData();
PXCMHandData.IHand handData;
if (handOutput.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, i, out handData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
if (handData != null)
{
//Iterate Joints
for (int j = 0; j < 0x20; j++)
{
PXCMHandData.JointData jointData;
handData.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData);
nodes[i][j] = jointData;
} // end iterating over joints
}
}
} // end itrating over hands
form.DisplayJoints(nodes, numOfHands);
}
else
{
form.DisplayJoints(null, 0);
}
}
/// <summary> 手のデータを更新する </summary>
private void UpdateHandFrame()
{
// 手のデータを更新する
handData.Update();
// データを初期化する
PartsCanvas.Children.Clear();
// 検出した手の数を取得する
var numOfHands = handData.QueryNumberOfHands();
for (int i = 0; i < numOfHands; i++)
{
// 手を取得する
PXCMHandData.IHand hand;
var sts = handData.QueryHandData(
PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_ID, i, out hand);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
//Console.WriteLine(hand.QueryUniqueId());
int side = (int)hand.QueryBodySide();
if (side > 0)
{
side2id[side - 1] = hand.QueryUniqueId();
}
GetFingerData(hand, PXCMHandData.JointType.JOINT_MIDDLE_TIP);
}
}
// 手のデータを更新する
private void UpdateHandFrame()
{
handData.Update();
// 画像の初期化
ImageHand1.Source = ImageHand2.Source = null;
// 検出した手の数を取得する
var numOfHands = handData.QueryNumberOfHands();
for (int i = 0; i < numOfHands; i++)
{
// 手を取得する
PXCMHandData.IHand hand;
var sts = handData.QueryHandData(
PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_ID, i, out hand);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// 手の画像を取得する
PXCMImage image;
sts = hand.QuerySegmentationImage(out image);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// マスク画像を取得する
PXCMImage.ImageData data;
sts = image.AcquireAccess(PXCMImage.Access.ACCESS_READ,
PXCMImage.PixelFormat.PIXEL_FORMAT_Y8, out data);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// マスク画像のサイズはDepthに依存
// 手は2つまで
var info = image.QueryInfo();
var bitmap = BitmapSource.Create(info.width, info.height, 96, 96,
PixelFormats.Gray8, null, data.planes[0],
data.pitches[0] * info.height, data.pitches[0]);
if (i == 0)
{
ImageHand1.Source = bitmap;
}
else
{
ImageHand2.Source = bitmap;
}
image.ReleaseAccess(data);
}
}
/* Displaying current frames hand joints */
private void TrackJoints(PXCMHandData handOutput)
{
numHands = handOutput.QueryNumberOfHands();
for (int i = 0; i < numHands; i++)
{
//Get hand by time of appearence
if (handOutput.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, i, out handData[i]) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
}
}
}
PXCMHandData.JointData[,] getHandInfo()
{
int handNum = handData.QueryNumberOfHands();
PXCMHandData.JointData[,] nodes = new PXCMHandData.JointData[handNum, PXCMHandData.NUMBER_OF_JOINTS];
// Iterate through hands
for (int i = 0; i < handNum; i++)
{
// Get hand joints by time of appearance
PXCMHandData.IHand ihandData;
if (handData.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, i, out ihandData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
for (int j = 0; j < PXCMHandData.NUMBER_OF_JOINTS; j++)
{
ihandData.QueryTrackedJoint((PXCMHandData.JointType)j, out nodes[i, j]);
}
}
}
return(nodes);
}
/* Displaying current frames hand joints */
private void DisplayJoints(PXCMHandData handOutput, long timeStamp = 0)
{
if (form.GetJointsState() || form.GetSkeletonState())
{
//Iterate hands
PXCMHandData.JointData[][] nodes = new PXCMHandData.JointData[][] { new PXCMHandData.JointData[0x20], new PXCMHandData.JointData[0x20] };
int numOfHands = handOutput.QueryNumberOfHands();
if (numOfHands > 1)
{
numOfHands = 1;
}
for (int i = 0; i < numOfHands; i++)
{
//Get hand by time of appearence
//PXCMHandAnalysis.HandData handData = new PXCMHandAnalysis.HandData();
PXCMHandData.IHand handData;
if (handOutput.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, i, out handData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
if (handData != null)
{
//Iterate Joints
float xsum = 0;
float ysum = 0;
int num = 0;
for (int j = 0; j < 0x20; j++)
{
PXCMHandData.JointData jointData;
handData.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData);
nodes[i][j] = jointData;
if (jointData.positionImage.x > 0 && jointData.positionImage.y > 0)
{
xsum += jointData.positionImage.x;
ysum += jointData.positionImage.y;
num++;
}
} // end iterating over joints
float xmean = xsum / num;
float ymean = ysum / num;
//Console.WriteLine(String.Format("({0},{0})",xsum/num ,ysum/num ));
form.DisplayHandPointAndPressKey(xmean, ymean);
}
}
} // end itrating over hands
form.DisplayJoints(nodes, numOfHands);
}
else
{
form.DisplayJoints(null, 0);
}
}
// 手のデータを更新する
private void UpdateHandFrame()
{
// 手のデータを更新する
handData.Update();
// データを初期化する
CanvasFaceParts.Children.Clear();
// 点に色を付ける
Brush[] colors = new Brush[] {
Brushes.Blue,
Brushes.Green,
Brushes.Red,
Brushes.Yellow,
Brushes.Purple,
Brushes.LightBlue,
};
// 検出した手の数を取得する
var numOfHands = handData.QueryNumberOfHands();
for (int i = 0; i < numOfHands; i++)
{
// 手を取得する
PXCMHandData.IHand hand;
var sts = handData.QueryHandData(
PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_ID, i, out hand);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// 指の関節を列挙する
for (int j = 0; j < PXCMHandData.NUMBER_OF_EXTREMITIES; j++)
{
PXCMHandData.ExtremityData extremityData;
sts = hand.QueryExtremityPoint((PXCMHandData.ExtremityType)j,
out extremityData);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
AddEllipse(CanvasFaceParts,
new Point(extremityData.pointImage.x, extremityData.pointImage.y),
10, colors[j], -1);
}
}
}
// 手のデータを更新する
private void UpdateHandFrame()
{
// 手のデータを更新する
handData.Update();
// データを初期化する
CanvasFaceParts.Children.Clear();
// 検出した手の数を取得する
var numOfHands = handData.QueryNumberOfHands();
for (int i = 0; i < numOfHands; i++)
{
// 手を取得する
PXCMHandData.IHand hand;
var sts = handData.QueryHandData(
PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_ID, i, out hand);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// 指の関節を列挙する
for (int j = 0; j < PXCMHandData.NUMBER_OF_JOINTS; j++)
{
// 指のデータを取得する
PXCMHandData.JointData jointData;
sts = hand.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// Depth座標系をカラー座標系に変換する
var depthPoint = new PXCMPoint3DF32[1];
var colorPoint = new PXCMPointF32[1];
depthPoint[0].x = jointData.positionImage.x;
depthPoint[0].y = jointData.positionImage.y;
depthPoint[0].z = jointData.positionWorld.z * 1000;
projection.MapDepthToColor(depthPoint, colorPoint);
AddEllipse(CanvasFaceParts,
new Point(colorPoint[0].x, colorPoint[0].y),
5, Brushes.Green);
}
}
}
// 手のデータを更新する
private void UpdateHandFrame()
{
// 手のデータを更新する
handData.Update();
// データを初期化する
CanvasFaceParts.Children.Clear();
// 検出した手の数を取得する
var numOfHands = handData.QueryNumberOfHands();
for (int i = 0; i < numOfHands; i++)
{
// 手を取得する
PXCMHandData.IHand hand;
var sts = handData.QueryHandData(
PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_ID, i, out hand);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// 指の関節を列挙する
for (int j = 0; j < PXCMHandData.NUMBER_OF_JOINTS; j++)
{
PXCMHandData.JointData jointData;
sts = hand.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
AddEllipse(CanvasFaceParts,
new Point(jointData.positionImage.x, jointData.positionImage.y),
5, Brushes.Green);
}
}
}
private void SaveHandData(PXCMHandData handAnalysis)
{
int numOfHands = handAnalysis.QueryNumberOfHands();
for (int j = 0; j < numOfHands; j++)
{
int id;
PXCMImage.ImageData data;
handAnalysis.QueryHandId(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, j, out id);
//Get hand by time of appearance
PXCMHandData.IHand handData;
handAnalysis.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, j, out handData);
if (handData != null)
{
HandData hd = new HandData();
hd.updateData(handData);
dbhelper.saveEntity(hd);
}
}
}
// 手のデータを更新する
private void UpdateHandFrame()
{
// 手のデータを更新する
handData.Update();
// ピクセルデータを初期化する
Array.Clear(imageBuffer, 0, imageBuffer.Length);
// 検出した手の数を取得する
var numOfHands = handData.QueryNumberOfHands();
for (int i = 0; i < numOfHands; i++)
{
// 手を取得する
PXCMHandData.IHand hand;
var sts = handData.QueryHandData(
PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_ID, i, out hand);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// 手の画像を取得する
PXCMImage image;
sts = hand.QuerySegmentationImage(out image);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// マスク画像を取得する
PXCMImage.ImageData data;
sts = image.AcquireAccess(PXCMImage.Access.ACCESS_READ,
PXCMImage.PixelFormat.PIXEL_FORMAT_Y8, out data);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// マスク画像のサイズはDepthに依存
// 手は2つまで
var info = image.QueryInfo();
// マスク画像をバイト列に変換する
var buffer = data.ToByteArray(0, data.pitches[0] * info.height);
for (int j = 0; j < info.height * info.width; ++j)
{
if (buffer[j] != 0)
{
var index = j * BYTE_PER_PIXEL;
// 手のインデックスで色を決める
// ID=0：127
// ID=1：254
var value = (byte)((i + 1) * 127);
imageBuffer[index + 0] = value;
imageBuffer[index + 1] = value;
imageBuffer[index + 2] = value;
}
}
image.ReleaseAccess(data);
}
// ピクセルデータを更新する
imageBitmap.WritePixels(imageRect, imageBuffer,
DEPTH_WIDTH * BYTE_PER_PIXEL, 0);
}
public void ProcessingThread()
{
// Start AcquireFrame/ReleaseFrame loop
while (senseManager.AcquireFrame(true) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
hand = senseManager.QueryHand();
if (hand != null)
{
// Retrieve the most recent processed data
handData = hand.CreateOutput();
handData.Update();
// Get number of tracked hands
nhands = handData.QueryNumberOfHands();
if (nhands > 0)
{
// Retrieve hand identifier
handData.QueryHandId(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, 0, out handId);
// Retrieve hand data
handData.QueryHandDataById(handId, out ihand);
PXCMHandData.BodySideType bodySideType = ihand.QueryBodySide();
if (bodySideType == PXCMHandData.BodySideType.BODY_SIDE_LEFT)
{
leftHand = true;
}
else if (bodySideType == PXCMHandData.BodySideType.BODY_SIDE_RIGHT)
{
leftHand = false;
}
// Retrieve all hand joint data
for (int i = 0; i < nhands; i++)
{
for (int j = 0; j < 0x20; j++)
{
PXCMHandData.JointData jointData;
ihand.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData);
nodes[i][j] = jointData;
}
}
// Get world coordinates for tip of middle finger on the first hand in camera range
handTipX = nodes[0][Convert.ToInt32(PXCMHandData.JointType.JOINT_MIDDLE_TIP)].positionWorld.x;
handTipY = nodes[0][Convert.ToInt32(PXCMHandData.JointType.JOINT_MIDDLE_TIP)].positionWorld.y;
handTipZ = nodes[0][Convert.ToInt32(PXCMHandData.JointType.JOINT_MIDDLE_TIP)].positionWorld.z;
swipehandTipX = nodes[0][Convert.ToInt32(PXCMHandData.JointType.JOINT_MIDDLE_TIP)].positionImage.x;
swipehandTipY = nodes[0][Convert.ToInt32(PXCMHandData.JointType.JOINT_MIDDLE_TIP)].positionImage.y;
swipehandTipZ = nodes[0][Convert.ToInt32(PXCMHandData.JointType.JOINT_MIDDLE_TIP)].positionImage.z;
//Console.Out.WriteLine("Before x={0}", swipehandTipX);
//Console.Out.WriteLine("Before speed={0}", nodes[0][Convert.ToInt32(PXCMHandData.JointType.JOINT_MIDDLE_TIP)].speed.x);
// Retrieve gesture data
if (handData.IsGestureFired("spreadfingers", out gestureData))
{
gesture = Gesture.FingerSpread;
}
else if (handData.IsGestureFired("two_fingers_pinch_open", out gestureData))
{
gesture = Gesture.Pinch;
}
else if (handData.IsGestureFired("wave", out gestureData))
{
gesture = Gesture.Wave;
}
else if (handData.IsGestureFired("swipe_left", out gestureData))
{
gesture = Gesture.SwipeLeft;
}
else if (handData.IsGestureFired("swipe_right", out gestureData))
{
gesture = Gesture.SwipeRight;
}
else if (handData.IsGestureFired("fist", out gestureData))
{
gesture = Gesture.Fist;
}
else if (handData.IsGestureFired("thumb_up", out gestureData))
{
gesture = Gesture.Thumb;
}
}
else
{
gesture = Gesture.Undefined;
}
UpdateUI();
if (handData != null)
{
handData.Dispose();
}
}
senseManager.ReleaseFrame();
}
}
private void ProcessingHandThread()
{
// Start AcquireFrame/ReleaseFrame loop
while (senseManager.AcquireFrame(true) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
hand = senseManager.QueryHand();
if (hand != null)
{
// Retrieve the most recent processed data
handData = hand.CreateOutput();
handData.Update();
// Get number of tracked hands
nhands = handData.QueryNumberOfHands();
if (nhands > 0)
{
// Retrieve hand identifier
handData.QueryHandId(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, 0, out handId);
// Retrieve hand data
handData.QueryHandDataById(handId, out ihand);
// Retrieve all hand joint data
for (int i = 0; i < nhands; i++)
{
for (int j = 0; j < 0x20; j++)
{
PXCMHandData.JointData jointData;
ihand.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData);
nodes[i][j] = jointData;
}
}
// Get world coordinates for tip of middle finger on the first hand in camera range
handTipX = nodes[0][Convert.ToInt32(PXCMHandData.JointType.JOINT_MIDDLE_TIP)].positionWorld.x;
handTipY = nodes[0][Convert.ToInt32(PXCMHandData.JointType.JOINT_MIDDLE_TIP)].positionWorld.y;
handTipZ = nodes[0][Convert.ToInt32(PXCMHandData.JointType.JOINT_MIDDLE_TIP)].positionWorld.z;
// Retrieve gesture data
if (handData.IsGestureFired("spreadfingers", out gestureData))
{
gesture = Gesture.FingerSpread;
}
else if (handData.IsGestureFired("two_fingers_pinch_open", out gestureData))
{
gesture = Gesture.Pinch;
}
else if (handData.IsGestureFired("wave", out gestureData))
{
gesture = Gesture.Wave;
}
}
else
{
gesture = Gesture.Undefined;
}
// Get alert status
for (int i = 0; i < handData.QueryFiredAlertsNumber(); i++)
{
PXCMHandData.AlertData alertData;
if (handData.QueryFiredAlertData(i, out alertData) != pxcmStatus.PXCM_STATUS_NO_ERROR) { continue; }
//Displaying last alert
switch (alertData.label)
{
case PXCMHandData.AlertType.ALERT_HAND_DETECTED:
detectionAlert = "Hand Detected";
detectionStatusOk = true;
break;
case PXCMHandData.AlertType.ALERT_HAND_NOT_DETECTED:
detectionAlert = "Hand Not Detected";
detectionStatusOk = false;
break;
case PXCMHandData.AlertType.ALERT_HAND_CALIBRATED:
calibrationAlert = "Hand Calibrated";
calibrationStatusOk = true;
break;
case PXCMHandData.AlertType.ALERT_HAND_NOT_CALIBRATED:
calibrationAlert = "Hand Not Calibrated";
calibrationStatusOk = false;
break;
case PXCMHandData.AlertType.ALERT_HAND_INSIDE_BORDERS:
bordersAlert = "Hand Inside Borders";
borderStatusOk = true;
break;
case PXCMHandData.AlertType.ALERT_HAND_OUT_OF_BORDERS:
bordersAlert = "Hand Out Of Borders";
borderStatusOk = false;
break;
}
}
UpdateUI();
if (handData != null) handData.Dispose();
}
senseManager.ReleaseFrame();
}
}
private static void ProcessHands(PXCMHandData handData)
{
// Querying how many hands were detected
int numberOfHands = handData.QueryNumberOfHands();
//Console.WriteLine("{0} hand(s) were detected.", numberOfHands);
// Querying the information about detected hands
for (int i = 0; i < numberOfHands; i++)
{
// Querying hand id
int handId;
pxcmStatus queryHandIdStatus = handData.QueryHandId(PXCMHandData.AccessOrderType.ACCESS_ORDER_NEAR_TO_FAR, i, out handId);
if (queryHandIdStatus != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Console.WriteLine("Failed to query the hand Id.");
continue;
}
//Console.WriteLine("Hand id: {0}", handId);
// Querying the hand data
PXCMHandData.IHand hand;
pxcmStatus queryHandStatus = handData.QueryHandDataById(handId, out hand);
if (queryHandStatus == pxcmStatus.PXCM_STATUS_NO_ERROR && hand != null)
{
// Querying the body side (Left/Right)
PXCMHandData.BodySideType bodySide = hand.QueryBodySide();
//Console.WriteLine("Body Side: {0}", bodySide);
// Querying the hand openness
int handOpenness = hand.QueryOpenness();
//Console.WriteLine("Hand openness: {0}", handOpenness);
SendMqttMessage(String.Format("red?{0}", (255 * handOpenness) / 100));
// Querying Hand 2D Position
PXCMPointF32 massCenterImage = hand.QueryMassCenterImage();
//Console.WriteLine("Hand position on image: {0} | {1}", massCenterImage.x, massCenterImage.y);
PXCMPoint4DF32 palmOrientation = hand.QueryPalmOrientation();
rotationHelper.SetFromQuaternion(palmOrientation);
PXCMPoint3DF32 rotationEuler = rotationHelper.QueryEulerAngles();
double angleInDegrees = (180 * rotationEuler.y / Math.PI) + 180;
Console.WriteLine("Angle in degrees: {0}", angleInDegrees);
SendMqttMessage(String.Format("red?{0}", (int)angleInDegrees));
// Console.WriteLine("Rotation x:{0},y:{1},z:{2}", rotationEuler.x, rotationEuler.y, rotationEuler.z);
// Querying Hand 3D Position
PXCMPoint3DF32 massCenterWorld = hand.QueryMassCenterWorld();
//Console.WriteLine("Hand position on world: {0} | {1} | {2}", massCenterWorld.x, massCenterWorld.y, massCenterWorld.z);
/*
* // Querying Hand Joints
* if (hand.HasTrackedJoints())
* {
* foreach (PXCMHandData.JointType jointType in Enum.GetValues(typeof(PXCMHandData.JointType)))
* {
* PXCMHandData.JointData jointData;
* pxcmStatus queryStatus = hand.QueryTrackedJoint(jointType, out jointData);
*
* if (queryStatus == pxcmStatus.PXCM_STATUS_NO_ERROR && jointData != null)
* {
* // Printing joint label and tracking confidence
* Console.WriteLine("Joint {0} with confidence {1}", jointType, jointData.confidence);
*
* // Printing the 2D position (image)
* Console.WriteLine("\t2D Position: {0} | {1}", jointData.positionImage.x, jointData.positionImage.y);
*
* // Printing the 3D position (depth)
* Console.WriteLine("\t3D Position: {0} | {1} | {2}", jointData.positionWorld.x, jointData.positionWorld.y, jointData.positionWorld.z);
* }
* }
* }*/
}
//Console.WriteLine("----------");
//Console.Clear();
}
}
/* Displaying Depth/Mask Images - for depth image only we use a delay of NumberOfFramesToDelay to sync image with tracking */
private unsafe void DisplayPicture(PXCMImage depth, PXCMHandData handAnalysis)
{
if (depth == null)
{
return;
}
PXCMImage image = depth;
//Mask Image
if (form.GetLabelmapState())
{
Bitmap labeledBitmap = null;
try
{
labeledBitmap = new Bitmap(image.info.width, image.info.height, PixelFormat.Format32bppRgb);
}
catch (Exception)
{
image.Dispose();
return;
}
for (int j = 0; j < handAnalysis.QueryNumberOfHands(); j++)
{
int id;
PXCMImage.ImageData data;
handAnalysis.QueryHandId(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, j, out id);
//Get hand by time of appearance
PXCMHandData.IHand handData;
handAnalysis.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, j, out handData);
if (handData != null &&
(handData.QuerySegmentationImage(out image) >= pxcmStatus.PXCM_STATUS_NO_ERROR))
{
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_Y8,
out data) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Rectangle rect = new Rectangle(0, 0, image.info.width, image.info.height);
BitmapData bitmapdata = labeledBitmap.LockBits(rect, ImageLockMode.ReadWrite, labeledBitmap.PixelFormat);
byte * numPtr = (byte *)bitmapdata.Scan0; //dst
byte * numPtr2 = (byte *)data.planes[0]; //row
int imagesize = image.info.width * image.info.height;
byte num2 = (byte)handData.QueryBodySide();
byte tmp = 0;
for (int i = 0; i < imagesize; i++, numPtr += 4, numPtr2++)
{
tmp = (byte)(LUT[numPtr2[0]] * num2 * 100);
numPtr[0] = (Byte)(tmp | numPtr[0]);
numPtr[1] = (Byte)(tmp | numPtr[1]);
numPtr[2] = (Byte)(tmp | numPtr[2]);
numPtr[3] = 0xff;
}
bool isError = false;
try
{
labeledBitmap.UnlockBits(bitmapdata);
}
catch (Exception)
{
isError = true;
}
try
{
image.ReleaseAccess(data);
}
catch (Exception)
{
isError = true;
}
if (isError)
{
labeledBitmap.Dispose();
image.Dispose();
return;
}
}
}
}
if (labeledBitmap != null)
{
form.DisplayBitmap(labeledBitmap);
labeledBitmap.Dispose();
}
image.Dispose();
}//end label image
//Depth Image
else
{
//collecting 3 images inside a queue and displaying the oldest image
PXCMImage.ImageInfo info;
PXCMImage image2;
info = image.QueryInfo();
image2 = form.g_session.CreateImage(info);
image2.CopyImage(image);
m_images.Enqueue(image2);
if (m_images.Count == NumberOfFramesToDelay)
{
Bitmap depthBitmap;
try
{
depthBitmap = new Bitmap(image.info.width, image.info.height, PixelFormat.Format32bppRgb);
}
catch (Exception)
{
image.Dispose();
PXCMImage queImage = m_images.Dequeue();
queImage.Dispose();
return;
}
PXCMImage.ImageData data3;
PXCMImage image3 = m_images.Dequeue();
if (image3.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, out data3) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
float fMaxValue = _maxRange;
byte cVal;
Rectangle rect = new Rectangle(0, 0, image.info.width, image.info.height);
BitmapData bitmapdata = depthBitmap.LockBits(rect, ImageLockMode.ReadWrite, depthBitmap.PixelFormat);
byte * pDst = (byte *)bitmapdata.Scan0;
short *pSrc = (short *)data3.planes[0];
int size = image.info.width * image.info.height;
for (int i = 0; i < size; i++, pSrc++, pDst += 4)
{
cVal = (byte)((*pSrc) / fMaxValue * 255);
if (cVal != 0)
{
cVal = (byte)(255 - cVal);
}
pDst[0] = cVal;
pDst[1] = cVal;
pDst[2] = cVal;
pDst[3] = 255;
}
try
{
depthBitmap.UnlockBits(bitmapdata);
}
catch (Exception)
{
image3.ReleaseAccess(data3);
depthBitmap.Dispose();
image3.Dispose();
return;
}
form.DisplayBitmap(depthBitmap);
image3.ReleaseAccess(data3);
}
depthBitmap.Dispose();
image3.Dispose();
}
}
}
/* Displaying current frames hand joints */
private void DisplayJoints(PXCMHandData handOutput, long timeStamp = 0)
{
if (form.GetJointsState() || form.GetSkeletonState())
{
//Iterate hands
PXCMHandData.JointData[][] nodes = new PXCMHandData.JointData[][] { new PXCMHandData.JointData[0x20], new PXCMHandData.JointData[0x20] };
int numOfHands = handOutput.QueryNumberOfHands();
for (int i = 0; i < numOfHands; i++)
{
//Get hand by time of appearence
//PXCMHandAnalysis.HandData handData = new PXCMHandAnalysis.HandData();
PXCMHandData.IHand handData;
if (handOutput.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, i, out handData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
if (handData != null)
{
Coordinate coor;
string handSide = "Unknown Hand";
handSide = handData.QueryBodySide() == PXCMHandData.BodySideType.BODY_SIDE_LEFT ? "Left Hand" : "Right Hand";
switch (handSide){
case("Left Hand"):
coor.handside = 0;
Rnum++;
break;
case("Right Hand"):
coor.handside = 1;
Lnum++;
break;
default:
coor.handside = 2;
break;
}
//Iterate Joints
for (int j = 0; j < 0x20; j++)
{
PXCMHandData.JointData jointData;
handData.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData);
nodes[i][j] = jointData;
if (handData.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData) == pxcmStatus.PXCM_STATUS_NO_ERROR && Handsinfo.JointToInt((PXCMHandData.JointType)j) != -1)
{
coor.finger = Handsinfo.JointToInt((PXCMHandData.JointType)j);
coor.x = jointData.positionWorld.x;
coor.y = jointData.positionWorld.y;
coor.z = jointData.positionWorld.z;
//right
if (coor.finger == 1 && coor.handside == 1)// && Rnum == 1)
{
sr.WriteLine(coor.x + " " + coor.y + " " + coor.z + "\n");
MYPoint p = new MYPoint(coor.x, coor.y, coor.z);
int num = rightcp.checkNote(p);
if (num != 0 && Rnum < 2 && Lnum < 2)
{
DateTime DateTime2 = DateTime.Now;
diff = tim.ExecTimeDiff(starttmp, DateTime2);
//form.UpdateInfo("play sound" + num.ToString(), Color.RoyalBlue);
playwave.play(num);
playwave.savehistory(num, (int)diff);
}
}
//left
if (coor.finger == 1 && coor.handside == 0)// && Lnum == 1)
{
sr.WriteLine(coor.x + " " + coor.y + " " + coor.z + "\n");
MYPoint p = new MYPoint(coor.x, coor.y, coor.z);
int num = leftcp.checkNote(p);
if (num != 0 && Rnum < 2 && Lnum < 2)
{
DateTime DateTime2 = DateTime.Now;
diff = tim.ExecTimeDiff(starttmp, DateTime2);
//form.UpdateInfo("play sound" + num.ToString(), Color.RoyalBlue);
playwave.play(num);
playwave.savehistory(num, (int)diff);
}
}
}
} // end iterating over joints
}
} // end itrating over hands
}
Lnum = 0; Rnum = 0;
form.DisplayJoints(nodes, numOfHands);
}
else
{
form.DisplayJoints(null, 0);
}
}
/* Displaying current frames hand joints */
private void DisplayJoints(PXCMHandData handOutput, long timeStamp = 0)
{
//Iterate hands
PXCMHandData.JointData[][] nodes = new PXCMHandData.JointData[][] { new PXCMHandData.JointData[0x20], new PXCMHandData.JointData[0x20] };
int numOfHands = handOutput.QueryNumberOfHands();
for (int i = 0; i < numOfHands; i++)
{
//Get hand by time of appearence
PXCMHandData.IHand handData;
if (handOutput.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, i, out handData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
if (handData != null)
{
Coordinate coor;
string handSide = "Unknown Hand";
handSide = handData.QueryBodySide() == PXCMHandData.BodySideType.BODY_SIDE_LEFT ? "Left Hand" : "Right Hand";
switch (handSide)
{
case ("Left Hand"): { coor.handside = 0; Rnum++; break; }
case ("Right Hand"): { coor.handside = 1; Lnum++; break; }
default: { coor.handside = 2; break; }
}
//Iterate Joints
for (int j = 0; j < 0x20; j++)
{
sr = File.CreateText(FILE_NAME);
PXCMHandData.JointData jointData;
handData.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData);
nodes[i][j] = jointData;
if (handData.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData) == pxcmStatus.PXCM_STATUS_NO_ERROR && Handsinfo.JointToInt((PXCMHandData.JointType)j) != -1)
{
coor.finger = Handsinfo.JointToInt((PXCMHandData.JointType)j);
coor.x = jointData.positionWorld.x;
coor.y = jointData.positionWorld.y;
coor.z = jointData.positionWorld.z;
//right
if (coor.finger == 1 && coor.handside == 1)
{
this.RHandMove(coor.x, coor.y);
sr.WriteLine(coor.x + " " + coor.y + " " + coor.z + "\n");
MYPoint p = new MYPoint(coor.x, coor.y, coor.z);
int num = rightcp.checkNote(p);
if (num != 0 && Rnum < 2 && Lnum < 2)
{
DateTime DateTime2 = DateTime.Now;
diff = tim.ExecTimeDiff(starttmp, DateTime2);
this.HandClick(num, (int)diff);
}
}
//left
if (coor.finger == 1 && coor.handside == 0)
{
this.LHandMove(coor.x, coor.y);
sr.WriteLine(coor.x + " " + coor.y + " " + coor.z + "\n");
MYPoint p = new MYPoint(coor.x, coor.y, coor.z);
int num = leftcp.checkNote(p);
if (num != 0 && Rnum < 2 && Lnum < 2)
{
DateTime DateTime2 = DateTime.Now;
diff = tim.ExecTimeDiff(starttmp, DateTime2);
this.HandClick(num, (int)diff);
}
}
}
sr.Close();
} // end iterating over joints
}
} // end itrating over hands
}
Lnum = 0; Rnum = 0;
}
/* Displaying current frames hand joints */
private void DisplayJoints(PXCMHandData handOutput, long timeStamp = 0)
{
if (form.GetJointsState() || form.GetSkeletonState())
{
//Iterate hands
PXCMHandData.JointData[][] nodes = new PXCMHandData.JointData[][] { new PXCMHandData.JointData[0x20], new PXCMHandData.JointData[0x20] };
int numOfHands = handOutput.QueryNumberOfHands();
for (int i = 0; i < numOfHands; i++)
{
//Get hand by time of appearence
//PXCMHandAnalysis.HandData handData = new PXCMHandAnalysis.HandData();
PXCMHandData.IHand handData;
if (handOutput.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, i, out handData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
if (handData != null)
{
//Iterate Joints
for (int j = 0; j < 0x20; j++)
{
PXCMHandData.JointData jointData;
handData.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData);
nodes[i][j] = jointData;
} // end iterating over joints
}
}
} // end itrating over hands
form.DisplayJoints(nodes, numOfHands);
DisplayGesturesNew(nodes, numOfHands);
}
else
{
form.DisplayJoints(null, 0);
}
}
// Update is called once per frame
void Update()
{
/* Make sure SenseManager Instance is valid */
if (sm == null)
{
return;
}
/* Wait until any frame data is available */
if (sm.AcquireFrame(false) != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
/* Retrieve hand tracking Module Instance */
handAnalyzer = sm.QueryHand();
if (handAnalyzer != null)
{
/* Retrieve hand tracking Data */
PXCMHandData _handData = handAnalyzer.CreateOutput();
if (_handData != null)
{
_handData.Update();
/* Retrieve all joint Data */
bool[] someJointsDetected = { false, false };
for (int i = 0; i < _handData.QueryNumberOfHands(); i++)
{
PXCMHandData.IHand _iHand;
if (_handData.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_FIXED, i, out _iHand) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
for (int j = 0; j < MaxJoints; j++)
{
if (_iHand.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData[i, j]) != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
jointData[i, j] = null;
}
else if (jointData[i, j].confidence == 100)
{
someJointsDetected[i] = true;
}
}
if (!handList.ContainsKey(_iHand.QueryUniqueId()))
{
handList.Add(_iHand.QueryUniqueId(), _iHand.QueryBodySide());
}
}
}
for (int i = 0; i < MaxHands; i++)
{
if (!someJointsDetected[i] && handWasTracked[i])
{
for (int j = 0; j < MaxJoints; j++)
{
Joint joint = myJoints[i, j];
if (joint == null)
{
continue;
}
joint.transform.position = joint.originalPosition;
joint.transform.rotation = joint.originalRotation;
foreach (Joint e in joint.extensions)
{
e.transform.position = e.originalPosition;
e.transform.rotation = e.originalRotation;
}
}
}
else
{
if (!handWasTracked[i])
{
if (myJoints[i, 0] == null || jointData[i, 0] == null || jointData[i, 0].confidence < 100)
{
break;
}
PXCMPoint3DF32 smoothedPoint = smoother3D[i, 0].SmoothValue(jointData[i, 0].positionWorld);
Vector3 targetPosition = new Vector3(-1 * smoothedPoint.x, smoothedPoint.y, smoothedPoint.z);
trackingOffset[i] = targetPosition - myJoints[i, 0].originalPosition;
}
/* Smooth the data and move the joints*/
MoveJoints(i);
}
handWasTracked[i] = someJointsDetected[i];
}
}
handAnalyzer.Dispose();
}
sm.ReleaseFrame();
RotateCam();
}
private static void ProcessHands(PXCMHandData handData)
{
// Querying how many hands were detected
int numberOfHands = handData.QueryNumberOfHands();
//Console.WriteLine("{0} hand(s) were detected.", numberOfHands);
// Querying the information about detected hands
for (int i = 0; i < numberOfHands; i++)
{
// Querying hand id
int handId;
pxcmStatus queryHandIdStatus = handData.QueryHandId(PXCMHandData.AccessOrderType.ACCESS_ORDER_NEAR_TO_FAR, i, out handId);
if (queryHandIdStatus != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Console.WriteLine("Failed to query the hand Id.");
continue;
}
//Console.WriteLine("Hand id: {0}", handId);
// Querying the hand data
PXCMHandData.IHand hand;
pxcmStatus queryHandStatus = handData.QueryHandDataById(handId, out hand);
if (queryHandStatus == pxcmStatus.PXCM_STATUS_NO_ERROR && hand != null)
{
// Querying the body side (Left/Right)
PXCMHandData.BodySideType bodySide = hand.QueryBodySide();
//Console.WriteLine("Body Side: {0}", bodySide);
// Querying the hand openness
int handOpenness = hand.QueryOpenness();
//Console.WriteLine("Hand openness: {0}", handOpenness);
//SendMqttMessage(String.Format("surfboard2/red?{0}", (255 *handOpenness)/100));
// Querying Hand 2D Position
PXCMPointF32 massCenterImage = hand.QueryMassCenterImage();
//Console.WriteLine("Hand position on image: {0} | {1}", massCenterImage.x, massCenterImage.y);
PXCMPoint4DF32 palmOrientation = hand.QueryPalmOrientation();
rotationHelper.SetFromQuaternion(palmOrientation);
PXCMPoint3DF32 rotationEuler = rotationHelper.QueryEulerAngles();
double angleInDegrees = (180 * rotationEuler.y / Math.PI) + 180;
Console.WriteLine("Angle in degrees: {0}", angleInDegrees);
SendMqttMessage(String.Format("servo?{0}", (int)angleInDegrees));
// Console.WriteLine("Rotation x:{0},y:{1},z:{2}", rotationEuler.x, rotationEuler.y, rotationEuler.z);
// Querying Hand 3D Position
PXCMPoint3DF32 massCenterWorld = hand.QueryMassCenterWorld();
//Console.WriteLine("Hand position on world: {0} | {1} | {2}", massCenterWorld.x, massCenterWorld.y, massCenterWorld.z);
/*
// Querying Hand Joints
if (hand.HasTrackedJoints())
{
foreach (PXCMHandData.JointType jointType in Enum.GetValues(typeof(PXCMHandData.JointType)))
{
PXCMHandData.JointData jointData;
pxcmStatus queryStatus = hand.QueryTrackedJoint(jointType, out jointData);
if (queryStatus == pxcmStatus.PXCM_STATUS_NO_ERROR && jointData != null)
{
// Printing joint label and tracking confidence
Console.WriteLine("Joint {0} with confidence {1}", jointType, jointData.confidence);
// Printing the 2D position (image)
Console.WriteLine("\t2D Position: {0} | {1}", jointData.positionImage.x, jointData.positionImage.y);
// Printing the 3D position (depth)
Console.WriteLine("\t3D Position: {0} | {1} | {2}", jointData.positionWorld.x, jointData.positionWorld.y, jointData.positionWorld.z);
}
}
}*/
}
//Console.WriteLine("----------");
//Console.Clear();
}
}
pxcmStatus newHandFrame(PXCMHandModule hand)
{
if (hand != null)
{
PXCMHandData handData = hand.CreateOutput();
handData.Update();
PXCMHandData.IHand iHandDataLeft = null, iHandDataRight = null;
PXCMHandData.JointData jointData = null;
PXCMImage image = null;
handData.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_LEFT_HANDS, 0, out iHandDataLeft);
handData.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_RIGHT_HANDS, 0, out iHandDataRight);
if (handForm != null && !handForm.IsDisposed)
{
this.handForm.HandCount = handData.QueryNumberOfHands();
if (iHandDataLeft != null)
{
iHandDataLeft.QuerySegmentationImage(out image);
if (image != null)
{
PXCMImage.ImageData data = new PXCMImage.ImageData();
image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out data);
handForm.LeftHand = data.ToBitmap(0, image.info.width, image.info.height);
image.ReleaseAccess(data);
}
}
if (iHandDataRight != null)
{
iHandDataRight.QuerySegmentationImage(out image);
if (image != null)
{
PXCMImage.ImageData data = new PXCMImage.ImageData();
image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out data);
handForm.RightHand = data.ToBitmap(0, image.info.width, image.info.height);
image.ReleaseAccess(data);
}
}
}
if (iHandDataLeft != null)
{
if (jointData == null)
{
iHandDataLeft.QueryTrackedJoint(PXCMHandData.JointType.JOINT_INDEX_TIP, out jointData);
}
}
if (iHandDataRight != null)
{
if (jointData == null)
{
iHandDataRight.QueryTrackedJoint(PXCMHandData.JointType.JOINT_INDEX_TIP, out jointData);
}
}
if (jointData != null && canTrack.Checked)
{
Cursor.Position = new System.Drawing.Point(
(int)((640.0f - jointData.positionImage.x) * Screen.PrimaryScreen.Bounds.Width / 640.0f),
(int)(jointData.positionImage.y * Screen.PrimaryScreen.Bounds.Height / 480.0f));
PXCMHandData.GestureData gestureData = null;
if (handData.IsGestureFired("two_fingers_pinch_open", out gestureData))
{
Program.DoMouseClick();
}
Console.WriteLine("Z Position: " + jointData.positionWorld.z);
}
handData.Dispose();
}
return(pxcmStatus.PXCM_STATUS_NO_ERROR);
}
/// <summary>
/// HandTrackingで取得したHandDataを受け取り
/// 画面にHandDataの情報を表示する
/// </summary>
private void DisplayHandTrackingData(PXCMHandData handData)
{
// SegmentationImageの情報格納用の変数
int segWbStride = 0;
Int32Rect segWbRect = new Int32Rect(0, 0, 0, 0);
Byte[] segImageBuffer = null;
// HandJointの情報格納用の変数
List<List<PXCMHandData.JointData>> handJointsList = new List<List<PXCMHandData.JointData>>();
for (int handIndex = 0; handIndex < handData.QueryNumberOfHands(); handIndex++)
{
// IHandDataを取得
PXCMHandData.IHand iHandData;
if (handData.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, handIndex, out iHandData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
// SegmentationImageを取得
PXCMImage image;
iHandData.QuerySegmentationImage(out image); // 取得出来る画像は8bitGrayスケール画像 手の部分が0xff(白) 背景が0x00(黒)
// Imageから画像データを取得
PXCMImage.ImageData data = null ;
pxcmStatus sts = image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_Y8, out data);
if (sts == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
// ImageDataが取得できたらWritableBitmapに書き込むためにバイト配列に変換する
int length = data.pitches[0] * image.info.height;
Byte[] tmpBuffer = data.ToByteArray(0, length);
// ストライドと描画領域を取得
segWbStride = data.pitches[0];
segWbRect = new Int32Rect(0, 0, image.info.width, image.info.height);
// Imageデータのアクセスを終了する
image.ReleaseAccess(data);
// HandSegmentationImageは複数ある可能性があるためすでにバイト配列を取得ずみの場合は重ね合わせる
if (segImageBuffer == null)
{
// まだない場合は、そのまま使用する
Array.Resize(ref segImageBuffer, tmpBuffer.Length);
tmpBuffer.CopyTo(segImageBuffer, 0);
}
else
{
// 既にひとつの手の情報がある場合は手の白部分(0xff)のみ重ね合わせる
for (int i=0; i<segImageBuffer.Length; i++)
{
segImageBuffer[i] = (byte)(segImageBuffer[i] | tmpBuffer[i]);
}
}
}
// TODO:後で取得してみる
//iHandData.QueryBoundingBoxImage // 手の領域
//iHandData.QueryMassCenterImage // 2D Image coordinatesでの手の中心座標
//iHandData.QueryMassCenterWorld // 3D World Coordinatesでの手の中心座標
//iHandData.QueryExtremityPoint // TODO:Extremitiesモードで取得してみる
// 1つの手のJointを入れるListを生成
List<PXCMHandData.JointData> jointList = new List<PXCMHandData.JointData>();
// 手のJoint座標を取得してListに格納
for (int jointIndex = 0; jointIndex < Enum.GetNames(typeof(PXCMHandData.JointType)).Length; jointIndex++)
{
// 手の1つのJoint座標を取得
PXCMHandData.JointData jointData;
iHandData.QueryTrackedJoint((PXCMHandData.JointType)jointIndex, out jointData);
jointList.Add(jointData);
}
// 作成した1つの手のJoint座標リストをListに格納
handJointsList.Add(jointList);
}
}
// SegmentationImageデータをバイト配列にしたものをWriteableBitmapに書き込む
if (segImageBuffer != null)
{
m_HandSegmentWBitmap.Dispatcher.BeginInvoke
(
new Action(() =>
{
m_HandSegmentWBitmap.WritePixels(segWbRect, segImageBuffer, segWbStride, 0);
}
));
}
// HandJointの座標を画面に表示
if (handJointsList.Count > 0)
{
m_ColorWBitmap.Dispatcher.BeginInvoke
(
new Action(() =>
{
foreach (List<PXCMHandData.JointData> jointList in handJointsList)
{
foreach (PXCMHandData.JointData joint in jointList)
{
PXCMPoint3DF32[] depthPoint = new PXCMPoint3DF32[1];
depthPoint[0].x = joint.positionImage.x;
depthPoint[0].y = joint.positionImage.y;
depthPoint[0].z = joint.positionWorld.z * 1000; // mmとpixcelを合わす
PXCMPointF32[] colorPoint = new PXCMPointF32[1];
pxcmStatus status = m_Projection.MapDepthToColor(depthPoint, colorPoint);
// 指の位置を描画
m_ColorWBitmap.FillEllipseCentered((int)colorPoint[0].x,
(int)colorPoint[0].y,
10, 10, Colors.YellowGreen);
}
}
}
));
}
m_HandJointWBitmap.Dispatcher.BeginInvoke
(
new Action(() =>
{
m_HandJointWBitmap.Clear();
foreach (List<PXCMHandData.JointData> jointList in handJointsList)
{
foreach (PXCMHandData.JointData joint in jointList)
{
m_HandJointWBitmap.FillEllipse(
(int)joint.positionImage.x, (int)joint.positionImage.y,
(int)joint.positionImage.x + 6, (int)joint.positionImage.y + 6, Colors.YellowGreen);
}
}
}
));
}
public Main.HandCoord[] GetJointCoordinates()
{
#region Catch exception errors
/* Wait until frame is available and SenseManager is ready */
/* Make sure SenseManager has an instance */
if (SenseManager == null)
{
return(null);
}
/* Wait until any frame data is available */
if (SenseManager.AcquireFrame(true) != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return(null);
}
#endregion
#region Get joint coordinates
HandModule = SenseManager.QueryHand(); // Get hand tracking Module instance
HandData = HandModule.CreateOutput(); // Get hand tracking data
if (HandData != null)
{
HandData.Update(); // Update hand data to the most current output
/*Reset the detected hands to zero*/
for (int i = 0; i < NumOfHands; i++)
{
Coordinates[i].HandDetected = false;
}
/* Retrieve all joint data */
for (int i = 0; i < HandData.QueryNumberOfHands(); i++)
{
Coordinates[i].HandDetected = true;
PXCMHandData.IHand iHand;
Status = HandData.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_FIXED, i, out iHand);
if (Status != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Debug.LogError(Status);
}
else
{
for (int j = 0; j < NumOfJoints; j++)
{
Status = iHand.QueryTrackedJoint((PXCMHandData.JointType)j, out JointData[i][j]);
if (Status == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Coordinates[i].Coordinates[0, j] = JointData[i][j].positionWorld.x * -100;
Coordinates[i].Coordinates[1, j] = JointData[i][j].positionWorld.y * 100;
Coordinates[i].Coordinates[2, j] = JointData[i][j].positionWorld.z * 100;
Coordinates[i].Coordinates[3, j] = JointData[i][j].speed.x * -100;
Coordinates[i].Coordinates[4, j] = JointData[i][j].speed.y * -100;
Coordinates[i].Coordinates[5, j] = JointData[i][j].speed.z * 100;
}
}
}
}
}
SenseManager.ReleaseFrame(); // Prepare for a new frame
return(Coordinates);
#endregion
}
/* Displaying Depth/Mask Images - for depth image only we use a delay of NumberOfFramesToDelay to sync image with tracking */
private unsafe void DisplayPicture(PXCMImage depth, PXCMHandData handAnalysis)
{
if (depth == null)
return;
PXCMImage image = depth;
//Mask Image
if (form.GetLabelmapState())
{
Bitmap labeledBitmap = null;
try
{
labeledBitmap = new Bitmap(image.info.width, image.info.height, PixelFormat.Format32bppRgb);
for (int j = 0; j < handAnalysis.QueryNumberOfHands(); j++)
{
int id;
PXCMImage.ImageData data;
handAnalysis.QueryHandId(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, j, out id);
//Get hand by time of appearance
PXCMHandData.IHand handData;
handAnalysis.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, j, out handData);
if (handData != null &&
(handData.QuerySegmentationImage(out image) >= pxcmStatus.PXCM_STATUS_NO_ERROR))
{
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_Y8,
out data) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Rectangle rect = new Rectangle(0, 0, image.info.width, image.info.height);
BitmapData bitmapdata = labeledBitmap.LockBits(rect, ImageLockMode.ReadWrite,
labeledBitmap.PixelFormat);
byte* numPtr = (byte*) bitmapdata.Scan0; //dst
byte* numPtr2 = (byte*) data.planes[0]; //row
int imagesize = image.info.width*image.info.height;
byte num2 = (byte) handData.QueryBodySide();
byte tmp = 0;
for (int i = 0; i < imagesize; i++, numPtr += 4, numPtr2++)
{
tmp = (byte) (LUT[numPtr2[0]]*num2*100);
numPtr[0] = (Byte) (tmp | numPtr[0]);
numPtr[1] = (Byte) (tmp | numPtr[1]);
numPtr[2] = (Byte) (tmp | numPtr[2]);
numPtr[3] = 0xff;
}
labeledBitmap.UnlockBits(bitmapdata);
image.ReleaseAccess(data);
}
}
}
if (labeledBitmap != null)
{
form.DisplayBitmap(labeledBitmap);
labeledBitmap.Dispose();
}
image.Dispose();
}
catch (Exception)
{
if (labeledBitmap != null)
{
labeledBitmap.Dispose();
}
if (image != null)
{
image.Dispose();
}
}
}//end label image
//Depth Image
else
{
//collecting 3 images inside a queue and displaying the oldest image
PXCMImage.ImageInfo info;
PXCMImage image2;
info = image.QueryInfo();
image2 = form.g_session.CreateImage(info);
if (image2 == null) { return; }
image2.CopyImage(image);
m_images.Enqueue(image2);
if (m_images.Count == NumberOfFramesToDelay)
{
Bitmap depthBitmap;
try
{
depthBitmap = new Bitmap(image.info.width, image.info.height, PixelFormat.Format32bppRgb);
}
catch (Exception)
{
image.Dispose();
PXCMImage queImage = m_images.Dequeue();
queImage.Dispose();
return;
}
PXCMImage.ImageData data3;
PXCMImage image3 = m_images.Dequeue();
if (image3.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, out data3) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
float fMaxValue = _maxRange;
byte cVal;
Rectangle rect = new Rectangle(0, 0, image.info.width, image.info.height);
BitmapData bitmapdata = depthBitmap.LockBits(rect, ImageLockMode.ReadWrite, depthBitmap.PixelFormat);
byte* pDst = (byte*)bitmapdata.Scan0;
short* pSrc = (short*)data3.planes[0];
int size = image.info.width * image.info.height;
for (int i = 0; i < size; i++, pSrc++, pDst += 4)
{
cVal = (byte)((*pSrc) / fMaxValue * 255);
if (cVal != 0)
cVal = (byte)(255 - cVal);
pDst[0] = cVal;
pDst[1] = cVal;
pDst[2] = cVal;
pDst[3] = 255;
}
try
{
depthBitmap.UnlockBits(bitmapdata);
}
catch (Exception)
{
image3.ReleaseAccess(data3);
depthBitmap.Dispose();
image3.Dispose();
return;
}
form.DisplayBitmap(depthBitmap);
image3.ReleaseAccess(data3);
}
depthBitmap.Dispose();
image3.Dispose();
}
}
}
// Update is called once per frame
void Update()
{
/* Make sure SenseManager Instance is valid */
if (sm == null)
{
return;
}
/* Wait until any frame data is available */
if (sm.AcquireFrame(false) != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
/* Retrieve hand tracking Module Instance */
handAnalyzer = sm.QueryHand();
if (handAnalyzer != null)
{
/* Retrieve hand tracking Data */
PXCMHandData _handData = handAnalyzer.CreateOutput();
if (_handData != null)
{
_handData.Update();
/* Retrieve Gesture Data to manipulate GUIText */
PXCMHandData.GestureData gestureData;
for (int i = 0; i < _handData.QueryFiredGesturesNumber(); i++)
{
if (_handData.QueryFiredGestureData(i, out gestureData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
DisplayGestures(gestureData);
}
}
/* Retrieve Alert Data to manipulate GUIText */
PXCMHandData.AlertData alertData;
for (int i = 0; i < _handData.QueryFiredAlertsNumber(); i++)
{
if (_handData.QueryFiredAlertData(i, out alertData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
ProcessAlerts(alertData);
}
}
/* Retrieve all joint Data */
for (int i = 0; i < _handData.QueryNumberOfHands(); i++)
{
PXCMHandData.IHand _iHand;
if (_handData.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_FIXED, i, out _iHand) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
for (int j = 0; j < MaxJoints; j++)
{
if (_iHand.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData[i][j]) != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
jointData[i][j] = null;
}
}
if (!handList.ContainsKey(_iHand.QueryUniqueId()))
{
handList.Add(_iHand.QueryUniqueId(), _iHand.QueryBodySide());
}
}
}
/* Smoothen and Display the Data - Joints and Bones*/
DisplayJoints();
}
handAnalyzer.Dispose();
}
sm.ReleaseFrame();
RotateCam();
}
private void getCursorPositions()
{
var numHands = mHand.QueryNumberOfHands();
int handId = -1;
if (numHands < 1)
{
mCursor0.transform.localPosition = Vector3.zero;
mCursor1.transform.localPosition = Vector3.zero;
}
if (!UseTwoHands)
{
mCursor1.transform.localPosition = Vector3.zero;
}
for (var i = 0; i < numHands; ++i)
{
mHand.QueryHandId(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_ID, i, out handId);
PXCMHandData.IHand handData;
if (mHand.QueryHandDataById(handId, out handData) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
if (handData.HasCursor())
{
PXCMHandData.ICursor cursor;
if (handData.QueryCursor(out cursor) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
var pos = cursor.QueryPointWorld();
if (MapZ)
{
var newZ = lmap(pos.z, ZRange.x, ZRange.y, ZRange.z, ZRange.w);
pos.z = newZ;
}
Vector3 newPos = new Vector3(pos.x, pos.y, pos.z) * CursorScale;
if (handData.QueryBodySide() == PXCMHandData.BodySideType.BODY_SIDE_RIGHT)
{
mCursor0.transform.localPosition = newPos;
if (getGestureState(PXCMHandData.BodySideType.BODY_SIDE_RIGHT))
{
mCursor0Active = !mCursor0Active;
if (mCursor0Active)
{
mCursor0.transform.localScale = new Vector3(CursorSize * 3, CursorSize * 3, CursorSize * 3);
}
else
{
mCursor0.transform.localScale = new Vector3(CursorSize, CursorSize, CursorSize);
}
}
}
else if (handData.QueryBodySide() == PXCMHandData.BodySideType.BODY_SIDE_LEFT && UseTwoHands)
{
mCursor1.transform.localPosition = newPos;
if (getGestureState(PXCMHandData.BodySideType.BODY_SIDE_LEFT))
{
mCursor1Active = !mCursor1Active;
if (mCursor1Active)
{
mCursor1.transform.localScale = new Vector3(CursorSize * 3, CursorSize * 3, CursorSize * 3);
}
else
{
mCursor1.transform.localScale = new Vector3(CursorSize, CursorSize, CursorSize);
}
}
}
}
}
else
{
if (i == 0)
{
mCursor0.transform.localPosition = Vector3.zero;
}
else if (i == 1)
{
mCursor1.transform.localPosition = Vector3.zero;
}
}
}
}
}
// 手のデータを更新する
private void UpdateHandFrame()
{
// 手のデータを更新する
handData.Update();
// ピクセルデータを初期化する
Array.Clear(imageBuffer, 0, imageBuffer.Length);
CanvasFaceParts.Children.Clear();
// 検出した手の数を取得する
var numOfHands = handData.QueryNumberOfHands();
for (int i = 0; i < numOfHands; i++)
{
// 手を取得する
PXCMHandData.IHand hand;
var sts = handData.QueryHandData(
PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_ID, i, out hand);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// 手の画像を取得する
PXCMImage image;
sts = hand.QuerySegmentationImage(out image);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// マスク画像を取得する
PXCMImage.ImageData data;
sts = image.AcquireAccess(PXCMImage.Access.ACCESS_READ,
PXCMImage.PixelFormat.PIXEL_FORMAT_Y8, out data);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// 手の左右を取得する
var side = (int)hand.QueryBodySide();
// 手の開閉度(0-100)を取得する
var openness = hand.QueryOpenness();
// マスク画像のサイズはDepthに依存
// 手は2つまで
var info = image.QueryInfo();
// マスク画像をバイト列に変換する
var buffer = data.ToByteArray(0, data.pitches[0] * info.height);
for (int j = 0; j < info.height * info.width; ++j)
{
if (buffer[j] != 0)
{
var index = j * BYTE_PER_PIXEL;
// 手の左右および手の開閉度で色合いを決める
// 左手=1：0-127の範囲
// 右手=2：0-254の範囲
var value = (byte)((side * 127) * (openness / 100.0f));
imageBuffer[index + 0] = value;
imageBuffer[index + 1] = value;
imageBuffer[index + 2] = value;
}
}
image.ReleaseAccess(data);
// 手の重心を表示する
var center = hand.QueryMassCenterImage();
var ellipse = new Ellipse()
{
Width = 5,
Height = 5,
Fill = Brushes.Blue,
};
Canvas.SetLeft(ellipse, center.x);
Canvas.SetTop(ellipse, center.y);
CanvasFaceParts.Children.Add(ellipse);
// 手の範囲を表示する
var boundingbox = hand.QueryBoundingBoxImage();
var rectangle = new Rectangle()
{
Width = boundingbox.w,
Height = boundingbox.h,
Stroke = Brushes.Red,
StrokeThickness = 2,
};
Canvas.SetLeft(rectangle, boundingbox.x);
Canvas.SetTop(rectangle, boundingbox.y);
CanvasFaceParts.Children.Add(rectangle);
}
// ピクセルデータを更新する
imageBitmap.WritePixels(imageRect, imageBuffer,
DEPTH_WIDTH * BYTE_PER_PIXEL, 0);
}
/* Displaying current frames hand joints */
private void DisplayJoints(PXCMHandData handOutput, long timeStamp = 0)
{
//if (form.GetJointsState() || form.GetSkeletonState())
//{
//Iterate hands
PXCMHandData.JointData[][] nodes = new PXCMHandData.JointData[][] { new PXCMHandData.JointData[0x20], new PXCMHandData.JointData[0x20] };
int numOfHands = handOutput.QueryNumberOfHands();
for (int i = 0; i < numOfHands; i++)
{
//Get hand by time of appearence
//PXCMHandAnalysis.HandData handData = new PXCMHandAnalysis.HandData();
String jointString = String.Empty;
PXCMHandData.IHand handData;
if (handOutput.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, i, out handData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
if (handData != null)
{
//Iterate Joints
for (int j = 0; j < 0x20; j++)
{
PXCMHandData.JointData jointData;
handData.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData);
nodes[i][j] = jointData;
} // end iterating over joints
//for (int j = 0; j < 22; j++)
//{
// Single x, y, z, w;
// x = nodes[i][j].localRotation.x;
// y = nodes[i][j].localRotation.y;
// z = nodes[i][j].localRotation.z;
// w = nodes[i][j].localRotation.w;
// jointString = i + "x" + j + ", " + x + "," + y + "," + z + "," + w +"\n";
// form.UpdateInfo(jointString, Color.Red);
//} // end iterating over joints
}
}
} // end itrating over hands
form.DisplayJoints(nodes, numOfHands);
//}
//else
//{
// form.DisplayJoints(null, 0);
//}
}
// Update is called once per frame
void Update()
{
// For testing only.
//handRenderer.queryLeftHand2DCoordinates ();
_outputData = SenseToolkitManager.Instance.HandDataOutput;
if (_outputData != null)
{
_outputData.Update();
/* Retrieve Hand Joints*/
joints = new PXCMHandData.JointData[2, PXCMHandData.NUMBER_OF_JOINTS];
for (int i = 0; i < _outputData.QueryNumberOfHands(); i++)
{
PXCMHandData.IHand _handData;
_outputData.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_FIXED, i, out _handData);
//Debug.Log(_handData.IsCalibrated().ToString());
//if(_handData!=null){
//else { Debug.Log ("No data"); continue; }
for (int j = 0; j < PXCMHandData.NUMBER_OF_JOINTS; j++)
{
_handData.QueryTrackedJoint((PXCMHandData.JointType)j, out joints[i, j]);
}
handIds[i] = _handData.QueryUniqueId();
bodySides[i] = _handData.QueryBodySide();
if (!_handData.IsCalibrated())
{
if (bodySides[i] == PXCMHandData.BodySideType.BODY_SIDE_LEFT)
{
isleftCalibrated = false;
}
else if (bodySides[i] == PXCMHandData.BodySideType.BODY_SIDE_RIGHT)
{
isrightCalibrated = false;
}
}
else
{
if (bodySides[i] == PXCMHandData.BodySideType.BODY_SIDE_LEFT)
{
isleftCalibrated = true;
}
else if (bodySides[i] == PXCMHandData.BodySideType.BODY_SIDE_RIGHT)
{
isrightCalibrated = true;
}
}
}
handRenderer.DisplaySmoothenedJoints(_outputData, joints, handIds, bodySides, _outputData.QueryNumberOfHands());
/* Retrieve Alert Data */
/*
* PXCMHandData.AlertData _alertData;
* for (int i = 0; i < _outputData.QueryFiredAlertsNumber(); i++)
* if (_outputData.QueryFiredAlertData(i, out _alertData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
* handRenderer.DisplayAlerts(_alertData);
*/
/* Retrieve Gesture Data */
PXCMHandData.GestureData _gestureData;
for (int i = 0; i < _outputData.QueryFiredGesturesNumber(); i++)
{
if (_outputData.QueryFiredGestureData(i, out _gestureData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
handRenderer.DisplayGestures(_gestureData);
}
}
if (_outputData.QueryNumberOfHands() == 0)
{
handRenderer.makeNull();
}
}
else
{
handRenderer.makeNull();
}
// For testing
//handRenderer.DisplayGest ();
displayHandGestures();
}
// 手のデータを更新する
private void UpdateHandFrame()
{
// 手のデータを更新する
handData.Update();
// データを初期化する
CanvasHandParts.Children.Clear();
// 検出した手の数を取得する
var numOfHands = handData.QueryNumberOfHands();
for (int i = 0; i < numOfHands; i++)
{
// 手を取得する
PXCMHandData.IHand hand;
var sts = handData.QueryHandData(
PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_ID, i, out hand);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// 指の関節を列挙する
for (int j = 0; j < PXCMHandData.NUMBER_OF_JOINTS; j++)
{
PXCMHandData.JointData jointData;
sts = hand.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
AddEllipse(CanvasHandParts,
new Point(jointData.positionImage.x, jointData.positionImage.y),
5, Brushes.Green);
}
}
// 認識したジェスチャーの数を取得する
var numOfGestures = handData.QueryFiredGesturesNumber();
for (int i = 0; i < numOfGestures; i++)
{
// 認識したジェスチャーを取得する
PXCMHandData.GestureData gesture;
var sts = handData.QueryFiredGestureData(i, out gesture);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// ジェスチャーをした手を取得する
PXCMHandData.IHand hand;
sts = handData.QueryHandDataById(gesture.handId, out hand);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// どちらの手でジェスチャーしたのか
var side = hand.QueryBodySide();
if (side == PXCMHandData.BodySideType.BODY_SIDE_LEFT)
{
++leftGestureCount;
}
else
{
++rightGestureCount;
}
}
TextLeftGesture.Text = string.Format("Left gesture : {0}", leftGestureCount);
TextRightGesture.Text = string.Format("Right gesture : {0}", rightGestureCount);
}
