// saves the currently estimated sensor pose private void SaveSensorPose(int sIndex, Vector3 camPos, Quaternion camRot, ref UserBodyPose userPose) { while (savedCamPos[sIndex].Count >= maxSavedSamples) { savedCamPos[sIndex].RemoveAt(0); } savedCamPos[sIndex].Add(camPos); while (savedCamRot[sIndex].Count >= maxSavedSamples) { savedCamRot[sIndex].RemoveAt(0); } savedCamRot[sIndex].Add(camRot); while (savedUserPose[sIndex].Count >= maxSavedSamples) { savedUserPose[sIndex].RemoveAt(0); } UserBodyPose userPoseCopy = userPose.GetDeepCopy(); savedUserPose[sIndex].Add(userPoseCopy); //int spi = savedCamPos[sIndex].Count - 1; //Debug.Log(string.Format("S{0}: saved cam p:{1}, r:{2}, i: {3}", sIndex, savedCamPos[sIndex][spi], savedCamRot[sIndex][spi].eulerAngles, spi)); if (savedCamPos[sIndex].Count == savedCamRot[sIndex].Count) { int numSavedPR = savedCamPos[sIndex].Count; bool bEnoughSamples = numSavedPR >= numMvaSamples; int fromSavedPR = bEnoughSamples ? (numSavedPR - numMvaSamples) : 0; int mvaCountPR = bEnoughSamples ? numMvaSamples : numSavedPR; int cpi = GetBestCamPoseIndex(sIndex, fromSavedPR, numSavedPR, ref minPoseError[sIndex]); if (cpi >= 0) { minPoseIndex[sIndex] = cpi; //Debug.Log(string.Format("S{0}: best cam p:{1}, r:{2}, i: {3}, err: {4}, from: {5}, to: {6}", sIndex, savedCamPos[sIndex][cpi], savedCamRot[sIndex][cpi].eulerAngles, cpi, minPoseError[sIndex], fromSavedPR, numSavedPR)); multiCamPose.camPose[sIndex].position = savedCamPos[sIndex][cpi]; multiCamPose.camPose[sIndex].rotation = savedCamRot[sIndex][cpi].eulerAngles; if (cameraUserMesh != null && cameraUserMesh[sIndex] != null) { cameraUserMesh[sIndex].transform.position = savedCamPos[sIndex][cpi]; cameraUserMesh[sIndex].transform.rotation = savedCamRot[sIndex][cpi]; } } } }
public UserBodyPose GetDeepCopy() { int jointLen = jointPos.Length; UserBodyPose copyTo = new UserBodyPose(jointLen); copyTo.bodyTime = bodyTime; copyTo.poseTime = poseTime; for (int j = 0; j < jointLen; j++) { copyTo.jointTracked[j] = jointTracked[j]; copyTo.jointPos[j] = jointPos[j]; copyTo.jointRot[j] = jointRot[j]; } return(copyTo); }
// gets the position and rotation of the given user private bool GetUserPose(int sIndex, KinectInterop.SensorData sensorData, int uIndex, ulong lastBodyTime, ref UserBodyPose userPose) { if (sensorData != null && sensorData.trackedBodiesCount > uIndex && (lastBodyTime == 0 || sensorData.lastBodyFrameTime != lastBodyTime)) { KinectInterop.BodyData bodyData = sensorData.alTrackedBodies[uIndex]; Vector3 spaceScale = sensorData.sensorSpaceScale; userPose.bodyTime = sensorData.lastBodyFrameTime; if (lastBodyTime != 0) { //Debug.Log(string.Format("S{0}, Time: {1}, Last: {2}", sIndex, sensorData.lastBodyFrameTime, lastBodyTime)); } if (bodyData.joint != null && bodyData.joint.Length > 0) { for (int j = 0; j < numJoints; j++) { int joint = trackedBodyJoint[j]; KinectInterop.TrackingState trackingState = bodyData.joint[joint].trackingState; userPose.jointTracked[j] = (trackingState == KinectInterop.TrackingState.Tracked) || (trackingState == KinectInterop.TrackingState.HighConf); if (userPose.jointTracked[j]) { Vector3 kinectPos = bodyData.joint[joint].kinectPos; userPose.jointPos[j] = new Vector3(kinectPos.x * spaceScale.x, kinectPos.y * spaceScale.y, kinectPos.z); userPose.jointRot[j] = initialUserRot * bodyData.joint[joint].mirroredRotation; //Debug.Log(string.Format("S{0}: {1} - p:{2}, r:{3}", sIndex, (KinectInterop.JointType)joint, userPose.jointPos[j], userPose.jointRot[j].eulerAngles)); } } } return(userPose.jointTracked[0]); } return(false); }
void Start() { kinectManager = KinectManager.Instance; numSensors = kinectManager ? kinectManager.GetSensorCount() : 0; numJoints = trackedBodyJoint.Length; if (numSensors > 1) { ShowDebugMessage(numSensors + " sensors found. Starting multi-camera setup..."); } else { ShowErrorMessage(numSensors + " sensor(s) found. No need for multi-camera setup."); return; } // user meshes & body transforms per camera if (userMeshPrefab != null) { cameraUserMesh = new GameObject[numSensors]; } if (bodyTransformPrefab != null) { bodyTransforms = new Transform[numSensors]; } // maximum saved samples maxSavedSamples = numberOfSamples; numMvaSamples = Mathf.Min(Mathf.Max(Mathf.RoundToInt(maxSavedSamples * optimizeForPercentage), 1), maxSavedSamples); //Debug.Log("OptForPerc: " + numMvaSamples); minPoseError = new float[numSensors]; minPoseIndex = new int[numSensors]; // init user pos & rot userBodyPose = new UserBodyPose[numSensors]; // init camera pos & rot savedCamPos = new List <Vector3> [numSensors]; savedCamRot = new List <Quaternion> [numSensors]; savedUserPose = new List <UserBodyPose> [numSensors]; // init multi-camera pose multiCamPose.version = 1; multiCamPose.camPose = new KinectInterop.CameraPose[numSensors]; multiCamPose.settings = new string[numSensors]; for (int i = 0; i < numSensors; i++) { KinectInterop.SensorData sensorData = kinectManager.GetSensorData(i); if (sensorData != null && sensorData.sensorInterface != null) { multiCamPose.camPose[i] = new KinectInterop.CameraPose(); multiCamPose.camPose[i].sensorType = (int)sensorData.sensorInterface.GetSensorPlatform(); multiCamPose.camPose[i].sensorIndex = ((DepthSensorBase)sensorData.sensorInterface).deviceIndex; multiCamPose.camPose[i].sensorId = sensorData.sensorInterface.GetSensorDeviceId(); DepthSensorBase.BaseSensorSettings settings = sensorData.sensorInterface.GetSensorSettings(null); multiCamPose.settings[i] = JsonUtility.ToJson(settings); // set master-sub frame sync sensorData.sensorInterface.EnableSensorSync(sensorData, useSynchronizedSamples); if (sensorData.sensorInterface.IsSensorMaster()) { masterIndex = i; } if (i == 0) { // first camera Transform sensorTrans = kinectManager.GetSensorTransform(0); refCameraPos = sensorTrans.position; refCameraRot = sensorTrans.rotation; // src camera matrix srcCameraMatrix = Matrix4x4.TRS(refCameraPos, refCameraRot, Vector3.one); } // reset poses of all cameras sensorData.sensorInterface.SetSensorToWorldMatrix(Vector3.zero, Quaternion.identity, true); } userBodyPose[i] = new UserBodyPose(numJoints); savedCamPos[i] = new List <Vector3>(); savedCamRot[i] = new List <Quaternion>(); savedUserPose[i] = new List <UserBodyPose>(); if (bodyTransformPrefab != null) { GameObject userBodyTransform = Instantiate(bodyTransformPrefab, Vector3.zero, Quaternion.identity); userBodyTransform.name = "BodyTransform" + i; Renderer renderer = userBodyTransform.GetComponent <Renderer>(); if (renderer != null) { renderer.material.color = bodyTransformColor[i % bodyTransformColor.Length]; } bodyTransforms[i] = userBodyTransform.transform; } if (userMeshPrefab != null) { cameraUserMesh[i] = Instantiate(userMeshPrefab, transform); cameraUserMesh[i].name = ((KinectInterop.DepthSensorPlatform)multiCamPose.camPose[i].sensorType).ToString() + multiCamPose.camPose[i].sensorIndex + "Mesh"; UserMeshRendererGpu userMeshRenderer = cameraUserMesh[i].GetComponent <UserMeshRendererGpu>(); if (userMeshRenderer != null) { userMeshRenderer.sensorIndex = i; } else { SceneMeshRendererGpu sceneMeshRenderer = cameraUserMesh[i].GetComponent <SceneMeshRendererGpu>(); if (sceneMeshRenderer != null) { sceneMeshRenderer.sensorIndex = i; } } if (bodyTransforms != null && bodyTransforms[i] != null) { bodyTransforms[i].parent = cameraUserMesh[i].transform; } } //minPoseError[i] = float.MaxValue; } //userWorldPos = new Vector3[numSensors]; //userWorldRot = new Quaternion[numSensors]; // initial user rotation initialUserRot = Quaternion.Euler(0f, 180f, 0f); // always mirrored if (updatePanel) { // hide the update panel updatePanel.gameObject.SetActive(false); sensorRotTransform = new Vector3[numSensors]; sensorRotAdjust = new Vector3[numSensors]; sensorPosTransform = new Vector3[numSensors]; sensorPosAdjust = new Vector3[numSensors]; if (sensorDropdown) { // create list of sensor indices (and exclude the 1st one) List <string> alSensorOptions = new List <string>(); for (int i = 1; i < numSensors; i++) { alSensorOptions.Add(i.ToString()); } sensorDropdown.ClearOptions(); sensorDropdown.AddOptions(alSensorOptions); } } bool bConfigLoaded = false; if (kinectManager.useMultiCamConfig) { // load the config file bConfigLoaded = LoadMultiCamConfig(); if (bConfigLoaded) { // show cam-pose adjustment panel ShowUpdatePanel(); } } if (!bConfigLoaded) { // start auto-calibration co-routine bStopCoroutine = false; StartCoroutine(AutoCalibrateCameras()); } }