#include "AzureKinectFunc.h"

AzureKinect::AzureKinect() {
    m_bOpen = false;
    tracker = NULL;
    device = NULL;
}

int AzureKinect::cameraCapture(int CapFreq, int CapTimes) {

    k4a_device_open(0, &device);

    const uint32_t device_count = k4a_device_get_installed_count();
    if (1 != device_count)
    {
        return -1;
    }

    //打开设备
    k4a_device_open(0, &device);
    //std::cout << "Done: open device. " << std::endl;

    // Start camera. Make sure depth camera is enabled.
    k4a_device_configuration_t deviceConfig = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
    deviceConfig.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
    deviceConfig.color_resolution = K4A_COLOR_RESOLUTION_720P;
    deviceConfig.camera_fps = K4A_FRAMES_PER_SECOND_30;
    deviceConfig.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
    deviceConfig.synchronized_images_only = true;// ensures that depth and color images are both available in the capture

    //开始相机
    //k4a_device_start_cameras(device, &deviceConfig);
    k4a_device_start_cameras(device, &deviceConfig);

    //查询传感器校准
    k4a_calibration_t sensor_calibration;
    k4a_device_get_calibration(device, deviceConfig.depth_mode, deviceConfig.color_resolution, &sensor_calibration);
  
    //创建人体跟踪器
    k4abt_tracker_configuration_t tracker_config = K4ABT_TRACKER_CONFIG_DEFAULT;
    k4abt_tracker_create(&sensor_calibration, tracker_config, &tracker);

    cv::Mat cv_rgbImage_with_alpha;
    cv::Mat cv_rgbImage_no_alpha;
    cv::Mat cv_depth;
    cv::Mat cv_depth_8U;

    std::ofstream fs;
    fs.open("skeletonpointsAK.txt");
    std::ofstream fs1;
    fs1.open("skeletonquatAK.txt");

    int frame_count = 0;
    m_bOpen = true;
    while (true)
    {
        if (!m_bOpen)
        {
            return 0;
        }
        k4a_capture_t sensor_capture;
        k4a_wait_result_t get_capture_result = k4a_device_get_capture(device, &sensor_capture, K4A_WAIT_INFINITE);
        //获取RGB和depth图像
        k4a_image_t rgbImage = k4a_capture_get_color_image(sensor_capture);
        k4a_image_t depthImage = k4a_capture_get_depth_image(sensor_capture);
        //RGB
        cv_rgbImage_with_alpha = cv::Mat(k4a_image_get_height_pixels(rgbImage), k4a_image_get_width_pixels(rgbImage), CV_8UC4, k4a_image_get_buffer(rgbImage));
        cvtColor(cv_rgbImage_with_alpha, cv_rgbImage_no_alpha, cv::COLOR_BGRA2BGR);
        //depth
        cv_depth = cv::Mat(k4a_image_get_height_pixels(depthImage), k4a_image_get_width_pixels(depthImage), CV_16U, k4a_image_get_buffer(depthImage), k4a_image_get_stride_bytes(depthImage));
        cv_depth.convertTo(cv_depth_8U, CV_8U, 1);


        //计算姿态
        if (get_capture_result == K4A_WAIT_RESULT_SUCCEEDED)
        {
            frame_count++;
            k4a_wait_result_t queue_capture_result = k4abt_tracker_enqueue_capture(tracker, sensor_capture, K4A_WAIT_INFINITE);
            k4a_capture_release(sensor_capture); // Remember to release the sensor capture once you finish using it
            if (queue_capture_result == K4A_WAIT_RESULT_TIMEOUT)
            {
                // It should never hit timeout when K4A_WAIT_INFINITE is set.
                //printf("Error! Add capture to tracker process queue timeout!\n");
                return -1;
                break;
            }
            else if (queue_capture_result == K4A_WAIT_RESULT_FAILED)
            {
                //printf("Error! Add capture to tracker process queue failed!\n");
                return -1;
                break;
            }

            k4abt_frame_t body_frame = NULL;
            k4a_wait_result_t pop_frame_result = k4abt_tracker_pop_result(tracker, &body_frame, K4A_WAIT_INFINITE);
            if (pop_frame_result == K4A_WAIT_RESULT_SUCCEEDED)
            {
                // Successfully popped the body tracking result. Start your processing 
                size_t num_bodies = k4abt_frame_get_num_bodies(body_frame);

                //std::cout << "Body Numbers: " << num_bodies << std::endl;
                if (frame_count%CapFreq==0)
                {
                    for (size_t i = 0; i < 1; i++)//num_bodies 现在只录一个人
                    {
                        k4abt_skeleton_t skeleton;
                        k4abt_frame_get_body_skeleton(body_frame, i, &skeleton);
                        //std::cout << typeid(skeleton.joints->position.v).name();

                        k4a_float2_t P_HipCenter_2D;
                        k4a_float2_t P_Spine_2D;
                        k4a_float2_t P_ShoulderCenter_2D;
                        k4a_float2_t P_Head_2D;
                        k4a_float2_t P_ShoulderLeft_2D;
                        k4a_float2_t P_ElbowLeft_2D;
                        k4a_float2_t P_WristLeft_2D;
                        k4a_float2_t P_HandLeft_2D;
                        k4a_float2_t P_ShoulderRight_2D;
                        k4a_float2_t P_ElbowRight_2D;
                        k4a_float2_t P_WristRight_2D;
                        k4a_float2_t P_HandRight_2D;
                        k4a_float2_t P_HipLeft_2D;
                        k4a_float2_t P_KneeLeft_2D;
                        k4a_float2_t P_AnkleLeft_2D;
                        k4a_float2_t P_FootLeft_2D;
                        k4a_float2_t P_HipRight_2D;
                        k4a_float2_t P_KneeRight_2D;
                        k4a_float2_t P_AnkleRight_2D;
                        k4a_float2_t P_FootRight_2D;
                        int result;

                        //HipCenter
                        k4abt_joint_t  P_HipCenter = skeleton.joints[K4ABT_JOINT_PELVIS];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_HipCenter.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_HipCenter_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_HipCenter_2D.xy.x, P_HipCenter_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_HipCenter.position.v[0] / 10 << " " << -P_HipCenter.position.v[1] / 10 << " " << P_HipCenter.position.v[2] / 10 << std::endl;
                        fs1 << P_HipCenter.orientation.wxyz.w << " " << P_HipCenter.orientation.wxyz.x << " " << P_HipCenter.orientation.wxyz.y << " " << P_HipCenter.orientation.wxyz.z << std::endl;

                        //Spine
                        k4abt_joint_t  P_Spine = skeleton.joints[K4ABT_JOINT_SPINE_NAVEL];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_Spine.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_Spine_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_Spine_2D.xy.x, P_Spine_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_Spine.position.v[0] / 10 << " " << -P_Spine.position.v[1] / 10 << " " << P_Spine.position.v[2] / 10 << std::endl;
                        fs1 << P_Spine.orientation.wxyz.w << " " << P_Spine.orientation.wxyz.x << " " << P_Spine.orientation.wxyz.y << " " << P_Spine.orientation.wxyz.z << std::endl;

                        //ShoulderCenter
                        k4abt_joint_t  P_ShoulderCenter = skeleton.joints[K4ABT_JOINT_NECK];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_ShoulderCenter.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_ShoulderCenter_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_ShoulderCenter_2D.xy.x, P_ShoulderCenter_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_ShoulderCenter.position.v[0] / 10 << " " << -P_ShoulderCenter.position.v[1] / 10 << " " << P_ShoulderCenter.position.v[2] / 10 << std::endl;
                        fs1 << P_ShoulderCenter.orientation.wxyz.w << " " << P_ShoulderCenter.orientation.wxyz.x << " " << P_ShoulderCenter.orientation.wxyz.y << " " << P_ShoulderCenter.orientation.wxyz.z << std::endl;

                        //Head
                        k4abt_joint_t  P_Head = skeleton.joints[K4ABT_JOINT_HEAD];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_Head.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_Head_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_Head_2D.xy.x, P_Head_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_Head.position.v[0] / 10 << " " << -P_Head.position.v[1] / 10 << " " << P_Head.position.v[2] / 10 << std::endl;
                        fs1 << P_Head.orientation.wxyz.w << " " << P_Head.orientation.wxyz.x << " " << P_Head.orientation.wxyz.y << " " << P_Head.orientation.wxyz.z << std::endl;

                        //ShoulderLeft
                        k4abt_joint_t  P_ShoulderLeft = skeleton.joints[K4ABT_JOINT_SHOULDER_LEFT];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_ShoulderLeft.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_ShoulderLeft_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_ShoulderLeft_2D.xy.x, P_ShoulderLeft_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_ShoulderLeft.position.v[0] / 10 << " " << -P_ShoulderLeft.position.v[1] / 10 << " " << P_ShoulderLeft.position.v[2] / 10 << std::endl;
                        fs1 << P_ShoulderLeft.orientation.wxyz.w << " " << P_ShoulderLeft.orientation.wxyz.x << " " << P_ShoulderLeft.orientation.wxyz.y << " " << P_ShoulderLeft.orientation.wxyz.z << std::endl;

                        //ElbowLeft
                        k4abt_joint_t  P_ElbowLeft = skeleton.joints[K4ABT_JOINT_ELBOW_LEFT];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_ElbowLeft.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_ElbowLeft_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_ElbowLeft_2D.xy.x, P_ElbowLeft_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_ElbowLeft.position.v[0] / 10 << " " << -P_ElbowLeft.position.v[1] / 10 << " " << P_ElbowLeft.position.v[2] / 10 << std::endl;
                        fs1 << P_ElbowLeft.orientation.wxyz.w << " " << P_ElbowLeft.orientation.wxyz.x << " " << P_ElbowLeft.orientation.wxyz.y << " " << P_ElbowLeft.orientation.wxyz.z << std::endl;

                        //WristLeft
                        k4abt_joint_t  P_WristLeft = skeleton.joints[K4ABT_JOINT_WRIST_LEFT];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_WristLeft.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_WristLeft_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_WristLeft_2D.xy.x, P_WristLeft_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_WristLeft.position.v[0] / 10 << " " << -P_WristLeft.position.v[1] / 10 << " " << P_WristLeft.position.v[2] / 10 << std::endl;
                        fs1 << P_WristLeft.orientation.wxyz.w << " " << P_WristLeft.orientation.wxyz.x << " " << P_WristLeft.orientation.wxyz.y << " " << P_WristLeft.orientation.wxyz.z << std::endl;

                        //HandLeft
                        k4abt_joint_t  P_HandLeft = skeleton.joints[K4ABT_JOINT_HAND_LEFT];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_HandLeft.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_HandLeft_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_HandLeft_2D.xy.x, P_HandLeft_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_HandLeft.position.v[0] / 10 << " " << -P_HandLeft.position.v[1] / 10 << " " << P_HandLeft.position.v[2] / 10 << std::endl;
                        fs1 << P_HandLeft.orientation.wxyz.w << " " << P_HandLeft.orientation.wxyz.x << " " << P_HandLeft.orientation.wxyz.y << " " << P_HandLeft.orientation.wxyz.z << std::endl;

                        //ShoulderRight
                        k4abt_joint_t  P_ShoulderRight = skeleton.joints[K4ABT_JOINT_SHOULDER_RIGHT];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_ShoulderRight.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_ShoulderRight_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_ShoulderRight_2D.xy.x, P_ShoulderRight_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_ShoulderRight.position.v[0] / 10 << " " << -P_ShoulderRight.position.v[1] / 10 << " " << P_ShoulderRight.position.v[2] / 10 << std::endl;
                        fs1 << P_ShoulderRight.orientation.wxyz.w << " " << P_ShoulderRight.orientation.wxyz.x << " " << P_ShoulderRight.orientation.wxyz.y << " " << P_ShoulderRight.orientation.wxyz.z << std::endl;

                        //ElbowRight
                        k4abt_joint_t  P_ElbowRight = skeleton.joints[K4ABT_JOINT_ELBOW_RIGHT];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_ElbowRight.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_ElbowRight_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_ElbowRight_2D.xy.x, P_ElbowRight_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_ElbowRight.position.v[0] / 10 << " " << -P_ElbowRight.position.v[1] / 10 << " " << P_ElbowRight.position.v[2] / 10 << std::endl;
                        fs1 << P_ElbowRight.orientation.wxyz.w << " " << P_ElbowRight.orientation.wxyz.x << " " << P_ElbowRight.orientation.wxyz.y << " " << P_ElbowRight.orientation.wxyz.z << std::endl;

                        //WristRight
                        k4abt_joint_t  P_WristRight = skeleton.joints[K4ABT_JOINT_WRIST_RIGHT];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_WristRight.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_WristRight_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_WristRight_2D.xy.x, P_WristRight_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_WristRight.position.v[0] / 10 << " " << -P_WristRight.position.v[1] / 10 << " " << P_WristRight.position.v[2] / 10 << std::endl;
                        fs1 << P_WristRight.orientation.wxyz.w << " " << P_WristRight.orientation.wxyz.x << " " << P_WristRight.orientation.wxyz.y << " " << P_WristRight.orientation.wxyz.z << std::endl;

                        //HandRight
                        k4abt_joint_t  P_HandRight = skeleton.joints[K4ABT_JOINT_HAND_RIGHT];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_HandRight.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_HandRight_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_HandRight_2D.xy.x, P_HandRight_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_HandRight.position.v[0] / 10 << " " << -P_HandRight.position.v[1] / 10 << " " << P_HandRight.position.v[2] / 10 << std::endl;
                        fs1 << P_HandRight.orientation.wxyz.w << " " << P_HandRight.orientation.wxyz.x << " " << P_HandRight.orientation.wxyz.y << " " << P_HandRight.orientation.wxyz.z << std::endl;

                        //HipLeft
                        k4abt_joint_t  P_HipLeft = skeleton.joints[K4ABT_JOINT_HIP_LEFT];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_HipLeft.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_HipLeft_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_HipLeft_2D.xy.x, P_HipLeft_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_HipLeft.position.v[0] / 10 << " " << -P_HipLeft.position.v[1] / 10 << " " << P_HipLeft.position.v[2] / 10 << std::endl;
                        fs1 << P_HipLeft.orientation.wxyz.w << " " << P_HipLeft.orientation.wxyz.x << " " << P_HipLeft.orientation.wxyz.y << " " << P_HipLeft.orientation.wxyz.z << std::endl;

                        //KneeLeft
                        k4abt_joint_t  P_KneeLeft = skeleton.joints[K4ABT_JOINT_KNEE_LEFT];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_KneeLeft.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_KneeLeft_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_KneeLeft_2D.xy.x, P_KneeLeft_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_KneeLeft.position.v[0] / 10 << " " << -P_KneeLeft.position.v[1] / 10 << " " << P_KneeLeft.position.v[2] / 10 << std::endl;
                        fs1 << P_KneeLeft.orientation.wxyz.w << " " << P_KneeLeft.orientation.wxyz.x << " " << P_KneeLeft.orientation.wxyz.y << " " << P_KneeLeft.orientation.wxyz.z << std::endl;

                        //AnkleLeft
                        k4abt_joint_t  P_AnkleLeft = skeleton.joints[K4ABT_JOINT_ANKLE_LEFT];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_AnkleLeft.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_AnkleLeft_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_AnkleLeft_2D.xy.x, P_AnkleLeft_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_AnkleLeft.position.v[0] / 10 << " " << -P_AnkleLeft.position.v[1] / 10 << " " << P_AnkleLeft.position.v[2] / 10 << std::endl;
                        fs1 << P_AnkleLeft.orientation.wxyz.w << " " << P_AnkleLeft.orientation.wxyz.x << " " << P_AnkleLeft.orientation.wxyz.y << " " << P_AnkleLeft.orientation.wxyz.z << std::endl;

                        //FootLeft
                        k4abt_joint_t  P_FootLeft = skeleton.joints[K4ABT_JOINT_FOOT_LEFT];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_FootLeft.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_FootLeft_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_FootLeft_2D.xy.x, P_FootLeft_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_FootLeft.position.v[0] / 10 << " " << -P_FootLeft.position.v[1] / 10 << " " << P_FootLeft.position.v[2] / 10 << std::endl;
                        fs1 << P_FootLeft.orientation.wxyz.w << " " << P_FootLeft.orientation.wxyz.x << " " << P_FootLeft.orientation.wxyz.y << " " << P_FootLeft.orientation.wxyz.z << std::endl;

                        //HipRight
                        k4abt_joint_t  P_HipRight = skeleton.joints[K4ABT_JOINT_HIP_RIGHT];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_HipRight.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_HipRight_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_HipRight_2D.xy.x, P_HipRight_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_HipRight.position.v[0] / 10 << " " << -P_HipRight.position.v[1] / 10 << " " << P_HipRight.position.v[2] / 10 << std::endl;
                        fs1 << P_HipRight.orientation.wxyz.w << " " << P_HipRight.orientation.wxyz.x << " " << P_HipRight.orientation.wxyz.y << " " << P_HipRight.orientation.wxyz.z << std::endl;

                        //KneeRight
                        k4abt_joint_t  P_KneeRight = skeleton.joints[K4ABT_JOINT_KNEE_RIGHT];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_KneeRight.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_KneeRight_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_KneeRight_2D.xy.x, P_KneeRight_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_KneeRight.position.v[0] / 10 << " " << -P_KneeRight.position.v[1] / 10 << " " << P_KneeRight.position.v[2] / 10 << std::endl;
                        fs1 << P_KneeRight.orientation.wxyz.w << " " << P_KneeRight.orientation.wxyz.x << " " << P_KneeRight.orientation.wxyz.y << " " << P_KneeRight.orientation.wxyz.z << std::endl;

                        //AnkleRight
                        k4abt_joint_t  P_AnkleRight = skeleton.joints[K4ABT_JOINT_ANKLE_RIGHT];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_AnkleRight.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_AnkleRight_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_AnkleRight_2D.xy.x, P_AnkleRight_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_AnkleRight.position.v[0] / 10 << " " << -P_AnkleRight.position.v[1] / 10 << " " << P_AnkleRight.position.v[2] / 10 << std::endl;
                        fs1 << P_AnkleRight.orientation.wxyz.w << " " << P_AnkleRight.orientation.wxyz.x << " " << P_AnkleRight.orientation.wxyz.y << " " << P_AnkleRight.orientation.wxyz.z << std::endl;

                        //FootRight
                        k4abt_joint_t  P_FootRight = skeleton.joints[K4ABT_JOINT_FOOT_RIGHT];
                        //3D转2D，并在color中画出
                        k4a_calibration_3d_to_2d(&sensor_calibration, &P_FootRight.position, K4A_CALIBRATION_TYPE_DEPTH, K4A_CALIBRATION_TYPE_COLOR, &P_FootRight_2D, &result);
                        cv::circle(cv_rgbImage_no_alpha, cv::Point(P_FootRight_2D.xy.x, P_FootRight_2D.xy.y), 3, cv::Scalar(0, 255, 255), -1);
                        fs << -P_FootRight.position.v[0] / 10 << " " << -P_FootRight.position.v[1] / 10 << " " << P_FootRight.position.v[2] / 10 << std::endl;
                        fs1 << P_FootRight.orientation.wxyz.w << " " << P_FootRight.orientation.wxyz.x << " " << P_FootRight.orientation.wxyz.y << " " << P_FootRight.orientation.wxyz.z << std::endl;

                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_Head_2D.xy.x, P_Head_2D.xy.y), cv::Point(P_ShoulderCenter_2D.xy.x, P_ShoulderCenter_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_HipCenter_2D.xy.x, P_HipCenter_2D.xy.y), cv::Point(P_HipLeft_2D.xy.x, P_HipLeft_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_HipCenter_2D.xy.x, P_HipCenter_2D.xy.y), cv::Point(P_HipRight_2D.xy.x, P_HipRight_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_HipCenter_2D.xy.x, P_HipCenter_2D.xy.y), cv::Point(P_Spine_2D.xy.x, P_Spine_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_Spine_2D.xy.x, P_Spine_2D.xy.y), cv::Point(P_ShoulderCenter_2D.xy.x, P_ShoulderCenter_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_ShoulderCenter_2D.xy.x, P_ShoulderCenter_2D.xy.y), cv::Point(P_ShoulderLeft_2D.xy.x, P_ShoulderLeft_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_ShoulderLeft_2D.xy.x, P_ShoulderLeft_2D.xy.y), cv::Point(P_ElbowLeft_2D.xy.x, P_ElbowLeft_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_ElbowLeft_2D.xy.x, P_ElbowLeft_2D.xy.y), cv::Point(P_WristLeft_2D.xy.x, P_WristLeft_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_WristLeft_2D.xy.x, P_WristLeft_2D.xy.y), cv::Point(P_HandLeft_2D.xy.x, P_HandLeft_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_ShoulderCenter_2D.xy.x, P_ShoulderCenter_2D.xy.y), cv::Point(P_ShoulderRight_2D.xy.x, P_ShoulderRight_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_ShoulderRight_2D.xy.x, P_ShoulderRight_2D.xy.y), cv::Point(P_ElbowRight_2D.xy.x, P_ElbowRight_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_ElbowRight_2D.xy.x, P_ElbowRight_2D.xy.y), cv::Point(P_WristRight_2D.xy.x, P_WristRight_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_WristRight_2D.xy.x, P_WristRight_2D.xy.y), cv::Point(P_HandRight_2D.xy.x, P_HandRight_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_HipLeft_2D.xy.x, P_HipLeft_2D.xy.y), cv::Point(P_KneeLeft_2D.xy.x, P_KneeLeft_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_KneeLeft_2D.xy.x, P_KneeLeft_2D.xy.y), cv::Point(P_AnkleLeft_2D.xy.x, P_AnkleLeft_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_AnkleLeft_2D.xy.x, P_AnkleLeft_2D.xy.y), cv::Point(P_FootLeft_2D.xy.x, P_FootLeft_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_HipRight_2D.xy.x, P_HipRight_2D.xy.y), cv::Point(P_KneeRight_2D.xy.x, P_KneeRight_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_KneeRight_2D.xy.x, P_KneeRight_2D.xy.y), cv::Point(P_AnkleRight_2D.xy.x, P_AnkleRight_2D.xy.y), cv::Scalar(0, 0, 255), 3);
                        cv::line(cv_rgbImage_no_alpha, cv::Point(P_AnkleRight_2D.xy.x, P_AnkleRight_2D.xy.y), cv::Point(P_FootRight_2D.xy.x, P_FootRight_2D.xy.y), cv::Scalar(0, 0, 255), 3);

                        uint32_t id = k4abt_frame_get_body_id(body_frame, i);

                    }
                }
                //printf("%zu bodies are detected!\n", num_bodies);

                k4abt_frame_release(body_frame); // Remember to release the body frame once you finish using it
            }
            else if (pop_frame_result == K4A_WAIT_RESULT_TIMEOUT)
            {
                //  It should never hit timeout when K4A_WAIT_INFINITE is set.
                //printf("Error! Pop body frame result timeout!\n");
                return -1;
                break;
            }
            else
            {
                //printf("Pop body frame result failed!\n");
                return -1;
                break;
            }
        }
        else if (get_capture_result == K4A_WAIT_RESULT_TIMEOUT)
        {
            // It should never hit time out when K4A_WAIT_INFINITE is set.
            //printf("Error! Get depth frame time out!\n");
            return -1;
            break;
        }
        else
        {
            //printf("Get depth capture returned error: %d\n", get_capture_result);
            return -1;
            break;
        }

        // show image
        //cv::resize(cv_rgbImage_no_alpha, cv_rgbImage_no_alpha, cv::Size(cv_rgbImage_no_alpha.cols / 2, cv_rgbImage_no_alpha.rows / 2));
        cv::flip(cv_rgbImage_no_alpha, cv_rgbImage_no_alpha, 1);
        imshow("color", cv_rgbImage_no_alpha);
        //imshow("depth", cv_depth_8U);
        if (frame_count == CapTimes)
        {
            break;
        }
        cv::waitKey(5);
        k4a_image_release(rgbImage);
    }

    //printf("Finished body tracking processing!\n");
    fs.close();
    fs1.close();
    cv::destroyAllWindows();
    k4abt_tracker_shutdown(tracker);
    k4abt_tracker_destroy(tracker);
    k4a_device_stop_cameras(device);
    k4a_device_close(device);
    m_bOpen = false;
    return 0;
}

int AzureKinect::cameraShutDown() {
    m_bOpen = false;
    cv::destroyAllWindows();
    if (tracker != NULL)
    {
        k4abt_tracker_shutdown(tracker);
        k4abt_tracker_destroy(tracker);
    }
    if (device != NULL)
    {
        k4a_device_stop_cameras(device);
        k4a_device_close(device);
    }
    return 0;
}