// 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); } } }