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