UE4数字人驱动（一）—坐标系转换

好记性不如烂笔头，之前通过研究VRPN实现了自定义的设备添加，现在需要在UE4里利用采集的人体运动数据进行数字人驱动。整个功能实现包括：UE4端VRPN数据接收、数据坐标系转化、动捕骨骼与数字人骨骼匹配与驱动三个部分。下边我挨个记录一下，由于篇幅问题，最后一部分动捕骨骼与数字人骨骼匹配与驱动我拆出来单独写一篇，本文仅涵盖前两部分内容。

1 UE4端VRPN数据接收

这一部分我用了同事帮忙写的模块，还没抽出时间自己搞一下，总体是参照VRPN里给的print_print_devices这个例子，里面教了怎么通过C++获取VRPN的各类数据，包括tracker、button、analog这些，参考这个然后在UE4里写个插件，起个线程获取并输出出来就行了，没什么技术难度就是花点时间，以后如果我有时间研究一下我就自己写一个补到这里。

2 数据坐标系转化

2.1 转化原理

这一部分是比较关键的，也相对比较麻烦，我数学比较渣，在找同学请教后才把关系基本搞清楚，在此我详细记录一下。这是个通用的问题，把optitrack接入存在，把其他动捕设备的数据接入也是一样的。

这个问题的核心是将一个任意朝向设备坐标系（左手系/右手系，对应外部动捕设备，在此为optitrack）中的位置坐标以及表征旋转的四元数转化到UE4坐标系下，本质是计算从单位正交基A到单位正交基B的转换矩阵T。现定义变量如下：

设Optitrack坐标系为O，其三个轴分别表示为；

设UE4坐标系为U,其三个轴分别表示为；

设Optitrack坐标系下任意一刚体的位置坐标为，旋转四元数为，旋转矩阵为，对应UE4坐标下的表示为，旋转四元数为，旋转矩阵为；

设从坐标系O到坐标系U的3*3转换矩阵为(坐标系间的位置偏差比较好处理就是加个offset的事，而且在UE4里可以通过添加父节点进行控制，所以在此，我们假设两个坐标系原点重合，只需要旋转或者翻转坐标轴)，从坐标系U到O的转换矩阵为。不论坐标系U或者O为左手系还是右手系，其构成均为单位正交基，故为一正交变换，则有。以及为相似变换。

则有：

四元数与旋转矩阵的转化关系网上有很多，就不多赘述了。综上，只要可以计算出，即可实现动捕数据中的位置以及姿态向UE4坐标系下的转化。

计算需要在空间中找3个点，并获得这三个点在坐标系O以及坐标系U中的表达式，为简化计算，我们选取坐标系O中的(1,0,0)，(0,1,0)以及(0,0,1)作为计算点。为进一步简化计算，在进行坐标系转化时，我们可以假设两个坐标系的三条轴是共线的，示例如下图所示。（对于不共线的情况，只要给出三个点在另一组坐标系下的表达式进行计算即可。通常如在UE4等渲染引擎中使用，可以直接在世界中设置一个节点，将此节点定义为Optitrack坐标系的原点（在此局部坐标系下坐标轴共线），进一步通过移动和旋转这个节点实现局部坐标系相对世界坐标的旋转与平移）

 我们按照示例的对应关系进行计算，则有坐标系O中的(1,0,0)，(0,1,0)以及(0,0,1)三个点在坐标系U下的表达分别为(0,-1,0)，(0,0,1)，(1,0,0)，带入上述公式易得：

2.2 程序设计

 现在原理基本都已经清楚了，现在就要想办法在UE4里实现一下了。

这部分代码我是直接在同事的接收模块里改的，就是增加一部分坐标系匹配的函数，在数据输出前进行一下转化。这里面比较麻烦的就是如何描述两个坐标系间的对应关系，这个对应关系虽然只有6*4*2一共48种，但是每种都独立写一个判断太麻烦了，所以我思考了一下，通过变量设置来指定关系。首先定义了前(front)，右(right)、上(up)三个方向，定义这个主要为了好理解，因为在进行动捕的时候很多应用场景会有一个主方向，比如驾驶模拟的前方，场景的进入方向等，增加这三个方向比较容易与物理世界进行匹配。由于可以在UE4中创建局部坐标，所以我按照UE4的预设直接定义front为UE4中的x轴，right为UE4中的y轴，up为UE4中的z轴。通过设置front，right以及up对应的Optitrack的轴向来确定两者之间的关系，具体也不好说清楚，直接上代码了，相关解释在注释里有。

/*Note*/
/*
These functions are used to transform the incoming tracking data coordinate to UE4 coordinate(x-Front, y-Right, z-Up)
The parameters front,right,up refers to the corresponding axis of current coordinate(Optitrack); 0,1,2 are x,y,z; 3,4,5 are -x,-y,-z 
*/

//judge Whether the given coordinate is a right hand coord or a left hand coord
//input:the coordinate of three axis represented in a right hand cordination 
bool MotiveTracking::IsRightHandCoord(int x[3], int y[3], int z[3]) {
	int CrossMV[3] = { x[1] * y[2] - x[2] * y[1],x[2] * y[0] - x[0] * y[2],x[0] * y[1] - x[1] * y[0] };
	if ((CrossMV[0] - z[0] + CrossMV[1] - z[1] + CrossMV[2] - z[2]) == 0) {
		return false;
	}
	else
	{
		return true;
	}

}
//Calculate the transpose of a matrix
void  MotiveTracking::TMatrix(q_matrix_type& transMatrix, q_matrix_type inMatrix) {
	for (size_t i = 0; i < 4; i++)
	{
		for (size_t j = 0; j < 4; j++)
		{
			transMatrix[i][j] = inMatrix[j][i];
		}

	}

}
//calculate the M*V
void  MotiveTracking::Matrix_LeftMulti_Vector(float resVector[4], q_matrix_type leftMatrix, float mulVector[4]) {
	q_matrix_type transMatrix = { {0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0} };
	for (size_t i = 0; i < 4; i++)
	{
		resVector[i] = 0;
		for (size_t j = 0; j < 4; j++)
		{
			resVector[i] += leftMatrix[i][j] * mulVector[j];
		}

	}

}
void MotiveTracking::CoordTransfromInit(int front, int right, int up) {
	int coordV[6] = { 1, 1, 1, -1, -1, -1 };//refer to x,y,z,-x,-y,-z
	//The axis in UE4 presented in the current coordinate
	//For example:
	//If front is y,right is -x,up is z. 
	//Then front=1,right=3,up=2. 
	//And x_invert = [0,1,0],y_invert = [-1,0,0],z_invert = [0,0,1]
	int x_invt[3] = { 0,0,0 };//the x axis of UE4 represented in Optitrack coordinate
	x_invt[front % 3] = coordV[front];
	int y_invt[3] = { 0,0,0 };//the y axis of UE4 represented in Optitrack coordinate
	y_invt[right % 3] = coordV[right];
	int z_invt[3] = { 0,0,0 };//the z axis of UE4 represented in Optitrack coordinate
	z_invt[up % 3] = coordV[up];
	//Wether is a right hand coord(useless)
	if (IsRightHandCoord(x_invt, y_invt, z_invt)) {
		isRighthandCoord = true;
	}
	else
	{
		isRighthandCoord = false;
	}
	//The TransMatrix from UE4 to current(Optitrack)
	
	q_matrix_type middleMatrix = { {x_invt[0],y_invt[0],z_invt[0],0},{x_invt[1],y_invt[1],z_invt[1],0},{x_invt[2],y_invt[2],z_invt[2],0},{0,0,0,1} };
	q_matrix_copy(TransMatrix_UE2Out, middleMatrix);
	//Tranpose the TransMatrix to get the inverse Matrix( the TransMatrix from Current to UE4)
	TMatrix(TransMatrix_Out2UE, TransMatrix_UE2Out);
}

FVector MotiveTracking::TransPos(float x, float y, float z,int zoom) {
	float CurrentPos4[4] = { x* zoom,y * zoom,z * zoom,1 };
	float pos4[4] = { 0,0,0,0 };
	Matrix_LeftMulti_Vector(pos4, TransMatrix_Out2UE, CurrentPos4);
	float pos[3] = { pos4[0],pos4[1], pos4[2] };

	return FVector(pos[0], pos[1], pos[2]);
}

FVector4 MotiveTracking::TransQuat(float x, float y, float z, float w) {
	q_type CurrentQuat = { x,y,z,w };
	q_matrix_type quatMatrix = { {0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0} };
	q_to_col_matrix(quatMatrix, CurrentQuat);
	q_matrix_type middleQuatMatrix = { {0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0} };
	q_matrix_type newQuatMatrix = { {0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0} };
	q_matrix_mult(middleQuatMatrix, quatMatrix, TransMatrix_UE2Out);
	q_matrix_mult(newQuatMatrix, TransMatrix_Out2UE, middleQuatMatrix);
	q_type destQuat = { 0,0,0,0 };
	q_from_col_matrix(destQuat, newQuatMatrix);

	return FVector4(destQuat[0], destQuat[1], destQuat[2], destQuat[3]);
}