/// <summary>
/// Adds the position measurement.
/// </summary>
/// <param name="timestamp">Timestamp.</param>
/// <param name="position">Position.</param>
public virtual void AddPositionMeasurement(double timestamp, Vector3 position)
{
TrackingDataPosition trackingDataPosition = new TrackingDataPosition(timestamp - positionLatency, position);
if (!isMainThread())
{
lock (dataQueue)
{
dataQueue.PushFront(trackingDataPosition);
dataInQueue = true;
}
}
else
{
AddPositionMeasurement(trackingDataPosition);
}
}
public void AddAccelerationMeasurement(TrackingDataIMU trackingDataIMU)
{
double lastAccTs = GetLastAccelerationTimestamp();
if (trackingDataIMU.timestamp - lastAccTs < 0.001)
{
//Debug.LogError("Acceleration Timestamp too close at " + trackingDataIMU.timestamp.ToString("F4") + " Previous: " + lastAccTs.ToString());
return;
}
int index = InsertByTimestamp(trackingDataIMU);
if (index + 1 >= trackingDataBuffer.Size)
{
// Debug.LogError("Index bigger than Buffer Size");
return;
}
//if (index != 0)
// Debug.LogError("Acceleration was not insered at last position " + trackingDataIMU.timestamp.ToString("0.000"));
if (trackingDataBuffer.Size < 2)
{
return;
}
double delaySinceLastUpdate = trackingDataIMU.timestamp - trackingDataBuffer[index + 1].timestamp;
if (delaySinceLastUpdate > maxDelaySinceLastMeasurement)
{
Debug.LogWarning("Too long delay since last update : " + delaySinceLastUpdate.ToString() + " " + trackingDataIMU.timestamp.ToString("0.000"));
if (trackingDataBuffer.Size > 0)
{
ResetFilter();
}
return;
}
double lastpositionts = GetLastPositionTimestamp();
// Debug.Log("Last pos ts " + lastpositionts.ToString("0.000"));
if (lastpositionts < 0 || trackingDataIMU.timestamp - lastpositionts > accelerationOnlyTrackingDelay)
{
// Debug.LogError("No previous position, or too long ago");
return;
}
// Calculate its speed and position using the previous info
// The previous data is an acceleration
if (trackingDataBuffer[index + 1].GetType() == typeof(TrackingDataIMU))
{
//TODO: handle offset correction ?
TrackingDataIMU previousAcceleration = ((TrackingDataIMU)trackingDataBuffer[index + 1]);
// Predict position and speed at this update
// Clamping to avoid predicting for too long
Vector3 newSpeed = (trackingDataIMU.timestamp - lastpositionts) < 0.11f ? previousAcceleration.speed + ((TrackingDataIMU)trackingDataBuffer[index]).acceleration * (float)delaySinceLastUpdate : Vector3.Slerp(previousAcceleration.speed + ((TrackingDataIMU)trackingDataBuffer[index]).acceleration * (float)delaySinceLastUpdate, Vector3.zero, (float)(trackingDataIMU.timestamp - lastpositionts) / accelerationOnlyTrackingDelay);
trackingDataBuffer[index].speed = newSpeed;
Vector3 newPositionOffset = (trackingDataIMU.timestamp - lastpositionts) < 0.11f ? previousAcceleration.speed * (float)delaySinceLastUpdate + 0.5f * ((TrackingDataIMU)trackingDataBuffer[index]).acceleration * (float)delaySinceLastUpdate * (float)delaySinceLastUpdate : Vector3.Slerp(previousAcceleration.speed * (float)delaySinceLastUpdate + 0.5f * ((TrackingDataIMU)trackingDataBuffer[index]).acceleration * (float)delaySinceLastUpdate * (float)delaySinceLastUpdate, Vector3.zero, (float)(trackingDataIMU.timestamp - lastpositionts) / accelerationOnlyTrackingDelay);
trackingDataBuffer[index].position = previousAcceleration.position + newPositionOffset;
// Debug.Log(" Offset from prev acceleration : " + (trackingDataBuffer[index].position - previousAcceleration.position).magnitude.ToString("0.000"));
}
// The previous data is a position
else if (trackingDataBuffer[index + 1].GetType() == typeof(TrackingDataPosition))
{
//TODO: handle offset correction
TrackingDataPosition previousPosition = ((TrackingDataPosition)trackingDataBuffer[index + 1]);
// Predict position and speed at this update
Vector3 newSpeed = delaySinceLastUpdate < 0.03f ? previousPosition.speed + ((TrackingDataIMU)trackingDataBuffer[index]).acceleration * (float)delaySinceLastUpdate : Vector3.Slerp(previousPosition.speed + ((TrackingDataIMU)trackingDataBuffer[index]).acceleration * (float)delaySinceLastUpdate, Vector3.zero, (float)(delaySinceLastUpdate) / accelerationOnlyTrackingDelay);
trackingDataBuffer[index].speed = newSpeed;
Vector3 newPositionOffset = delaySinceLastUpdate < 0.03f ? previousPosition.speed * (float)delaySinceLastUpdate + 0.5f * ((TrackingDataIMU)trackingDataBuffer[index]).acceleration * (float)delaySinceLastUpdate * (float)delaySinceLastUpdate : Vector3.Slerp(previousPosition.speed * (float)delaySinceLastUpdate + 0.5f * ((TrackingDataIMU)trackingDataBuffer[index]).acceleration * (float)delaySinceLastUpdate * (float)delaySinceLastUpdate, Vector3.zero, (float)(trackingDataIMU.timestamp - lastpositionts) / accelerationOnlyTrackingDelay);
trackingDataBuffer[index].position = previousPosition.position + newPositionOffset;
//Debug.Log(" Offset from prev position : " + (trackingDataBuffer[index].position-previousPosition.position).magnitude.ToString("0.000"));
}
lastCalculatedPosition = trackingDataBuffer[0].position;
lastCalculatedPositionTimestamp = trackingDataBuffer[0].timestamp;
// if (lastCalculatedPosition.magnitude > 5)
// Debug.LogError("TS " + lastCalculatedPositionTimestamp.ToString("0.000") + " MAG: " + lastCalculatedPosition.magnitude.ToString());
}
public void AddPositionMeasurement(TrackingDataPosition trackingDataPosition)
{
double lastPosTs = GetLastPositionTimestamp();
if (trackingDataPosition.timestamp - lastPosTs < 0.001f)
{
// Debug.LogError("Position Timestamp too close at " + trackingDataPosition.timestamp.ToString("F4") + " last timestamp: " + lastPosTs.ToString("F4"));
return;
}
int index = InsertByTimestamp(trackingDataPosition);
if (index + 1 >= trackingDataBuffer.Size)
{
Debug.LogError("Index bigger than Buffer Size");
return;
}
// Debug.Log("POS MEASUREMENT AT " + (timestamp-positionLatency).ToString("0.000") + " POS: " + position.ToString("0.000"));
// Check there is no position with a more recent timestamp, either a network error or impossible state
for (int i = index - 1; i >= 0; i--)
{
if (trackingDataBuffer[i].GetType() == typeof(TrackingDataPosition))
{
Debug.LogError("The position received is older than other position at " + trackingDataPosition.timestamp.ToString("F4"));
//TODO Remove from buffer ?
return;
}
}
if (trackingDataBuffer.Size < 2)
{
return;
}
// Check time since last measurement (of any type : position or acc)
double delaySinceLastUpdate = trackingDataPosition.timestamp - trackingDataBuffer[index + 1].timestamp;
// if (delaySinceLastUpdate > maxDelaySinceLastMeasurement)
// Debug.LogWarning("Too long delay since last update : " + delaySinceLastUpdate.ToString());
bool predictSpeedFromAcc = false; // Boolean to che kif we should calculate the speed from the last acceleration (in case of jump, no position etc)
// Try to find position jumps
TrackingDataPosition previousPosition = GetPreviousPositionData(index);
if (previousPosition != null)
{
float speedMagnitudeLastPositions = ((trackingDataPosition.position - previousPosition.position) / (float)(trackingDataPosition.timestamp - previousPosition.timestamp)).magnitude;
if (speedMagnitudeLastPositions > discardSpeed || (trackingDataPosition.position - previousPosition.position).magnitude > discardDistance)
{
((TrackingDataPosition)trackingDataBuffer[index]).jump = true;
predictSpeedFromAcc = true;
// Debug.LogWarning("Jump detected of speed " + speedMagnitudeLastPositions.ToString() + " at TS " + trackingDataPosition.timestamp.ToString());
}
}
// We couldn't find any previous position in the buffer so we add an offset to smooth from the last calculated position to here
else
{
offsets.Add(new PositionOffset(trackingDataPosition.timestamp, lastCalculatedPosition - trackingDataPosition.position, 0.4f));
offsets.Add(new PositionOffset(trackingDataPosition.timestamp, lastCalculatedPosition - trackingDataPosition.position, 0.4f));
// Debug.LogWarning("Jump detected of speed ");
}
if (!predictSpeedFromAcc)
{
List <TrackingDataPosition> lastPositions = GetPreviousPositionData(index, speedCalculationDelay);
// There is not last position
if (lastPositions.Count <= 1)
{
predictSpeedFromAcc = true;
// Debug.LogError("Could not find any previous position " + index.ToString() + " - " + trackingDataPosition.timestamp.ToString() + " count " + lastPositions.Count.ToString() + " TS: " + timestamp.ToString("0.000"));
}
else
{
// Check for jump in last positions
foreach (TrackingDataPosition pos in lastPositions)
{
if (pos.jump)
{
predictSpeedFromAcc = true;
break;
}
}
// Calculate speed from last positions
if (!predictSpeedFromAcc)
{
Vector3 speed = (lastPositions[0].position - lastPositions[lastPositions.Count - 1].position) / (float)(lastPositions[0].timestamp - lastPositions[lastPositions.Count - 1].timestamp);
double speedtimestamp = (lastPositions[0].timestamp + lastPositions[lastPositions.Count - 1].timestamp) / 2.0d;
// Debug.Log(" Calculating speed form pos : " + speed.magnitude.ToString("0.000"));
// Propagate with acceleration until this time
List <TrackingDataIMU> accelerations = GetPreviousIMUDataUntilTimestamp(index, speedtimestamp);
if (accelerations.Count > 0)
{
for (int i = accelerations.Count - 1; i >= 0; i--)
{
speed += accelerations[i].acceleration * (float)(accelerations[i].timestamp - speedtimestamp);
speedtimestamp = accelerations[i].timestamp;
}
// Could be improved here (like by checking if we have another acc jsut after current index)
speed += accelerations[0].acceleration * (float)(trackingDataBuffer[index].timestamp - accelerations[0].timestamp);
// Debug.Log(" Predicting speed from acc to ts : " + speed.magnitude.ToString("0.000"));
}
((TrackingDataPosition)trackingDataBuffer[index]).speed = speed;
}
}
}
// Calculate speed from previous acceleration
if (predictSpeedFromAcc)
{
// The previous data is an acceleration
if (trackingDataBuffer[index + 1].GetType() == typeof(TrackingDataIMU))
{
//TODO: handle offset correction ?
TrackingDataIMU previousAcceleration = ((TrackingDataIMU)trackingDataBuffer[index + 1]);
// Predict position and speed at this update
trackingDataBuffer[index].speed = previousAcceleration.speed + previousAcceleration.acceleration * (float)delaySinceLastUpdate;
// Debug.Log(" Predicting speed from acc : " + trackingDataBuffer[index].speed.magnitude.ToString("0.000"));
}
// The previous data is a position
else if (trackingDataBuffer[index + 1].GetType() == typeof(TrackingDataPosition))
{
//TODO: handle offset correction
// No prediction : use speed of last position
((TrackingDataPosition)trackingDataBuffer[index + 1]).speed = previousPosition.speed;
// Debug.Log(" Using previous speed : " + previousPosition.speed.magnitude.ToString("0.000"));
}
}
// Correct following positions and speeds
// Keep latest position before propagation to calculate the offset
Vector3 latestPosition = trackingDataBuffer[0].position;
for (int i = index - 1; i >= 0; i--)
{
// At this point we can only have IMU data up to the end of the vector
double deltaTime = trackingDataBuffer[i].timestamp - trackingDataBuffer[i + 1].timestamp;
trackingDataBuffer[i].speed = trackingDataBuffer[i + 1].speed + ((TrackingDataIMU)trackingDataBuffer[i]).acceleration * (float)deltaTime;
trackingDataBuffer[i].position = trackingDataBuffer[i + 1].position + trackingDataBuffer[i + 1].speed * (float)deltaTime + 0.5f * ((TrackingDataIMU)trackingDataBuffer[i]).acceleration * (float)deltaTime * (float)deltaTime;
}
Vector3 positionOffsetAfterPropagation = trackingDataBuffer[0].position - latestPosition;
if (positionOffsetAfterPropagation.magnitude > 0.04f && previousPosition != null)
{
offsets.Add(new PositionOffset(trackingDataPosition.timestamp, previousPosition.position - trackingDataPosition.position));
// Debug.LogWarning(" Offset : " + positionOffsetAfterPropagation.magnitude.ToString("0.000") + " at " + trackingDataBuffer[0].timestamp.ToString("0.000"));
}
lastCalculatedPosition = trackingDataBuffer[0].position;
lastCalculatedPositionTimestamp = trackingDataBuffer[0].timestamp;
// if (lastCalculatedPosition.magnitude > 5)
// Debug.LogError("TS " + lastCalculatedPositionTimestamp.ToString("0.000") + " MAG: " + lastCalculatedPosition.magnitude.ToString());
}