使用Kinect2.0控制VREP中的虚拟模型

VREP中直接设置物体姿态的函数有3个：

1. simSetObjectOrientation：通过欧拉角设置姿态
2. simSetObjectQuaternion：通过四元数设置姿态
3. simSetObjectMatrix：通过旋转矩阵设置姿态（同时也可以设置位置）

　　通过设置物体矩阵可以同时改变其位置与姿态，参数matrix是一个包含12个元素的列表: 12 simFloat values (the last row of the 4x4 matrix (0,0,0,1) is not needed).

　　The x-axis of the orientation component is (matrix[0],matrix[4],matrix[8])

　　The y-axis of the orientation component is (matrix[1],matrix[5],matrix[9])

　　The z-axis of the orientation component is (matrix[2],matrix[6],matrix[10])

　　The translation component is (matrix[3],matrix[7],matrix[11])

　　下面将一个坐标系绕X轴旋转90°，则可以分别使用欧拉角、四元数或变换矩阵实现：

handle=simGetObjectHandle('Marker')

local eulerAngles = {math.pi/, , }   -- X-Y-Z Euler angles
local quaternion = {0.707, , , 0.707} -- {x,y,z,w}
local matrix = {,,,, ,,-,, ,,,0.5}
simSetObjectOrientation(handle, -, eulerAngles)
--simSetObjectQuaternion(handle, -1, quaternion)
--simSetObjectMatrix(handle, -1, matrix)

• 球关节

　　Spherical joints have three DoF and are used to describe rotational movements (with 3 DoF) between objects. Their configuration is defined by three values that represent the amount of rotation around their first reference frame's x-, y- and z-axis. The three values that define a spherical joint's configuration are specified as Euler angles. In some situations, a spherical joint can be thought of as 3 concurrent and orthogonal to each other joints, that are parented in a hierarchy-chain. Spherical joints are always passive joints, and cannot act as motors.

[Two equivalent mechanisms (in this configuration): spherical joint (left) and 3 revolute joints (right)]

　　在场景中创建一个球关节和一个连杆（处于竖直状态），将连杆拖到球关节下面作为其子节点，球关节设置为Passive模式。下面的代码控制了球关节的姿态，使用simSetSphericalJointMatrix函数设置关节旋转矩阵，使得连杆绕X轴旋转90°

handle=simGetObjectHandle('Spherical_joint')
local matrix = {,,,,  ,,-,,  ,,,}  -- the translational components will be ignored
-- Sets the intrinsic orientation matrix of a spherical joint object. This function cannot be used with non-spherical joints
simSetSphericalJointMatrix(handle, matrix) 

---

• C++客户端程序与VREP通信

　　进行C++客户端程序与VREP服务端通信，需要对工程进行如下配置：

　　1. 在项目中包含下列文件（可以在V-REP安装路径的programming/remoteApi文件夹下找到这些文件）

• extApi.h
• extApi.c
• extApiPlatform.h (contains platform specific code)
• extApiPlatform.c (contains platform specific code)

　　2. 在项目属性-->C/C++-->预处理器-->预处理器定义中定义：NON_MATLAB_PARSING 和 MAX_EXT_API_CONNECTIONS=255

　　3. 在项目属性-->C/C++-->常规-->附加包含目录中包含：

　　C:\Program Files\V-REP3\V-REP_PRO_EDU\programming\include

　　C:\Program Files\V-REP3\V-REP_PRO_EDU\programming\remoteApi

---

　　下面创建一个简单的场景使用Kinect来控制NAO机器人头部的运动。具体步骤是获得Neck关节的姿态四元数后将其转换为欧拉角，经过适当转换后通过simxSetJointPosition函数直接设置转动关节的角度（关节要设置为Passive模式）。如果不通过关节来控制头部的运动也可以直接采用上面提到的SetObjectOrientation、SetObjectQuaternion或SetObjectMatrix方式来设置头部姿态。需要注意的是Kinect的Head关节为末端节点，不包含姿态信息，因此这里采用了Neck关节的姿态来控制头部，但效果不是很好。如果想直接获取头部姿态，可以参考Kinect for Windows SDK 2.0中的HD Face Basics例子，其中FaceFrameResult Class可以获取代表面部姿态的四元数：

hr = pFaceFrameResult -> get_FaceRotationQuaternion(&faceRotation);

　　下面是一些与之相关的代码。最容易出错的部分是在于Kinect坐标系和VREP坐标系的姿态不一样，因此获得的角度要经过适当转换：Kinect中头部左右摇摆是绕Y轴，而VREP中是绕Z轴；Kinect中头向上仰为绕X轴正方向，而VREP中头向上仰是绕Y轴负方向...

#define PI 3.1415926

int   Sign(double x) { if (x < ) return -; else return ; }

float R2D(float angle){ return angle * 180.0 / PI; }

enum  RotSeq{ zyx, xyz }; // 欧拉角旋转次序

CameraSpacePoint CBodyBasics::QuaternionToEuler(Vector4 q, RotSeq rotSeq)
{
    CameraSpacePoint euler = {  };
    const double Epsilon = 0.0009765625f;
    const double Threshold = 0.5f - Epsilon;

    switch (rotSeq)
    {
        case zyx:  // Z-Y-X Euler angles(RPY angles)
        {
            double TEST = q.w*q.y - q.x*q.z;
            if (TEST < -Threshold || TEST > Threshold) // 奇异姿态,俯仰角为±90°
            {
                int sign = Sign(TEST);
                euler.Z = - * sign * (double)atan2(q.x, q.w); // yaw
                euler.Y = sign * (PI / 2.0); // pitch
                euler.X = ; // roll

            }
            else
            {
                euler.X = atan2( * (q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z); // roll
                euler.Y = asin(- * (q.x*q.z - q.w*q.y));                                         // pitch
                euler.Z = atan2( * (q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z); // yaw
            }
        }
        break;

        case xyz:  // X-Y-Z Euler angles
        {
            double TEST = q.x*q.z + q.w*q.y;
            if (TEST < -Threshold || TEST > Threshold) // 奇异姿态,俯仰角为±90°
            {
                int sign = Sign(TEST);
                euler.X =  * sign * (double)atan2(q.x, q.w);
                euler.Y = sign * (PI / 2.0);
                euler.Z = ;
            }
            else
            {
                euler.X = atan2(- * (q.y*q.z - q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
                euler.Y = asin( * (q.x*q.z + q.w*q.y));
                euler.Z = atan2(- * (q.x*q.y - q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);
            }
        }

    }
    return euler;
}

/// Handle new body data
void CBodyBasics::ProcessBody(IBody* pBody)
{
    HRESULT hr;
    BOOLEAN bTracked = false;
    hr = pBody->get_IsTracked(&bTracked);  // Retrieves a boolean value that indicates if the body is tracked

    if (SUCCEEDED(hr) && bTracked)  // 判断是否追踪到骨骼
    {
        Joint joints[JointType_Count];
        JointOrientation jointOrientations[JointType_Count];

        HandState leftHandState = HandState_Unknown;
        HandState rightHandState = HandState_Unknown;

        DepthSpacePoint *depthSpacePosition = new DepthSpacePoint[_countof(joints)]; // 存储深度坐标系中的关节点位置

        pBody->get_HandLeftState(&leftHandState);  // 获取左右手状态
        pBody->get_HandRightState(&rightHandState);

        hr = pBody->GetJointOrientations(_countof(joints), jointOrientations); // 获取joint orientation
        if (SUCCEEDED(hr))
        {
            CameraSpacePoint euler = QuaternionToEuler(jointOrientations[JointType_Neck].Orientation, xyz); // 四元数转换为X-Y-Z欧拉角

            simxSetJointPosition(clientID, Handle_Yaw,   euler.Y, simx_opmode_oneshot);     // 控制头部左右摆动
            simxSetJointPosition(clientID, Handle_Pitch, PI-euler.X, simx_opmode_oneshot);  // 控制头部俯仰

            extApi_sleepMs();
        }

        hr = pBody->GetJoints(_countof(joints), joints); // 获得25个关节点
        if (SUCCEEDED(hr))
        {
            // Filtered Joint
            filter.Update(joints);
            const DirectX::XMVECTOR* vec = filter.GetFilteredJoints();    // Retrive Filtered Joints

            for (int type = ; type < JointType_Count; type++)
            {
                if (joints[type].TrackingState != TrackingState::TrackingState_NotTracked)
                {
                    float x = 0.0f, y = 0.0f, z = 0.0f;
                    // Retrieve the x/y/z component of an XMVECTOR Data and storing that component's value in an instance of float referred to by a pointer
                    DirectX::XMVectorGetXPtr(&x, vec[type]);
                    DirectX::XMVectorGetYPtr(&y, vec[type]);
                    DirectX::XMVectorGetZPtr(&z, vec[type]);
                    CameraSpacePoint cameraSpacePoint = { x, y, z };
                    m_pCoordinateMapper->MapCameraPointToDepthSpace(cameraSpacePoint, &depthSpacePosition[type]);
                }
            }
            DrawBody(joints, depthSpacePosition);
            DrawHandState(depthSpacePosition[JointType_HandLeft], leftHandState);
            DrawHandState(depthSpacePosition[JointType_HandRight], rightHandState);
        }
        delete[] depthSpacePosition;
    }
    cv::imshow("skeletonImg", skeletonImg);
    cv::waitKey(); // 延时5ms

/// Constructor
CBodyBasics::CBodyBasics() :
m_pKinectSensor(NULL),
m_pCoordinateMapper(NULL),
m_pBodyFrameReader(NULL)
{
    clientID = simxStart((simxChar*)"127.0.0.1", , true, true, , ); // 连接VREP服务端
    if (clientID != -)
    {
        std::cout << "Connected to remote API server" << std::endl;

        // Send some data to V-REP in a non-blocking fashion:
        simxAddStatusbarMessage(clientID, "Connected to V-REP!", simx_opmode_oneshot);

        Handle_Yaw = ;
        Handle_Pitch = ;
        simxGetObjectHandle(clientID, "HeadYaw",   &Handle_Yaw, simx_opmode_oneshot_wait);   // 获取VREP中Yaw关节的句柄
        simxGetObjectHandle(clientID, "HeadPitch", &Handle_Pitch, simx_opmode_oneshot_wait); // 获取VREP中Pitch关节句柄
    }

}

/// Destructor
CBodyBasics::~CBodyBasics()
{
    SafeRelease(m_pBodyFrameReader);
    SafeRelease(m_pCoordinateMapper);

    if (m_pKinectSensor)
    {
        m_pKinectSensor->Close();
    }
    SafeRelease(m_pKinectSensor);

    // Close the connection to V-REP:
    simxFinish(clientID);
}

　　另外还有一个问题就是原始数据的抖动比较大，头部旋转的时候不够平滑，有很多种滤波方式可以解决这一问题。最简单的移动平均滤波参考代码如下：

#include <iostream>
#include <stddef.h>
#include <assert.h>

using std::cout;
using std::endl;

// Simple_moving_average
class SMA
{
public:
    SMA(unsigned int period) :period(period), window(new double[period]), head(NULL), tail(NULL), total()
    {
        assert(period >= );
    }
    ~SMA()
    {
        delete[] window;
    }

    // Adds a value to the average, pushing one out if nescessary
    void add(double val)
    {
        // Special case: Initialization
        if (head == NULL)
        {
            head = window;
            *head = val;
            tail = head;
            inc(tail);
            total = val;
            return;
        }

        // Were we already full?
        if (head == tail)
        {
            // Fix total-cache
            total -= *head;
            // Make room
            inc(head);
        }

        // Write the value in the next spot.
        *tail = val;
        inc(tail);

        // Update our total-cache
        total += val;
    }

    // Returns the average of the last P elements added to this SMA.
    // If no elements have been added yet, returns 0.0
    double avg() const
    {
        ptrdiff_t size = this->size();
        if (size == )
            return ; // No entries => 0 average

        return total / (double)size; // Cast to double for floating point arithmetic
    }

private:
    unsigned int period;
    double * window; // Holds the values to calculate the average of.

    // Logically, head is before tail
    double * head; // Points at the oldest element we've stored.
    double * tail; // Points at the newest element we've stored.

    double total; // Cache the total so we don't sum everything each time.

    // Bumps the given pointer up by one.
    // Wraps to the start of the array if needed.
    void inc(double * & p)
    {
        if (++p >= window + period)
        {
            p = window;
        }
    }

    // Returns how many numbers we have stored.
    ptrdiff_t size() const
    {
        if (head == NULL)
            return ;
        if (head == tail)
            return period;
        return (period + tail - head) % period;
    }
};

int main(int argc, char * * argv)
{
    SMA foo();
    SMA bar();

    int data[] = { , , , , , , , , ,  };
    for (int * itr = data; itr < data + ; itr++)
    {
        foo.add(*itr);
        cout << "Added " << *itr << " avg: " << foo.avg() << endl;
    }

    cout << endl;

    for (int * itr = data; itr < data + ; itr++)
    {
        bar.add(*itr);
        cout << "Added " << *itr << " avg: " << bar.avg() << endl;
    }

    return ;
}

　　下面是NAO随着我的头部先进行俯仰，然后左右摇摆的动态图：

　　另外也可以使用获取到的三维坐标点计算出关节夹角，以此来控制虚拟模型。下面计算出肘关节和肩关节角度，来控制VREP场景中的一个2*度手臂：

参考：

Kinect2.0骨骼层次与Joint Orientation

quaternions.online

Averages/Simple moving average