// tries to estimate the camera pose, according to the tracked body joints
private void GetEstCameraPose(int sIndex, out Vector3 estCamPos, out Quaternion estCamRot)
{
Matrix4x4 matRefUser = Matrix4x4.identity;
Matrix4x4 matCamUser = Matrix4x4.identity;
Vector3 dstCamPos = Vector3.zero;
Quaternion dstCamRot = Quaternion.identity;
Vector3 camPosSum = Vector3.zero;
Vector4 camRotSum = Vector4.zero;
Quaternion camRotFirst = Quaternion.identity;
int numTrackedJoints = 0;
int numEstJoints = 1; // numJoints
for (int j = 0; j < numEstJoints; j++)
{
if (!userBodyPose[0].jointTracked[j] || !userBodyPose[sIndex].jointTracked[j])
{
continue;
}
matRefUser.SetTRS(userBodyPose[0].jointPos[j], userBodyPose[0].jointRot[j], Vector3.one);
matCamUser.SetTRS(userBodyPose[sIndex].jointPos[j], userBodyPose[sIndex].jointRot[j], Vector3.one);
Matrix4x4 dstCamMatrix = srcCameraMatrix * matRefUser * matCamUser.inverse;
dstCamPos = dstCamMatrix.GetColumn(3);
dstCamRot = dstCamMatrix.rotation;
if (j == 0)
{
camRotFirst = dstCamRot;
}
camPosSum += dstCamPos;
KinectInterop.SumUpQuaternions(ref camRotSum, dstCamRot, camRotFirst);
numTrackedJoints++;
//Debug.Log(string.Format("S{0} J{1} - dst cam p:{2}, r:{3}", sIndex, j, dstCamPos, dstCamRot.eulerAngles));
}
if (numTrackedJoints > 1)
{
estCamPos = numTrackedJoints > 0 ? camPosSum / (float)numTrackedJoints : Vector3.zero;
estCamRot = numTrackedJoints > 0 ? KinectInterop.AverageQuaternions(camRotSum, numTrackedJoints).normalized : Quaternion.identity;
}
else
{
estCamPos = dstCamPos;
estCamRot = dstCamRot;
}
//Debug.Log(string.Format("S{0} - est cam p:{1}, r:{2}", sIndex, estCamPos, estCamRot.eulerAngles));
}
// saves the currently estimated sensor pose
private void SaveSensorPose(int sIndex, Vector3 sPos, Quaternion sRot)
{
while (savedCamPos[sIndex].Count >= MAX_SAVED_POSES)
{
savedCamPos[sIndex].RemoveAt(0);
}
savedCamPos[sIndex].Add(sPos);
while (savedCamRot[sIndex].Count >= MAX_SAVED_POSES)
{
savedCamRot[sIndex].RemoveAt(0);
}
savedCamRot[sIndex].Add(sRot);
// estimate the average camera pose
//multiCamPose.camPose[sIndex].position = sPos;
//multiCamPose.camPose[sIndex].rotation = sRot.eulerAngles;
if (savedCamPos[sIndex].Count == savedCamRot[sIndex].Count)
{
Vector3 camPosSum = Vector3.zero;
Vector4 camRotSum = Vector4.zero;
int numSavedPR = savedCamPos[sIndex].Count;
for (int i = 0; i < numSavedPR; i++)
{
camPosSum += savedCamPos[sIndex][i];
KinectInterop.SumUpQuaternions(ref camRotSum, savedCamRot[sIndex][i], savedCamRot[sIndex][0]);
}
multiCamPose.camPose[sIndex].position = camPosSum / (float)numSavedPR;
multiCamPose.camPose[sIndex].rotation = KinectInterop.AverageQuaternions(camRotSum, numSavedPR).eulerAngles;
if (cameraUserMesh[sIndex] != null)
{
cameraUserMesh[sIndex].transform.position = multiCamPose.camPose[sIndex].position;
cameraUserMesh[sIndex].transform.rotation = Quaternion.Euler(multiCamPose.camPose[sIndex].rotation);
}
}
}
