Vector3 ComplementaryFilterEuler(ImuSample sample, Vector3 currentOrientation, float dt) { // Integrate the gyroscope data -> int(angularSpeed) = angle float[] accData = { sample.AccelerometerSample.X, sample.AccelerometerSample.Y, sample.AccelerometerSample.Z }; float[] gyrData = { sample.GyroSample.X, sample.GyroSample.Y, sample.GyroSample.Z }; currentOrientation.x += (dt * gyrData[1]) * (180f / (float)Math.PI); // Angle around the X-axis currentOrientation.y += (dt * gyrData[2]) * (180f / (float)Math.PI); // Angle around the Y-axis currentOrientation.z += (dt * gyrData[0]) * (180f / (float)Math.PI); // Angle around the Z-axis currentOrientation.x = ((currentOrientation.x % 360) + 360) % 360; currentOrientation.y = ((currentOrientation.y % 360) + 360) % 360; currentOrientation.z = ((currentOrientation.z % 360) + 360) % 360; // Compensate for drift with accelerometer data if in force in range float forceMagnitudeApprox = Math.Abs(accData[0] / 9.8f) + Math.Abs(accData[1] / 9.8f) + Math.Abs(accData[2] / 9.8f); if (forceMagnitudeApprox > accelerometerMinValid && forceMagnitudeApprox < accelerometerMaxValid) { float roll = Mathf.Atan2(accData[1], accData[2]) * (180f / Mathf.PI) - 180; roll = ((roll % 360) + 360) % 360; float pitch = Mathf.Atan2(accData[2], accData[0]) * (180f / Mathf.PI) - 270; pitch = ((pitch % 360) + 360) % 360; currentOrientation.x = Mathf.LerpAngle(currentOrientation.x, pitch, accelerometerWeighting); currentOrientation.z = Mathf.LerpAngle(currentOrientation.z, roll, accelerometerWeighting); } return currentOrientation; }
public void ProcessCameraFrame() { if (_IsCameraStarted) { Capture capture = _KinectSensor.GetCapture(); if (capture.Color != null) { _RawColorImage = capture.Color.Memory.ToArray(); _TransformedColorImage = _Transformation.ColorImageToDepthCamera(capture).Memory.ToArray(); } if (capture.Depth != null) { Image depthImage = capture.Depth; Image transformedDepthImage = _Transformation.DepthImageToColorCamera(capture); _RawDepthImage = depthImage.GetPixels <short>().ToArray(); _TransformedDepthImage = transformedDepthImage.GetPixels <short>().ToArray(); _PointCloud = _Transformation.DepthImageToPointCloud(depthImage) .GetPixels <Short3>().ToArray(); } _ImuSample = _KinectSensor.GetImuSample(); capture.Dispose(); } }
private void DisplayInformation(Frame frame, ImuSample imu) { var heightPixels = frame.BodyIndexMap.HeightPixels; var widthPixels = frame.BodyIndexMap.WidthPixels; var totalPixels = heightPixels * widthPixels; TotalPixels.Text = totalPixels.ToString(); string frameTimeStamp = frame.DeviceTimestamp.ToString(); string imuTimeStamp = imu.AccelerometerTimestamp.ToString(); IMUTimestamp.Text = imuTimeStamp; FrameTimestamp.Text = frameTimeStamp; //GetImu(); var position = frame.GetBodySkeleton(0).GetJoint(JointId.SpineNavel).Position; var quaternion = frame.GetBodySkeleton(0).GetJoint(JointId.SpineNavel).Quaternion; NumberOfBodies.Text = frame.NumberOfBodies.ToString(); positionData.Text = "Position:\n X: " + position.X + "\n Y: " + (-1 * position.Y) + "\n Z: " + position.Z; quaternionData.Text = "Quaternion:\n X: " + quaternion.X + "\n Y: " + quaternion.Y + "\n Z: " + quaternion.Z + "\n W: " + quaternion.W; GetData.ColorImage(frame.Capture, in transformation, in colorBitmap); GetData.DepthImage(frame.Capture, in depthBitmap); pictureBox1.Image = depthBitmap; pictureBox2.Image = colorBitmap; }
public void Apply(ImuSample value) { var accValue = value.accSample; var gyroValue = value.gyroSample; this.imuDataText.text = $"IMU: TMP={value.temperature:0.000} " + $"ACC=({accValue.x:0.000},{accValue.y:0.000},{accValue.z:0.000}) " + $"GYRO=({gyroValue.x:0.00},{gyroValue.y:0.00},{gyroValue.z:0.00})"; var direction = new Vector3(-accValue.x, accValue.y, -accValue.z) - Gravity; var accRotation = Quaternion.FromToRotation(-this.transform.right, direction); gyroValue = value.integralGyro * Mathf.Rad2Deg; var gyroRotation = Quaternion.Euler(gyroValue.x, -gyroValue.y, -gyroValue.z); this.deviceModel.transform.localRotation = Quaternion.identity; switch (this.target) { case Targets.Acceleration: this.deviceModel.transform.Rotate(accRotation.eulerAngles, Space.Self); break; case Targets.Gyro: this.deviceModel.transform.Rotate(gyroRotation.eulerAngles, Space.Self); break; } }
public bool TryDetectFloorPlane(out Plane?plane, Short3[] pointCloudBuffer, int bufferWidth, int bufferHeight, int downsampleStep, ImuSample imuSample, Calibration sensorCalibration, int minimumFloorPointCount) { if (_task == null || _task.IsCompleted) { _cts = new CancellationTokenSource(); _task = Task.Run(async() => { List <Vector3> cloudPoints = GenerateCloudPoints(pointCloudBuffer, bufferWidth, bufferHeight, downsampleStep); return(DetectFloorPlane(cloudPoints, imuSample, sensorCalibration, minimumFloorPointCount)); } , _cts.Token) .ContinueWith(task => { if (task.Result.HasValue) { _resultQueue.Enqueue(task.Result.Value); } _cts.Dispose(); _cts = null; }); } if (_resultQueue.TryDequeue(out Plane detectedPlane)) { plane = detectedPlane; return(true); } plane = null; return(false); }
// determine camera orientation from IMU static SensorOrientation ImuOrientation(ImuSample imu) { const double halfGravity = 9.8 / 2; return ((imu.AccelerometerSample.Z > halfGravity) ? SensorOrientation.Flip180 : (imu.AccelerometerSample.Y > halfGravity) ? SensorOrientation.Clockwise90 : (imu.AccelerometerSample.Y < -halfGravity) ? SensorOrientation.CounterClockwise90 : SensorOrientation.Default); // upright }
// processes the IMU frame private void ProcessImuFrame(ImuSample imuSample) { if (kinectPlayback != null) { //WaitForPlaybackTimestamp("acc", imuSample.AccelerometerTimestampInUsec); //WaitForPlaybackTimestamp("gyro", imuSample.GyroTimestampInUsec); } lastImuSample = curImuSample; curImuSample = imuSample; }
private async Task IMULoop(Device device) { lastFrame = DateTime.Now; currentAngles = this.transform.rotation.eulerAngles; while (running) { if (collectIMUData) { ImuSample imuSample = new ImuSample(); await Task.Run(() => imuSample = device.GetImuSample()).ConfigureAwait(true); dt = (DateTime.Now - lastFrame).Milliseconds; lastFrame = DateTime.Now; currentAngles = ComplementaryFilterEuler(imuSample, currentAngles, dt / 1000); this.transform.rotation = Quaternion.Euler(currentAngles); } else { await Task.Run(() => { }); } } }
private async Task IMULoop(Device device) { currentAngles = this.transform.rotation.eulerAngles; while (true) { if (collectIMUData) { ImuSample imuSample = new ImuSample(); await Task.Run(() => imuSample = device.GetImuSample()).ConfigureAwait(true); if (lastFrame == null) { lastFrame = imuSample.AccelerometerTimestamp; } currentAngles = ComplementaryFilterEuler(imuSample, currentAngles, imuSample.AccelerometerTimestamp.Subtract(lastFrame).Milliseconds / 1000); lastFrame = imuSample.AccelerometerTimestamp; processor.currentIMUAngles = currentAngles; } else { await Task.Run(() => { }); } } }
public override void PollSensorFrames(KinectInterop.SensorData sensorData) { try { if (kinectPlayback != null) { if (kinectPlayback.IsEndOfStream()) { return; } long currentPlayTime = DateTime.Now.Ticks - playbackStartTime; if ((frameSourceFlags & (KinectInterop.FrameSource) 0x7F) != 0) { kinectPlayback.SeekTimestamp((ulong)(currentPlayTime / 10)); Capture capture = kinectPlayback.GetNextCapture(); if (capture != null) { ProcessCameraFrame(sensorData, capture); capture.Dispose(); currentFrameNumber++; } else { Debug.Log("End of recording detected."); } } if ((frameSourceFlags & KinectInterop.FrameSource.TypePose) != 0) { ImuSample imuSample = kinectPlayback.GetNextImuSample(); while (imuSample != null) { ProcessImuFrame(imuSample); ulong imuTimestamp = (ulong)imuSample.AccelerometerTimestamp.Ticks; if (kinectPlayback.IsEndOfStream() || imuTimestamp >= rawDepthTimestamp) { break; } imuSample = kinectPlayback.GetNextImuSample(); } } } else { if (isCamerasStarted) { Capture capture = kinectSensor.GetCapture(timeToWait); ProcessCameraFrame(sensorData, capture); capture.Dispose(); currentFrameNumber++; } if (isImuStarted) { ImuSample imuSample = kinectSensor.GetImuSample(timeToWait); while (imuSample != null) { ProcessImuFrame(imuSample); imuSample = kinectSensor.GetImuSample(timeToWait); } } } } catch (System.TimeoutException) { // do nothing } catch (System.Exception ex) { Debug.LogException(ex); } }
//// in playback mode - waits until the given time stamp //private void WaitForPlaybackTimestamp(string frameSource, long frameTimestamp) //{ // long currentPlayTime = DateTime.Now.Ticks - playbackStartTime; // if (currentPlayTime < frameTimestamp) // { // Debug.Log("Waiting for " + frameSource + " timestamp " + frameTimestamp + ". Current play time is: " + currentPlayTime); // } // while (currentPlayTime < frameTimestamp) // { // currentPlayTime = DateTime.Now.Ticks - playbackStartTime; // } //} // processes the IMU frame private void ProcessImuFrame(ImuSample imuSample) { if (kinectPlayback != null) { //WaitForPlaybackTimestamp("acc", imuSample.AccelerometerTimestampInUsec); //WaitForPlaybackTimestamp("gyro", imuSample.GyroTimestampInUsec); } lastImuSample = curImuSample; curImuSample = imuSample; if (!flipImuRotation && lastImuSample == null && Mathf.Abs(imuSample.AccelerometerSample.Y) >= 1f) { // check for rolled imu flipImuRotation = true; imuTurnRot2 = Quaternion.Euler(90f, -90f, 0f); } if (lastImuSample != null && imuAhrs != null && imuSample.AccelerometerTimestamp.Ticks != lastImuSample.AccelerometerTimestamp.Ticks) { prevRotZ = imuRotation.eulerAngles.z; Array.Copy(imuAhrs.Quaternion, prevQuat, prevQuat.Length); imuAhrs.SamplePeriod = ((imuSample.AccelerometerTimestamp.Ticks - lastImuSample.AccelerometerTimestamp.Ticks) / 10) * 0.000001f; imuAhrs.Update(imuSample.GyroSample.X, imuSample.GyroSample.Y, imuSample.GyroSample.Z, imuSample.AccelerometerSample.X, imuSample.AccelerometerSample.Y, imuSample.AccelerometerSample.Z); float[] ahrsQuat = imuAhrs.Quaternion; imuRotation = new Quaternion(ahrsQuat[1], ahrsQuat[2], ahrsQuat[3], ahrsQuat[0]); Quaternion transRotation = imuTurnRot2 * imuRotation * imuTurnRot1; if (imuAhrs.E2 < 0.0001f && Mathf.Abs(imuRotation.eulerAngles.z - prevRotZ) <= 0.1f) { // correct the continuous yaw, when imu doesn't move Array.Copy(prevQuat, imuAhrs.Quaternion, prevQuat.Length); ahrsQuat = prevQuat; imuRotation = new Quaternion(ahrsQuat[1], ahrsQuat[2], ahrsQuat[3], ahrsQuat[0]); transRotation = imuTurnRot2 * imuRotation * imuTurnRot1; if (!imuYawAtStartSet) { // initial yaw imuYawAtStartSet = true; imuYawAtStart = new Vector3(0f, transRotation.eulerAngles.y, 0f); imuFixAtStart = Quaternion.Inverse(Quaternion.Euler(imuYawAtStart)); } } if (imuYawAtStartSet) { // fix the initial yaw transRotation = imuFixAtStart * transRotation; } lock (poseFrameLock) { // rawPosePosition rawPoseRotation = transRotation; rawPoseTimestamp = (ulong)imuSample.AccelerometerTimestamp.Ticks; //poseFrameNumber = currentFrameNumber; } } //var onImuSample = OnImuSample; //if(onImuSample != null) //{ // onImuSample.Invoke(imuSample, rawDepthTimestamp, rawColorTimestamp); //} }
public override void PollSensorFrames(KinectInterop.SensorData sensorData) { try { if (kinectPlayback != null) { if (kinectPlayback.IsEndOfStream() && !loopPlayback && playbackPosSeconds < 0f) { return; } if (playbackStartTime == 0) { // start time playbackStartTime = DateTime.Now.Ticks; } long currentPlayTime = playbackPosSeconds < 0f ? DateTime.Now.Ticks - playbackStartTime : (long)(playbackPosSeconds * 10000000); if ((frameSourceFlags & (KinectInterop.FrameSource) 0x7F) != 0) { kinectPlayback.SeekTimestamp((ulong)(currentPlayTime / 10)); Capture capture = kinectPlayback.GetNextCapture(); //Debug.Log("Play time: " + (currentPlayTime / 10) + ", capture: " + capture); if (capture != null) { ProcessCameraFrames(sensorData, capture); capture.Dispose(); //currentFrameNumber++; } else { Debug.Log("End of recording reached: " + recordingFile); if (loopPlayback) { Debug.Log("Looping playback..."); playbackStartTime = DateTime.Now.Ticks; } } } //if ((frameSourceFlags & KinectInterop.FrameSource.TypePose) != 0) //{ // ImuSample imuSample = kinectPlayback.GetNextImuSample(); // while (imuSample != null) // { // ProcessImuFrame(imuSample); // ulong imuTimestamp = (ulong)imuSample.AccelerometerTimestamp.Ticks; // if (kinectPlayback.IsEndOfStream() || imuTimestamp >= rawDepthTimestamp) // break; // imuSample = kinectPlayback.GetNextImuSample(); // } //} } else { if (isCamerasStarted) { Capture capture = kinectSensor.GetCapture(timeToWait); ProcessCameraFrames(sensorData, capture); capture.Dispose(); //currentFrameNumber++; } if (isImuStarted) { ImuSample imuSample = kinectSensor.GetImuSample(timeToWait); while (imuSample != null) { ProcessImuFrame(imuSample); imuSample = kinectSensor.GetImuSample(timeToWait); } } } } catch (System.TimeoutException) { // do nothing } catch (System.Exception ex) { Debug.LogException(ex); } }
public override void PollSensorFrames(KinectInterop.SensorData sensorData) { try { if (kinectPlayback != null) { if (kinectPlayback.IsEndOfStream()) { return; } long currentPlayTime = DateTime.Now.Ticks - playbackStartTime; Capture capture = kinectPlayback.GetNextCapture(); if (capture != null) { ProcessCameraFrame(sensorData, capture); capture.Dispose(); currentFrameNumber++; } ImuSample imuSample = kinectPlayback.GetNextImuSample(); while (imuSample != null) { ProcessImuFrame(imuSample); ulong imuTimestamp = (ulong)(imuSample.GyroTimestamp.Ticks + imuSample.AccelerometerTimestamp.Ticks) >> 1; // avg if (kinectPlayback.IsEndOfStream() || imuTimestamp >= rawDepthTimestamp) { break; } imuSample = kinectPlayback.GetNextImuSample(); } } else { if (isCamerasStarted) { Capture capture = kinectSensor.GetCapture(timeToWait); ProcessCameraFrame(sensorData, capture); capture.Dispose(); currentFrameNumber++; } if (isImuStarted) { ImuSample imuSample = kinectSensor.GetImuSample(timeToWait); while (imuSample != null) { ProcessImuFrame(imuSample); imuSample = kinectSensor.GetImuSample(timeToWait); } } } } catch (System.TimeoutException) { // do nothing } catch (System.Exception ex) { Debug.LogException(ex); } }