Kinect体感机器人上位机源码

/**************************************************************************** * * * OpenNI 1.x Alpha * * Copyright (C) 2011 PrimeSense Ltd. * * * * This file is part of OpenNI. * * * * OpenNI is free software: you can redistribute it and/or modify * * it under the terms of the GNU Lesser General Public License as published * * by the Free Software Foundation, either version 3 of the License, or * * (at your option) any later version. * * * * OpenNI is distributed in the hope that it will be useful, * * but WITHOUT ANY WARRANTY; without even the implied warranty of * * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * * GNU Lesser General Public License for more details. * * * * You should have received a copy of the GNU Lesser General Public License * * along with OpenNI. If not, see <http://www.gnu.org/licenses/>. * * * ****************************************************************************/ /* * Copyright (C) 2012 NsLib * * This library is free software; you can redistribute it and/or * modify it under the terms of the GNU Library General Public * License as published by the Free Software Foundation; either * version 2 of the License, or (at your option) any later version. * * This library is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * Library General Public License for more details. * * You should have received a copy of the GNU Library General Public * License along with this library; if not, write to the Free * Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. */ /** * @file SceneDrawer.cpp * @author 马冬亮 * @brief 绘制骨骼图像, 由于效率因素, 大量函数内联实现. * * @details 本程序使用了OpenNI例程中显示骨骼的部分, 对OpenGL有部分修改. */ #include "../include/SceneDrawer.h" #include <iostream> #include <cmath> #include <limits> #include <map> #include <GL/glut.h> #include "../include/AppConfig.h" #include "../include/TrackerViewer.h" #include "../include/ControlRobot.h" #include "../include/MyLogger.h" /** * @brief 机器人控制. * @ingroup SceneDrawer */ extern NsLib::ControlRobot g_controlRobot; namespace NsLib { //////////////////////////////////////////////////////////////////////////////// // 追踪设置 //////////////////////////////////////////////////////////////////////////////// /** * @brief 追踪用户的数量, 由于我们的嵌入式开发板性能有限, 只能追踪一个用户. * @ingroup SceneDrawer */ #define MAX_TRACK_USER (1) /** * @brief Kinect捕获关节的确认度阀值. * @ingroup SceneDrawer */ #define JOINT_CONFIDENCE (0.35) //////////////////////////////////////////////////////////////////////////////// // 调试开关 //////////////////////////////////////////////////////////////////////////////// /** * @brief 调试信息总开关. * @ingroup SceneDrawer */ //#define DEBUG_MSG_ON #ifdef DEBUG_MSG_ON /** * @brief 用于分割调试信息. * @ingroup SceneDrawer */ #define DEBUG_MSG_SPLITER /** * @brief 输出左臂的调试信息. * @ingroup SceneDrawer */ #define DEBUG_MSG_LEFT_ARM /** * @brief 输出右臂的调试信息. * @ingroup SceneDrawer */ #define DEBUG_MSG_RIGHT_ARM /** * @brief 输出左腿的调试信息. * @ingroup SceneDrawer */ #define DEBUG_MSG_LEFT_LEG /** * @brief 输出右腿的调试信息. * @ingroup SceneDrawer */ #define DEBUG_MSG_RIGHT_LEG /** * @brief 输出机器人姿势. * @ingroup SceneDrawer */ #define DEBUG_MSG_POSE #endif // DEBUG_MSG_ON //////////////////////////////////////////////////////////////////////////////// // 姿势检测阀值 //////////////////////////////////////////////////////////////////////////////// /** * @brief 机器人下蹲的角度阀值. * @ingroup SceneDrawer */ #define ROBOT_POSE_SQUAT_THRESHOLD (75) /** * @brief 机器人向前走角度阀值. * @ingroup SceneDrawer */ #define ROBOT_POSE_WALK_FRONT_THRESHOLD (75) /** * @brief 机器人向后走角度阀值. * @ingroup SceneDrawer */ #define ROBOT_POSE_WALK_BACK_THRESHOLD (120) /** * @brief 机器人向后左角度阀值. * @ingroup SceneDrawer */ #define ROBOT_POSE_WALK_LEFT_THRESHOLD (75) /** * @brief 机器人向后右角度阀值. * @ingroup SceneDrawer */ #define ROBOT_POSE_WALK_RIGHT_THRESHOLD (75) //////////////////////////////////////////////////////////////////////////////// // 滤波阀值 //////////////////////////////////////////////////////////////////////////////// /** * @brief 左肩膀过滤阀值. * @ingroup SceneDrawer */ #define FILTER_LEFT_SHOULDER (25) /** * @brief 左肘过滤阀值. * @ingroup SceneDrawer */ #define FILTER_LEFT_ELBOW (50) /** * @brief 左手过滤阀值. * @ingroup SceneDrawer */ #define FILTER_LEFT_HAND (50) /** * @brief 右肩膀过滤阀值. * @ingroup SceneDrawer */ #define FILTER_RIGHT_SHOULDER (25) /** * @brief 右肘过滤阀值. * @ingroup SceneDrawer */ #define FILTER_RIGHT_ELBOW (50) /** * @brief 右手过滤阀值. * @ingroup SceneDrawer */ #define FILTER_RIGHT_HAND (50) /** * @brief 左髋过滤阀值. * @ingroup SceneDrawer */ #define FILTER_LEFT_HIP (50) /** * @brief 左膝过滤阀值. * @ingroup SceneDrawer */ #define FILTER_LEFT_KNEE (50) /** * @brief 左脚过滤阀值. * @ingroup SceneDrawer */ #define FILTER_LEFT_FOOT (50) /** * @brief 右髋过滤阀值. * @ingroup SceneDrawer */ #define FILTER_RIGHT_HIP (50) /** * @brief 右膝过滤阀值. * @ingroup SceneDrawer */ #define FILTER_RIGHT_KNEE (50) /** * @brief 右脚过滤阀值. * @ingroup SceneDrawer */ #define FILTER_RIGHT_FOOT (50) //////////////////////////////////////////////////////////////////////////////// // OpenNI使用 //////////////////////////////////////////////////////////////////////////////// static std::map<XnUInt32, std::pair<XnCalibrationStatus, XnPoseDetectionStatus> > s_Errors; void XN_CALLBACK_TYPE MyCalibrationInProgress(xn::SkeletonCapability& capability, XnUserID id, XnCalibrationStatus calibrationError, void* pCookie) { s_Errors[id].first = calibrationError; } void XN_CALLBACK_TYPE MyPoseInProgress(xn::PoseDetectionCapability& capability, const XnChar* strPose, XnUserID id, XnPoseDetectionStatus poseError, void* pCookie) { s_Errors[id].second = poseError; } unsigned int getClosestPowerOfTwo(unsigned int n) { unsigned int m = 2; while(m < n) m<<=1; return m; } GLuint initTexture(void** buf, int& width, int& height) { GLuint texID = 0; glGenTextures(1,&texID);