public PoseFilterState PoseAtTime(double timestamp) { if (timestamp < oldestTime || timestamp > newestTime) { throw new ArgumentOutOfRangeException("Requested time is outside range of buffer"); } PoseFilterState q0; PoseFilterState q1; //Find the two pose states whose time stamps lie on either side of the desired time lock (this) { PoseFilterState tempState = new PoseFilterState(0, 0, 0, 0, 0, 0, timestamp); int index = stateBuffer.BinarySearch(tempState); if (index < 0) { index = ~index; q0 = stateBuffer[index - 1]; q1 = stateBuffer[index]; } else { return(stateBuffer[index]); } } return(QuaternionInterpolant(q0, q1, timestamp)); }
public void Add(PoseFilterState state) { lock (this) { //if the buffer fills up knock of the last one if (stateBuffer.Count >= bufferSize) { stateBuffer.RemoveAt(0); oldestTime = stateBuffer[0].timestamp; } if (stateBuffer.Count == 0) { stateBuffer.Add(state); oldestTime = stateBuffer[0].timestamp; newestTime = stateBuffer[0].timestamp; } else { int index = 0; for (int i = stateBuffer.Count - 1; i >= 0; i--) { if (state.timestamp > stateBuffer[i].timestamp) { index = i + 1; break; } } if (index == stateBuffer.Count) { newestTime = state.timestamp; } if (index == 0) { oldestTime = state.timestamp; } stateBuffer.Insert(index, state); } } }
private PoseFilterState QuaternionInterpolant(PoseFilterState q0, PoseFilterState q1, double timestamp) { double a1; double a2; double u = (timestamp - q0.timestamp) / (q1.timestamp - q0.timestamp); double q01 = q0.q1; double q02 = q0.q2; double q03 = q0.q3; double q04 = q0.q4; double q11 = q1.q1; double q12 = q1.q2; double q13 = q1.q3; double q14 = q1.q4; //double qSquareSum = q01 * q01 + q02 * q02 + q03 * q03 + q04 * q04; //double qp1 = (-1 * q01 * q14 - q02 * q13 + q03 * q12 + q04 * q11) / qSquareSum; //double qp2 = (q01 * q13 - q02 * q14 - q03 * q11 + q04 * q12) / qSquareSum; //double qp3 = (-1 * q01 * q12 + q02 * q11 - q03 * q14 + q04 * q13) / qSquareSum; //double qp4 = (q01 * q11 + q02 * q12 + q03 * q13 + q04 * q14) / qSquareSum; //double omega = Math.Acos(qp4); //qp1 = qp1 * Math.Sin(u * omega); //qp2 = qp2 * Math.Sin(u * omega); //qp3 = qp3 * Math.Sin(u * omega); //qp4 = Math.Cos(u * omega); //double qu1 = (-1 * q01 * qp4 - q02 * qp3 + q03 * qp2 + q04 * qp1); //double qu2 = (q01 * qp3 - q02 * qp4 - q03 * qp1 + q04 * qp2); //double qu3 = (-1 * q01 * qp2 + q02 * qp1 - q03 * qp4 + q04 * qp3); //double qu4 = (q01 * qp1 + q02 * qp2 + q03 * qp3 + q04 * qp4); double theta = Math.Acos(q01 * q11 + q02 * q12 + q03 * q13 + q04 * q14); //Correct for possible loss of numerical precision on q1,q2 if (Double.IsNaN(theta) || theta == 0) { theta = 0; a1 = 1 - u; a2 = u; } else { a1 = Math.Sin((1 - u) * theta) / Math.Sin(theta); a2 = Math.Sin(u * theta) / Math.Sin(theta); } double qu1 = a1 * q01 + a2 * q11; double qu2 = a1 * q02 + a2 * q12; double qu3 = a1 * q03 + a2 * q13; double qu4 = a1 * q04 + a2 * q14; if (Double.IsNaN(qu1) || Double.IsNaN(qu2) || Double.IsNaN(qu3) || Double.IsNaN(qu4)) { throw new ArithmeticException("Bad"); } if (Math.Abs(qu1) > 1 || Math.Abs(qu2) > 1 || Math.Abs(qu3) > 1 || Math.Abs(qu4) > 1) { throw new ArithmeticException("Bigger than 1"); } double xu = u * q1.x + (1 - u) * q0.x; double yu = u * q1.y + (1 - u) * q0.y; double zu = u * q1.z + (1 - u) * q0.z; Matrix P = q0.Covariance * (1 - u) + q1.Covariance * u; PoseFilterState poseToReturn = new PoseFilterState(xu, yu, zu, qu1, qu2, qu3, qu4, timestamp, P); poseToReturn.vx = u * q1.vx + (1 - u) * q0.vx; poseToReturn.vy = u * q1.vy + (1 - u) * q0.vy; poseToReturn.vz = u * q1.vz + (1 - u) * q0.vz; poseToReturn.wx = u * q1.wx + (1 - u) * q0.wx; poseToReturn.wy = u * q1.wy + (1 - u) * q0.wy; poseToReturn.wz = u * q1.wz + (1 - u) * q0.wz; return(poseToReturn); }