int GenerateMinecraftCharacter(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, const XnRGB24Pixel* image)
{
int ret = 0;
int xRes = dmd.XRes();
int yRes = dmd.YRes();
cv::Mat inputImage = cv::Mat(yRes, xRes, CV_8UC3);
cv::Mat skin = cv::Mat::zeros(cv::Size(64,32), CV_8UC3);
XnToCV(image,&inputImage);
cv::cvtColor(inputImage,inputImage,CV_RGB2BGR);
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
g_UserGenerator.GetUsers(aUsers, nUsers);
int i = 0;
for (i = 0; i < nUsers; ++i) {
if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) break;
}
// No users being tracked
if (i == nUsers) return -1;
ret = GenerateSkin(aUsers[i], &inputImage, &skin);
printf("GenerateSkin returned %d on user %d\n",ret,(int)aUsers[i]);
cv::imwrite("skin.png",skin);
SegmentUser(aUsers[i], &inputImage, smd);
DrawDebugPoints(aUsers[i], &inputImage);
cv::imwrite("blah.png",inputImage);
sync();
system("convert skin.png -transparent black skin.png && composite -geometry +32+0 hardhat.png skin.png skin.png");
sync();
return ret;
}
void send(const xn::DepthMetaData& dmd)
{
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
userGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
memset(&SendBuf, '\0', BufLen); // this may be expensive
if (userGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
strcpy(&SendBuf[0], "sensor-update");
for(int j = 0; j < 24 ; j++)
{
if(jointPos(aUsers[i], jointList[j]) == 0)
{
sprintf(&SendBuf[strlen(SendBuf)], " \"%s_x\" %f", jointNames[j], jointCoords[1]); // I know, I know :(
sprintf(&SendBuf[strlen(SendBuf)], " \"%s_y\" %f", jointNames[j], jointCoords[2]);
}
}
// Before we send each bundle, we need to send some size code
unsigned char sizeBytes[4];
//strcpy(SendBuf, P.c_str());
int len = strlen(SendBuf);
sizeBytes[0] =(unsigned char)( len >> 24 );
sizeBytes[1] =(unsigned char)( (len << 8) >> 24 );
sizeBytes[2] =(unsigned char)( (len << 16) >> 24 );
sizeBytes[3] =(unsigned char)( (len << 24) >> 24 );
if (debug)
printf("%s\n\n", SendBuf);
sendto(SendSocket, (char*)sizeBytes, 4, 0, (struct sockaddr *) &RecvAddr, sizeof(RecvAddr));
sendto(SendSocket, SendBuf, strlen(SendBuf), 0, (struct sockaddr *) &RecvAddr, sizeof(RecvAddr));
}
}
void XN_CALLBACK_TYPE UserCalibration_CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie)
{
if (bSuccess)
{
ROS_INFO("Calibration complete, start tracking user %d", nId);
g_UserGenerator.GetSkeletonCap().StartTracking(nId);
if (default_user == 0) // set default user if still unset
{
default_user = nId;
ROS_INFO_STREAM("OpenNI tracker: Default user has been initialised with user " << default_user << ".");
}
std_msgs::UInt16MultiArray tracked_users;
XnUserID users[15];
XnUInt16 users_count = 15;
g_UserGenerator.GetUsers(users, users_count);
for (unsigned int user = 0; user < users_count; ++user)
{
tracked_users.data.push_back(users[user]);
}
available_tracked_users_pub.publish(tracked_users);
}
else
{
ROS_INFO("Calibration failed for user %d", nId);
if (g_bNeedPose)
{
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
}
else
{
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
}
}
void publishTransforms(const std::string& frame_id) {
XnUserID users[15];
XnUInt16 users_count = 15;
g_UserGenerator.GetUsers(users, users_count);
for (int i = 0; i < users_count; ++i) {
XnUserID user = users[i];
if (!g_UserGenerator.GetSkeletonCap().IsTracking(user))
continue;
publishTransform(user, XN_SKEL_HEAD, frame_id, "head2");
publishTransform(user, XN_SKEL_NECK, frame_id, "neck2");
publishTransform(user, XN_SKEL_TORSO, frame_id, "torso2");
publishTransform(user, XN_SKEL_LEFT_SHOULDER, frame_id, "left_shoulder2");
publishTransform(user, XN_SKEL_LEFT_ELBOW, frame_id, "left_elbow2");
publishTransform(user, XN_SKEL_LEFT_HAND, frame_id, "left_hand2");
publishTransform(user, XN_SKEL_RIGHT_SHOULDER, frame_id, "right_shoulder2");
publishTransform(user, XN_SKEL_RIGHT_ELBOW, frame_id, "right_elbow2");
publishTransform(user, XN_SKEL_RIGHT_HAND, frame_id, "right_hand2");
publishTransform(user, XN_SKEL_LEFT_HIP, frame_id, "left_hip2");
publishTransform(user, XN_SKEL_LEFT_KNEE, frame_id, "left_knee2");
publishTransform(user, XN_SKEL_LEFT_FOOT, frame_id, "left_foot2");
publishTransform(user, XN_SKEL_RIGHT_HIP, frame_id, "right_hip2");
publishTransform(user, XN_SKEL_RIGHT_KNEE, frame_id, "right_knee2");
publishTransform(user, XN_SKEL_RIGHT_FOOT, frame_id, "right_foot2");
}
}
// Callback: New user was detected
void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie)
{
XnUInt32 epochTime = 0;
xnOSGetEpochTime(&epochTime);
XnUserID aUsers[MAX_NUM_USERS];
XnUInt16 nUsers;
g_UserGenerator.GetUsers(aUsers, nUsers);
printf("%d New User %d\n", epochTime, nId);
// New user found
//validate the number of users
XnUInt16 usersTracked=0;
for(XnUInt16 i=0; i<nUsers; i++)
{
if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])==TRUE)
usersTracked++;
}
if(usersTracked >= numOfUser)
{
printf("%d users are currently being tracked, no more users allowed\n",numOfUser);
return;
}
if (g_bNeedPose)
{
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
}
else
{
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
}
void gl_onKeyboard(unsigned char key, int x, int y)
{
XnUInt16 nUser = 16;
XnUserID users[16];
switch(key){
case 27:
gContext.Shutdown();
exit(1);
break;
case 'c':
gUserGenerator.GetUsers(users, nUser);
if(nUser == 0){
fprintf(stderr, "cannot find user\n");
}else if(capture->start(*std::min_element(users, &users[nUser]))){
fprintf(stderr, "start recording\n");
}
break;
case 's':
if(capture->stop()){
fprintf(stderr, "stop recording\n");
}
break;
}
}
// Load calibration from file
bool LoadCalibration( xn::UserGenerator& userGenerator )
{
printf("LoadCalibration\n");
XnUserID aUserIDs[20] = {0};
XnUInt16 nUsers = 20;
userGenerator.GetUsers(aUserIDs, nUsers);
for (int i = 0; i < nUsers; ++i) {
// ユーザーがキャリブレーションされているときは何もしない
if (userGenerator.GetSkeletonCap().IsCalibrated(aUserIDs[i]))
continue;
if (userGenerator.GetSkeletonCap().IsCalibrating(aUserIDs[i]))
continue;
// ファイルから、キャリブレーション情報を取得し、骨格の追跡を開始する
printf("Load user's calibration from file: user %d\n", aUserIDs[i]);
XnStatus rc = userGenerator.GetSkeletonCap().LoadCalibrationDataFromFile(aUserIDs[i], XN_CALIBRATION_FILE_NAME);
if (rc == XN_STATUS_OK) {
printf("Make sure state is coherent: user %d\n", aUserIDs[i]);
userGenerator.GetPoseDetectionCap().StopPoseDetection(aUserIDs[i]);
userGenerator.GetSkeletonCap().StartTracking(aUserIDs[i]);
return true;
}
break;
}
return false;
}
void publishTransforms(const std::string& frame_id, ros::Publisher& publisher)
{
XnUserID users[NUSERS];
XnUInt16 users_count = NUSERS;
g_UserGenerator.GetUsers(users, users_count);
for (int i = 0; i < users_count; ++i)
{
XnUserID user = users[i];
XnPoint3D point;
g_UserGenerator.GetCoM(users[i], point);
int idx = user - 1;
if (isnan(point.X)==1||isnan(point.Y)==1||isnan(point.Z)==1||point.Z==0) // TODO
kinectUsers[idx].active == FALSE;
if (kinectUsers[idx].active == TRUE)
estimateVelocity(point, kinectUsers[idx]);
//if (point.X-oldX>=20)
//publisher.publish(kinectUsers[idx]);
//oldX=point.X;
//}
//g_DepthGenerator.ConvertRealWorldToProjective(1, &point, &point);
// original sceleton transform publishing.
/*if (g_UserGenerator.GetSkeletonCap().IsTracking(user))
{
publishTransform(user, XN_SKEL_HEAD, frame_id, "head");
publishTransform(user, XN_SKEL_NECK, frame_id, "neck");
publishTransform(user, XN_SKEL_TORSO, frame_id, "torso");
publishTransform(user, XN_SKEL_LEFT_SHOULDER, frame_id, "left_shoulder");
publishTransform(user, XN_SKEL_LEFT_ELBOW, frame_id, "left_elbow");
publishTransform(user, XN_SKEL_LEFT_HAND, frame_id, "left_hand");
publishTransform(user, XN_SKEL_RIGHT_SHOULDER, frame_id, "right_shoulder");
publishTransform(user, XN_SKEL_RIGHT_ELBOW, frame_id, "right_elbow");
publishTransform(user, XN_SKEL_RIGHT_HAND, frame_id, "right_hand");
publishTransform(user, XN_SKEL_LEFT_HIP, frame_id, "left_hip");
publishTransform(user, XN_SKEL_LEFT_KNEE, frame_id, "left_knee");
publishTransform(user, XN_SKEL_LEFT_FOOT, frame_id, "left_foot");
publishTransform(user, XN_SKEL_RIGHT_HIP, frame_id, "right_hip");
publishTransform(user, XN_SKEL_RIGHT_KNEE, frame_id, "right_knee");
publishTransform(user, XN_SKEL_RIGHT_FOOT, frame_id, "right_foot");
}*/
}
for (int i = 0; i < NUSERS; ++i)
{
if (kinectUsers[i].id != 0)
publisher.publish(kinectUsers[i]);
//if (kinectUsers[i].active == 0)
//kinectUsers[i].id = 0;
}
}
void publishTransforms(const std::string& frame_id) {
XnUserID users[15];
XnUInt16 users_count = 15;
g_UserGenerator.GetUsers(users, users_count);
for (int i = 0; i < users_count; ++i) {
XnUserID user = users[i];
publishTransform(user, frame_id, "user");
}
}
// this function will print the confidence values of all the joints
void jointConf()
{
XnUserID nId[15];
XnUInt16 oId = 15;
g_UserGenerator.GetUsers(nId, oId);
int userSum = 0;
float userConfidence = 0;
// ITERATE THROUGH ALL TRACKED USERS AND OUTPUT INDIVIDUAL JOINT CONFIDENCE VALUES
for (int i = 0; i < oId; ++i){
if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(nId[i])){
XnSkeletonJointPosition jointData;
printf("\n");
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_HEAD, jointData);
std::cout << "User " << nId[i] << " - Head " << jointData.fConfidence << std::endl;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_NECK, jointData);
std::cout << "User " << nId[i] << " - Neck " << jointData.fConfidence << std::endl;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_TORSO, jointData);
std::cout << "User " << nId[i] << " - Torso " << jointData.fConfidence << std::endl;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_SHOULDER, jointData);
std::cout << "User " << nId[i] << " - L Shoulder " << jointData.fConfidence << std::endl;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_ELBOW, jointData);
std::cout << "User " << nId[i] << " - L Elbow " << jointData.fConfidence << std::endl;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_HAND, jointData);
std::cout << "User " << nId[i] << " - L Hand " << jointData.fConfidence << std::endl;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_SHOULDER, jointData);
std::cout << "User " << nId[i] << " - R Shoulder " << jointData.fConfidence << std::endl;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_ELBOW, jointData);
std::cout << "User " << nId[i] << " - R Elbow " << jointData.fConfidence << std::endl;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_HAND, jointData);
std::cout << "User " << nId[i] << " - R Hand " << jointData.fConfidence << std::endl;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_HIP, jointData);
std::cout << "User " << nId[i] << " - L Hip " << jointData.fConfidence << std::endl;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_KNEE, jointData);
std::cout << "User " << nId[i] << " - L Knee " << jointData.fConfidence << std::endl;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_FOOT, jointData);
std::cout << "User " << nId[i] << " - L Foot " << jointData.fConfidence << std::endl;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_HIP, jointData);
std::cout << "User " << nId[i] << " - R Hip " << jointData.fConfidence << std::endl;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_KNEE, jointData);
std::cout << "User " << nId[i] << " - R Knee " << jointData.fConfidence << std::endl;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_FOOT, jointData);
std::cout << "User " << nId[i] << " - R Foot " << jointData.fConfidence << std::endl;
}
}
}
XnUserID FirstTrackingUser(xn::UserGenerator userGenerator) {
XnUserID aUsers[10];
XnUInt16 nUsers = 10;
userGenerator.GetUsers(aUsers, nUsers);
// The first user that's tracking will be in control
for (int i = 0; i < nUsers; ++i) {
if(userGenerator.GetSkeletonCap().IsTracking(aUsers[i])){
return aUsers[i];
}
}
return NULL;
}
XnUserID hiKinect::getTrackedUserID(){
XnUInt16 nUsers = g_UserGenerator.GetNumberOfUsers();
XnUserID aUsers[nUsers];
g_UserGenerator.GetUsers(aUsers, nUsers);
xn::SkeletonCapability cap = g_UserGenerator.GetSkeletonCap();
for (int i = 0; i < nUsers; ++i) {
if (cap.IsTracking(aUsers[i])) {
return aUsers[i];
}
}
return 0;
}
// this function will calculate the average joint confidence value for every user
void jointConfidence()
{
XnUserID nId[15];
XnUInt16 oId = 15;
g_UserGenerator.GetUsers(nId, oId);
int userSum = 0;
float userConfidence = 0;
for (int i = 0; i < oId; ++i){
if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(nId[i])){
XnSkeletonJointPosition jointData;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_HEAD, jointData);
userSum += jointData.fConfidence;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_NECK, jointData);
userSum += jointData.fConfidence;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_TORSO, jointData);
userSum += jointData.fConfidence;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_SHOULDER, jointData);
userSum += jointData.fConfidence;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_ELBOW, jointData);
userSum += jointData.fConfidence;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_HAND, jointData);
userSum += jointData.fConfidence;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_SHOULDER, jointData);
userSum += jointData.fConfidence;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_ELBOW, jointData);
userSum += jointData.fConfidence;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_HAND, jointData);
userSum += jointData.fConfidence;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_HIP, jointData);
userSum += jointData.fConfidence;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_KNEE, jointData);
userSum += jointData.fConfidence;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_LEFT_FOOT, jointData);
userSum += jointData.fConfidence;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_HIP, jointData);
userSum += jointData.fConfidence;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_KNEE, jointData);
userSum += jointData.fConfidence;
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(nId[i], XN_SKEL_RIGHT_FOOT, jointData);
userSum += jointData.fConfidence;
userConfidence = userSum / 15.0f;
csvConfidence(nId[i], userConfidence);
}
}
}
// Save calibration to file
void SaveCalibration(){
XnUserID aUserIDs[20] = {0};
XnUInt16 nUsers = 20;
g_UserGenerator.GetUsers(aUserIDs, nUsers);
for (int i = 0; i < nUsers; ++i){
// Find a user who is already calibrated
if (g_UserGenerator.GetSkeletonCap().IsCalibrated(aUserIDs[i])){
// Save user's calibration to file
g_UserGenerator.GetSkeletonCap().SaveCalibrationDataToFile(aUserIDs[i], XN_CALIBRATION_FILE_NAME);
break;
}
}
}
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd)
{
char strLabel[50] = "";
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
glBegin(GL_POINT);
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])){
DrawJoint(aUsers[i], XN_SKEL_HEAD);
DrawJoint(aUsers[i], XN_SKEL_NECK);
DrawJoint(aUsers[i], XN_SKEL_TORSO);
DrawJoint(aUsers[i], XN_SKEL_WAIST);
DrawJoint(aUsers[i], XN_SKEL_LEFT_COLLAR);
DrawJoint(aUsers[i], XN_SKEL_LEFT_SHOULDER);
DrawJoint(aUsers[i], XN_SKEL_LEFT_ELBOW);
DrawJoint(aUsers[i], XN_SKEL_LEFT_WRIST);
DrawJoint(aUsers[i], XN_SKEL_LEFT_HAND);
DrawJoint(aUsers[i], XN_SKEL_LEFT_FINGERTIP);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_COLLAR);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_SHOULDER);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_ELBOW);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_WRIST);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_HAND);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_FINGERTIP);
DrawJoint(aUsers[i], XN_SKEL_LEFT_HIP);
DrawJoint(aUsers[i], XN_SKEL_LEFT_KNEE);
DrawJoint(aUsers[i], XN_SKEL_LEFT_ANKLE);
DrawJoint(aUsers[i], XN_SKEL_LEFT_FOOT);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_HIP);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_KNEE);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_ANKLE);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_FOOT);
}
}
glEnd();
}
void XN_CALLBACK_TYPE User_LostUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie)
{
ROS_INFO_STREAM("Lost user " << nId << ".");
std_msgs::UInt16MultiArray tracked_users;
XnUserID users[15];
XnUInt16 users_count = 15;
g_UserGenerator.GetUsers(users, users_count);
for (unsigned int user = 0; user < users_count; ++user)
{
tracked_users.data.push_back(users[user]);
}
available_tracked_users_pub.publish(tracked_users);
}
void XN_CALLBACK_TYPE
User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) {
printf("New User: %d\n", nId);
FBLOG_INFO("User_NewUser", nId);
XnUInt16 nUsers = g_UserGenerator.GetNumberOfUsers();
XnUserID aUsers[nUsers];
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i) {
if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) {
return;
}
}
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(POSE_TO_USE, nId);
(*((hiKinect *)pCookie)->callbackPtr)->InvokeAsync("", FB::variant_list_of("NEW_USER"));
}
void processUsers(uint32_t frameId)
{
// Process users
XnUserID aUsers[20];
XnUInt16 nUsers = 20;
g_UserGenerator.GetUsers(aUsers, nUsers);
std::vector<XnUserID> users;
for (int i = 0; i < nUsers; ++i)
{
if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
users.push_back(aUsers[i]);
}
}
processAndPublish(users, frameId);
}
// Save calibration to file
void SaveCalibration( xn::UserGenerator& userGenerator )
{
printf("SaveCalibration\n");
XnUserID aUserIDs[20] = {0};
XnUInt16 nUsers = 20;
userGenerator.GetUsers( aUserIDs, nUsers );
for ( int i = 0; i < nUsers; ++i ) {
// ユーザーがキャリブレーションされていたら、その情報を保存する
if ( userGenerator.GetSkeletonCap().IsCalibrated(aUserIDs[i]) ) {
printf("Save user's calibration to file: user %d\n", aUserIDs[i]);
userGenerator.GetSkeletonCap().SaveCalibrationDataToFile(aUserIDs[i], XN_CALIBRATION_FILE_NAME);
break;
}
}
}
void FindPlayer()
{
if (g_nPlayer != 0)
{
return;
}
XnUserID aUsers[20];
XnUInt16 nUsers = 20;
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
if (AssignPlayer(aUsers[i]))
return;
}
}
// track two users
// take down users' joint positions
// calculate joint angles and etc.
void CheckPosture(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, FILE *fp)
{
XnUserID aUsers[2];
XnUInt16 nUsers = 2;
g_UserGenerator.GetUsers(aUsers, nUsers);
for(int i = 0; i < nUsers; ++i)
{
// if the user is being tracked
// take down his/her joint positions
if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
// check user posture
RecordAngle(aUsers[i], fp);
}
}
}
// Load calibration from file
void LoadCalibration(){
XnUserID aUserIDs[20] = {0};
XnUInt16 nUsers = 20;
g_UserGenerator.GetUsers(aUserIDs, nUsers);
for (int i = 0; i < nUsers; ++i){
// Find a user who isn't calibrated or currently in pose
if (g_UserGenerator.GetSkeletonCap().IsCalibrated(aUserIDs[i])) continue;
if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUserIDs[i])) continue;
// Load user's calibration from file
XnStatus rc = g_UserGenerator.GetSkeletonCap().LoadCalibrationDataFromFile(aUserIDs[i], XN_CALIBRATION_FILE_NAME);
if (rc == XN_STATUS_OK){
// Make sure state is coherent
g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(aUserIDs[i]);
g_UserGenerator.GetSkeletonCap().StartTracking(aUserIDs[i]);
}
break;
}
}
void publishTransforms(const std::string& frame_id)
{
ros::Time now = ros::Time::now();
XnUInt16 users_count = MAX_USERS;
XnUserID users[MAX_USERS];
g_UserGenerator.GetUsers(users, users_count);
//per ogni utente pubblica la trasformazione
for(int i = 0; i < users_count; ++i)
{
XnUserID user = users[i];
if(checkCenterOfMass(user))
{
publishUserTransforms(user, frame_id);
}
}
}
void XN_CALLBACK_TYPE CalibrationEnded(xn::SkeletonCapability& skeleton, XnUserID user, XnBool bSuccess, void* cxt)
{
printf("Calibration done [%d] %ssuccessfully\n", user, bSuccess?"":"un");
if (bSuccess)
{
if (!g_bCalibrated)
{
g_UserGenerator.GetSkeletonCap().SaveCalibrationData(user, 0);
g_nPlayer = user;
g_UserGenerator.GetSkeletonCap().StartTracking(user);
g_bCalibrated = TRUE;
}
XnUserID aUsers[10];
XnUInt16 nUsers = 10;
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(aUsers[i]);
}
}
void publishTransforms(const std::string& frame_id, image_transport::Publisher& pub)
{
XnUserID users[15];
XnUInt16 users_count = 15;
g_UserGenerator.GetUsers(users, users_count);
for (int i = 0; i < users_count; ++i)
{
XnUserID user = users[i];
if (!g_UserGenerator.GetSkeletonCap().IsTracking(user))
continue;
publishTransform(user, XN_SKEL_HEAD, frame_id, "head");
publishTransform(user, XN_SKEL_NECK, frame_id, "neck");
publishTransform(user, XN_SKEL_TORSO, frame_id, "torso");
publishTransform(user, XN_SKEL_LEFT_SHOULDER, frame_id, "left_shoulder");
publishTransform(user, XN_SKEL_LEFT_ELBOW, frame_id, "left_elbow");
publishTransform(user, XN_SKEL_LEFT_HAND, frame_id, "left_hand");
publishTransform(user, XN_SKEL_RIGHT_SHOULDER, frame_id, "right_shoulder");
publishTransform(user, XN_SKEL_RIGHT_ELBOW, frame_id, "right_elbow");
publishTransform(user, XN_SKEL_RIGHT_HAND, frame_id, "right_hand");
publishTransform(user, XN_SKEL_LEFT_HIP, frame_id, "left_hip");
publishTransform(user, XN_SKEL_LEFT_KNEE, frame_id, "left_knee");
publishTransform(user, XN_SKEL_LEFT_FOOT, frame_id, "left_foot");
publishTransform(user, XN_SKEL_RIGHT_HIP, frame_id, "right_hip");
publishTransform(user, XN_SKEL_RIGHT_KNEE, frame_id, "right_knee");
publishTransform(user, XN_SKEL_RIGHT_FOOT, frame_id, "right_foot");
}
// publish segmented image
xn::SceneMetaData sceneMD;
xn::DepthMetaData depthMD;
g_DepthGenerator.GetMetaData(depthMD);
g_UserGenerator.GetUserPixels(0, sceneMD);
PublishPeopleImage(depthMD, sceneMD, pub);
}
reader_result kinect_reader2::get_next() {
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
XnSkeletonJointPosition joint;
while(true) {
// Read next available data
nRetVal = g_Context.WaitOneUpdateAll(g_DepthGenerator);
if(nRetVal != XN_STATUS_OK) {
printf("Failed updating data: %s\n", xnGetStatusString(nRetVal));
continue;
}
if(g_UserGenerator.GetNumberOfUsers() > 0) {
g_UserGenerator.GetUsers(aUsers, nUsers);
for(int i=0; i< nUsers; i++)
{
if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
reader_result new_tuple;
// player
new_tuple.push_back(boost::any((int)aUsers[i]));
// 1 = XN_SKEL_HEAD, ..., 24 = XN_SKEL_RIGHT_FOOD
for(short j = 1; j <= 24; j++) {
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], (XnSkeletonJoint)j, joint);
new_tuple.push_back(boost::any(boost::lexical_cast<std::string>((float)joint.position.X)));
new_tuple.push_back(boost::any(boost::lexical_cast<std::string>((float)joint.position.Y)));
new_tuple.push_back(boost::any(boost::lexical_cast<std::string>((float)joint.position.Z)));
new_tuple.push_back(boost::any(boost::lexical_cast<std::string>((float)joint.fConfidence)));
}
return new_tuple;
}
}
}
}
}
void getSkels(std::vector<mapping_msgs::PolygonalMap> &pmaps, body_msgs::Skeletons &skels){
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
body_msgs::Skeleton skel;
getSkeleton(aUsers[i],skel);
skels.skeletons.push_back(skel);
mapping_msgs::PolygonalMap pmap;
getPolygon(aUsers[i], XN_SKEL_HEAD, XN_SKEL_NECK, pmap);
// printPt(aUsers[i], XN_SKEL_RIGHT_HAND);
getPolygon(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER, pmap);
getPolygon(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW, pmap);
getPolygon(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_RIGHT_SHOULDER, pmap);
// ptdist(pmap.polygons.back());
getPolygon(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND, pmap);
getPolygon(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER, pmap);
getPolygon(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW, pmap);
getPolygon(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND, pmap);
getPolygon(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO, pmap);
getPolygon(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO, pmap);
getPolygon(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP, pmap);
getPolygon(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE, pmap);
getPolygon(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT, pmap);
getPolygon(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP, pmap);
getPolygon(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE, pmap);
getPolygon(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT, pmap);
getPolygon(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP, pmap);
// getSkel(aUsers[i],pmap);
pmaps.push_back(pmap);
}
}
}
int main(int argc, char **argv)
{
XnStatus nRetVal = XN_STATUS_OK;
xn::EnumerationErrors errors;
TotalFrames = 0;
static struct rusage ru;
long double t;
double w;
struct timeval timStart;
struct timeval timEnd;
struct timeval Wall;
bool Sample = true;
XnFPSData xnFPS;
XnUserID aUsers[MAX_NUM_USERS];
XnUInt16 nUsers;
XnSkeletonJointTransformation torsoJoint;
if (argc > 1)
{
//parse the cmd line
if (strcasecmp(argv[1],"--help") == 0)
{
PrintHelpHeader(argv[0]);
return 1;
}
numOfUser = atoi(argv[1]);
if(numOfUser == 0)
{
PrintHelpHeader(argv[0]);
return 1;
}
else if(numOfUser > 2)
{
printf("Maximal Users allowed is 2\n");
return 1;
}
}
else
{
numOfUser = 4;
}
const char *fn = NULL;
if (fileExists(SAMPLE_XML_PATH)) fn = SAMPLE_XML_PATH;
else if (fileExists(SAMPLE_XML_PATH_LOCAL)) fn = SAMPLE_XML_PATH_LOCAL;
else {
printf("Could not find '%s' nor '%s'. Aborting.\n" , SAMPLE_XML_PATH, SAMPLE_XML_PATH_LOCAL);
return XN_STATUS_ERROR;
}
printf("Reading config from: '%s'\n", fn);
nRetVal = g_Context.InitFromXmlFile(fn, g_scriptNode, &errors);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
return (nRetVal);
}
else if (nRetVal != XN_STATUS_OK)
{
printf("Open failed: %s\n", xnGetStatusString(nRetVal));
return (nRetVal);
}
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
CHECK_RC(nRetVal,"No depth");
#if (XN_PLATFORM != XN_PLATFORM_MACOSX)
//we want out benchmark application will be running only on one CPU core
cpu_set_t mask;
CPU_ZERO(&mask);
CPU_SET(1,&mask);
sched_setaffinity(0,sizeof(mask),&mask);
#endif
//initialize the FPS calculator
nRetVal = xnFPSInit(&xnFPS, 90);
CHECK_RC(nRetVal, "FPS Init");
//ensure the User generator exists
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
if (nRetVal != XN_STATUS_OK)
{
nRetVal = g_UserGenerator.Create(g_Context);
CHECK_RC(nRetVal, "Find user generator");
}
//register to generators callbacks
XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete, hPoseDetected;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
{
printf("Supplied user generator doesn't support skeleton\n");
return 1;
}
nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
CHECK_RC(nRetVal, "Register to user callbacks");
nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationStart(UserCalibration_CalibrationStart, NULL, hCalibrationStart);
CHECK_RC(nRetVal, "Register to calibration start");
nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, NULL, hCalibrationComplete);
CHECK_RC(nRetVal, "Register to calibration complete");
//ensure calibration
if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration())
{
g_bNeedPose = TRUE;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
{
printf("Pose required, but not supported\n");
return 1;
}
nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, NULL, hPoseDetected);
CHECK_RC(nRetVal, "Register to Pose Detected");
g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
}
//set skeleton profile (all joints)
g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
//start to generate
nRetVal = g_Context.StartGeneratingAll();
CHECK_RC(nRetVal, "StartGenerating");
printf("%c[%d;%d;%dmPrimeSense Skeleton Benchmark Application\n ", 0x1B, BRIGHT,BLUE,BG_BLACK);
printf("%c[%d;%d;%dmSet Maximal users to %d ", 0x1B, BRIGHT,BLUE,BG_BLACK,numOfUser);
printf("%c[%dm\n", 0x1B, 0);
printf("Starting to run\n");
if(g_bNeedPose)
{
printf("Assume calibration pose\n");
}
XnUInt32 epochTime = 0;
//each 30 frames (1 second) we start the CPU resources usages
while (!xnOSWasKeyboardHit())
{
if (Sample)
{ //get the beginning sample of CPU resources
getrusage(RUSAGE_SELF, &ru);
timStart=ru.ru_utime;
t=(double)timStart.tv_sec * 1000000.0 + (double)timStart.tv_usec \
+ (double)ru.ru_stime.tv_sec*1000000.0+(double)ru.ru_stime.tv_usec;
//get the wall clock time
gettimeofday(&Wall,NULL);
w=(double)Wall.tv_sec * 1000000.0 + (double)Wall.tv_usec;
Sample = false;
}
g_Context.WaitOneUpdateAll(g_UserGenerator);
xnFPSMarkFrame(&xnFPS);
// print the torso information for the first user already tracking every 1 second to prevent CPU of printf
if(TotalFrames % 30 == 0)
{
nUsers=MAX_NUM_USERS;
g_UserGenerator.GetUsers(aUsers, nUsers);
int numTracked=0;
int userToPrint=-1;
for(XnUInt16 i=0; i<nUsers; i++)
{
if(g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])==FALSE)
continue;
g_UserGenerator.GetSkeletonCap().GetSkeletonJoint(aUsers[i],XN_SKEL_TORSO,torsoJoint);
printf("User %d Located At distance of %6.2f mm from the sensor\n",aUsers[i],torsoJoint.position.position.Z);
}
//get the finish sample of the CPU resources
getrusage(RUSAGE_SELF, &ru);
timEnd=ru.ru_utime;
t = (double)timEnd.tv_sec * 1000000.0 + (double)timEnd.tv_usec \
+ (double)ru.ru_stime.tv_sec*1000000.0+(double)ru.ru_stime.tv_usec - t;
//get the wall clock
gettimeofday(&Wall,NULL);
w = (double)Wall.tv_sec * 1000000.0 + (double)Wall.tv_usec - w;
XnDouble fps=xnFPSCalc(&xnFPS);
//print stuff.
printf("%c[%d;%d;%dmCPU Utilization=%3.2f%%\t", 0x1B, BRIGHT,RED,BG_BLACK,(double)100*t/w);
printf("%c[%d;%d;%dmFPS=%3.2f ", 0x1B, BRIGHT,RED,BG_BLACK,(double)fps);
printf("%c[%dm\n", 0x1B, 0);
Sample= true;
}
TotalFrames++;
}
g_scriptNode.Release();
g_DepthGenerator.Release();
g_UserGenerator.Release();
g_Context.Release();
}
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd)
{
static bool bInitialized = false;
static GLuint depthTexID;
static unsigned char* pDepthTexBuf;
static int texWidth, texHeight;
float topLeftX;
float topLeftY;
float bottomRightY;
float bottomRightX;
float texXpos;
float texYpos;
if(!bInitialized)
{
texWidth = getClosestPowerOfTwo(dmd.XRes());
texHeight = getClosestPowerOfTwo(dmd.YRes());
// printf("Initializing depth texture: width = %d, height = %d\n", texWidth, texHeight);
depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ;
// printf("Initialized depth texture: width = %d, height = %d\n", texWidth, texHeight);
bInitialized = true;
topLeftX = dmd.XRes();
topLeftY = 0;
bottomRightY = dmd.YRes();
bottomRightX = 0;
texXpos =(float)dmd.XRes()/texWidth;
texYpos =(float)dmd.YRes()/texHeight;
memset(texcoords, 0, 8*sizeof(float));
texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos;
}
unsigned int nValue = 0;
unsigned int nHistValue = 0;
unsigned int nIndex = 0;
unsigned int nX = 0;
unsigned int nY = 0;
unsigned int nNumberOfPoints = 0;
XnUInt16 g_nXRes = dmd.XRes();
XnUInt16 g_nYRes = dmd.YRes();
unsigned char* pDestImage = pDepthTexBuf;
const XnDepthPixel* pDepth = dmd.Data();
const XnLabel* pLabels = smd.Data();
// Calculate the accumulative histogram
memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX<g_nXRes; nX++)
{
nValue = *pDepth;
if (nValue != 0)
{
g_pDepthHist[nValue]++;
nNumberOfPoints++;
}
pDepth++;
}
}
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
}
if (nNumberOfPoints)
{
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints)));
}
}
pDepth = dmd.Data();
if (g_bDrawPixels)
{
XnUInt32 nIndex = 0;
// Prepare the texture map
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX < g_nXRes; nX++, nIndex++)
{
pDestImage[0] = 0;
pDestImage[1] = 0;
pDestImage[2] = 0;
if (g_bDrawBackground || *pLabels != 0)
{
nValue = *pDepth;
XnLabel label = *pLabels;
XnUInt32 nColorID = label % nColors;
if (label == 0)
{
nColorID = nColors;
}
if (nValue != 0)
{
nHistValue = g_pDepthHist[nValue];
pDestImage[0] = nHistValue * Colors[nColorID][0];
pDestImage[1] = nHistValue * Colors[nColorID][1];
pDestImage[2] = nHistValue * Colors[nColorID][2];
}
}
pDepth++;
pLabels++;
pDestImage+=3;
}
pDestImage += (texWidth - g_nXRes) *3;
}
}
else
{
xnOSMemSet(pDepthTexBuf, 0, 3*2*g_nXRes*g_nYRes);
}
glBindTexture(GL_TEXTURE_2D, depthTexID);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf);
// Display the OpenGL texture map
glColor4f(0.75,0.75,0.75,1);
glEnable(GL_TEXTURE_2D);
DrawTexture(dmd.XRes(),dmd.YRes(),0,0);
glDisable(GL_TEXTURE_2D);
char strLabel[50] = "";
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
if (g_bPrintID)
{
XnPoint3D com;
g_UserGenerator.GetCoM(aUsers[i], com);
g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);
xnOSMemSet(strLabel, 0, sizeof(strLabel));
if (!g_bPrintState)
{
// Tracking
sprintf(strLabel, "%d", aUsers[i]);
}
else if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
// Tracking
sprintf(strLabel, "%d - Tracking", aUsers[i]);
}
else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
{
// Calibrating
sprintf(strLabel, "%d - Calibrating [%s]", aUsers[i], GetCalibrationErrorString(m_Errors[aUsers[i]].first));
}
else
{
// Nothing
sprintf(strLabel, "%d - Looking for pose [%s]", aUsers[i], GetPoseErrorString(m_Errors[aUsers[i]].second));
}
glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1);
glRasterPos2i(com.X, com.Y);
glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
}
//Draw skeleton
if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
glBegin(GL_LINES);
glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1);
DrawLimb(aUsers[i], XN_SKEL_HEAD, XN_SKEL_NECK);
DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);
DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);
DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
DrawLimb(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);
DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT);
DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP);
glEnd();
}
}
}
int main(int argc, char **argv)
{
XnStatus nRetVal = XN_STATUS_OK;
xn::EnumerationErrors errors;
if( USE_RECORED_DATA ){
g_Context.Init();
g_Context.OpenFileRecording(RECORD_FILE_PATH);
xn::Player player;
// Player nodeの取得
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_PLAYER, player);
CHECK_RC(nRetVal, "Find player");
LOG_D("PlaybackSpeed: %d", player.GetPlaybackSpeed());
xn:NodeInfoList nodeList;
player.EnumerateNodes(nodeList);
for( xn::NodeInfoList::Iterator it = nodeList.Begin();
it != nodeList.End(); ++it){
if( (*it).GetDescription().Type == XN_NODE_TYPE_IMAGE ){
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_ImageGenerator);
CHECK_RC(nRetVal, "Find image node");
LOG_D("%s", "ImageGenerator created.");
}
else if( (*it).GetDescription().Type == XN_NODE_TYPE_DEPTH ){
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
CHECK_RC(nRetVal, "Find depth node");
LOG_D("%s", "DepthGenerator created.");
}
else{
LOG_D("%s %s %s", ::xnProductionNodeTypeToString((*it).GetDescription().Type ),
(*it).GetInstanceName(),
(*it).GetDescription().strName);
}
}
}
else{
LOG_I("Reading config from: '%s'", CONFIG_XML_PATH);
nRetVal = g_Context.InitFromXmlFile(CONFIG_XML_PATH, g_scriptNode, &errors);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT){
XnChar strError[1024];
errors.ToString(strError, 1024);
LOG_E("%s\n", strError);
return (nRetVal);
}
else if (nRetVal != XN_STATUS_OK){
LOG_E("Open failed: %s", xnGetStatusString(nRetVal));
return (nRetVal);
}
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
CHECK_RC(nRetVal,"No depth");
// ImageGeneratorの作成
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_ImageGenerator);
CHECK_RC(nRetVal, "Find image generator");
}
// UserGeneratorの取得
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
if(nRetVal!=XN_STATUS_OK){
nRetVal = g_UserGenerator.Create(g_Context);
CHECK_RC(nRetVal, "Create user generator");
}
XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete, hPoseDetected;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)){
LOG_E("%s", "Supplied user generator doesn't support skeleton");
return 1;
}
nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
CHECK_RC(nRetVal, "Register to user callbacks");
g_SkeletonCap = g_UserGenerator.GetSkeletonCap();
nRetVal = g_SkeletonCap.RegisterToCalibrationStart(UserCalibration_CalibrationStart, NULL, hCalibrationStart);
CHECK_RC(nRetVal, "Register to calibration start");
nRetVal = g_SkeletonCap.RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, NULL, hCalibrationComplete);
CHECK_RC(nRetVal, "Register to calibration complete");
if (g_SkeletonCap.NeedPoseForCalibration()){
g_bNeedPose = TRUE;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)){
LOG_E("%s", "Pose required, but not supported");
return 1;
}
nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, NULL, hPoseDetected);
CHECK_RC(nRetVal, "Register to Pose Detected");
g_SkeletonCap.GetCalibrationPose(g_strPose);
}
g_SkeletonCap.SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
nRetVal = g_Context.StartGeneratingAll();
CHECK_RC(nRetVal, "StartGenerating");
// 表示用の画像データの作成
XnMapOutputMode mapMode;
g_ImageGenerator.GetMapOutputMode(mapMode);
g_rgbImage = cvCreateImage(cvSize(mapMode.nXRes, mapMode.nYRes), IPL_DEPTH_8U, 3);
LOG_I("%s", "Starting to run");
if(g_bNeedPose){
LOG_I("%s", "Assume calibration pose");
}
xn::Recorder recorder;
if( DO_RECORED && !USE_RECORED_DATA ){
// レコーダーの作成
LOG_I("%s", "Setup Recorder");
nRetVal = recorder.Create(g_Context);
CHECK_RC(nRetVal, "Create recorder");
// 保存設定
nRetVal = recorder.SetDestination(XN_RECORD_MEDIUM_FILE, RECORD_FILE_PATH);
CHECK_RC(nRetVal, "Set recorder destination file");
// 深度、ビデオカメラ入力を保存対象として記録開始
nRetVal = recorder.AddNodeToRecording(g_DepthGenerator, XN_CODEC_NULL);
CHECK_RC(nRetVal, "Add depth node to recording");
nRetVal = recorder.AddNodeToRecording(g_ImageGenerator, XN_CODEC_NULL);
CHECK_RC(nRetVal, "Add image node to recording");
LOG_I("%s", "Recorder setup done.");
}
while (!xnOSWasKeyboardHit())
{
g_Context.WaitOneUpdateAll(g_UserGenerator);
if( DO_RECORED && !USE_RECORED_DATA ){
nRetVal = recorder.Record();
CHECK_RC(nRetVal, "Record");
}
// ビデオカメラ画像の生データを取得
xn::ImageMetaData imageMetaData;
g_ImageGenerator.GetMetaData(imageMetaData);
// メモリコピー
xnOSMemCopy(g_rgbImage->imageData, imageMetaData.RGB24Data(), g_rgbImage->imageSize);
// BGRからRGBに変換して表示
cvCvtColor(g_rgbImage, g_rgbImage, CV_RGB2BGR);
// UserGeneratorからユーザー識別ピクセルを取得
xn::SceneMetaData sceneMetaData;
g_UserGenerator.GetUserPixels(0, sceneMetaData);
XnUserID allUsers[MAX_NUM_USERS];
XnUInt16 nUsers = MAX_NUM_USERS;
g_UserGenerator.GetUsers(allUsers, nUsers);
for (int i = 0; i < nUsers; i++) {
// キャリブレーションに成功しているかどうか
if (g_SkeletonCap.IsTracking(allUsers[i])) {
// スケルトンを描画
DrawSkelton(allUsers[i], i);
}
}
// 表示
cvShowImage("User View", g_rgbImage);
// ESCもしくはqが押されたら終了させる
if (cvWaitKey(10) == 27) {
break;
}
}
if( !USE_RECORED_DATA ){
g_scriptNode.Release();
}
g_DepthGenerator.Release();
g_UserGenerator.Release();
g_Context.Release();
if (g_rgbImage != NULL) {
cvReleaseImage(&g_rgbImage);
}
g_Context.Shutdown();
}
