Velocity Tracker II: The Revenge of Velocity Tracker
Bug: 5265529 Rewrote the velocity tracker to fit a polynomial curve to pointer movements using least squares linear regression. The velocity is simply the first derivative of this polynomial. Clients can also obtain an Estimator that describes the complete terms of the estimating polynomial including the coefficient of determination which provides a measure of the quality of the fit (confidence). Enhanced PointerLocation to display the movement curve predicted by the estimator in addition to the velocity vector. By default, the algorithm computes a 2nd degree (quadratic) polynomial based on a 100ms recent history horizon. Change-Id: Id377bef44117fce68fee2c41f90134ce3224d3a1
This commit is contained in:
Jeff Brown
parent
0984130e74
commit
73aaf0d8d2
|
|
@ -620,10 +620,41 @@ private:
|
|||
*/
|
||||
class VelocityTracker {
|
||||
public:
|
||||
// Default polynomial degree. (used by getVelocity)
|
||||
static const uint32_t DEFAULT_DEGREE = 2;
|
||||
|
||||
// Default sample horizon. (used by getVelocity)
|
||||
// We don't use too much history by default since we want to react to quick
|
||||
// changes in direction.
|
||||
static const nsecs_t DEFAULT_HORIZON = 100 * 1000000; // 100 ms
|
||||
|
||||
struct Position {
|
||||
float x, y;
|
||||
};
|
||||
|
||||
struct Estimator {
|
||||
static const size_t MAX_DEGREE = 2;
|
||||
|
||||
// Polynomial coefficients describing motion in X and Y.
|
||||
float xCoeff[MAX_DEGREE + 1], yCoeff[MAX_DEGREE + 1];
|
||||
|
||||
// Polynomial degree (number of coefficients), or zero if no information is
|
||||
// available.
|
||||
uint32_t degree;
|
||||
|
||||
// Confidence (coefficient of determination), between 0 (no fit) and 1 (perfect fit).
|
||||
float confidence;
|
||||
|
||||
inline void clear() {
|
||||
degree = 0;
|
||||
confidence = 0;
|
||||
for (size_t i = 0; i <= MAX_DEGREE; i++) {
|
||||
xCoeff[i] = 0;
|
||||
yCoeff[i] = 0;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
VelocityTracker();
|
||||
|
||||
// Resets the velocity tracker state.
|
||||
|
|
@ -645,10 +676,16 @@ public:
|
|||
void addMovement(const MotionEvent* event);
|
||||
|
||||
// Gets the velocity of the specified pointer id in position units per second.
|
||||
// Returns false and sets the velocity components to zero if there is no movement
|
||||
// information for the pointer.
|
||||
// Returns false and sets the velocity components to zero if there is
|
||||
// insufficient movement information for the pointer.
|
||||
bool getVelocity(uint32_t id, float* outVx, float* outVy) const;
|
||||
|
||||
// Gets a quadratic estimator for the movements of the specified pointer id.
|
||||
// Returns false and clears the estimator if there is no information available
|
||||
// about the pointer.
|
||||
bool getEstimator(uint32_t id, uint32_t degree, nsecs_t horizon,
|
||||
Estimator* outEstimator) const;
|
||||
|
||||
// Gets the active pointer id, or -1 if none.
|
||||
inline int32_t getActivePointerId() const { return mActivePointerId; }
|
||||
|
||||
|
|
@ -657,13 +694,7 @@ public:
|
|||
|
||||
private:
|
||||
// Number of samples to keep.
|
||||
static const uint32_t HISTORY_SIZE = 10;
|
||||
|
||||
// Oldest sample to consider when calculating the velocity.
|
||||
static const nsecs_t MAX_AGE = 100 * 1000000; // 100 ms
|
||||
|
||||
// The minimum duration between samples when estimating velocity.
|
||||
static const nsecs_t MIN_DURATION = 5 * 1000000; // 5 ms
|
||||
static const uint32_t HISTORY_SIZE = 20;
|
||||
|
||||
struct Movement {
|
||||
nsecs_t eventTime;
|
||||
|
|
|
|||
|
|
@ -13,6 +13,9 @@
|
|||
// Log debug messages about velocity tracking.
|
||||
#define DEBUG_VELOCITY 0
|
||||
|
||||
// Log debug messages about least squares fitting.
|
||||
#define DEBUG_LEAST_SQUARES 0
|
||||
|
||||
// Log debug messages about acceleration.
|
||||
#define DEBUG_ACCELERATION 0
|
||||
|
||||
|
|
@ -682,9 +685,61 @@ bool MotionEvent::isTouchEvent(int32_t source, int32_t action) {
|
|||
|
||||
// --- VelocityTracker ---
|
||||
|
||||
const uint32_t VelocityTracker::DEFAULT_DEGREE;
|
||||
const nsecs_t VelocityTracker::DEFAULT_HORIZON;
|
||||
const uint32_t VelocityTracker::HISTORY_SIZE;
|
||||
const nsecs_t VelocityTracker::MAX_AGE;
|
||||
const nsecs_t VelocityTracker::MIN_DURATION;
|
||||
|
||||
static inline float vectorDot(const float* a, const float* b, uint32_t m) {
|
||||
float r = 0;
|
||||
while (m--) {
|
||||
r += *(a++) * *(b++);
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
static inline float vectorNorm(const float* a, uint32_t m) {
|
||||
float r = 0;
|
||||
while (m--) {
|
||||
float t = *(a++);
|
||||
r += t * t;
|
||||
}
|
||||
return sqrtf(r);
|
||||
}
|
||||
|
||||
#if DEBUG_LEAST_SQUARES || DEBUG_VELOCITY
|
||||
static String8 vectorToString(const float* a, uint32_t m) {
|
||||
String8 str;
|
||||
str.append("[");
|
||||
while (m--) {
|
||||
str.appendFormat(" %f", *(a++));
|
||||
if (m) {
|
||||
str.append(",");
|
||||
}
|
||||
}
|
||||
str.append(" ]");
|
||||
return str;
|
||||
}
|
||||
|
||||
static String8 matrixToString(const float* a, uint32_t m, uint32_t n, bool rowMajor) {
|
||||
String8 str;
|
||||
str.append("[");
|
||||
for (size_t i = 0; i < m; i++) {
|
||||
if (i) {
|
||||
str.append(",");
|
||||
}
|
||||
str.append(" [");
|
||||
for (size_t j = 0; j < n; j++) {
|
||||
if (j) {
|
||||
str.append(",");
|
||||
}
|
||||
str.appendFormat(" %f", a[rowMajor ? i * n + j : j * m + i]);
|
||||
}
|
||||
str.append(" ]");
|
||||
}
|
||||
str.append(" ]");
|
||||
return str;
|
||||
}
|
||||
#endif
|
||||
|
||||
VelocityTracker::VelocityTracker() {
|
||||
clear();
|
||||
|
|
@ -733,16 +788,15 @@ void VelocityTracker::addMovement(nsecs_t eventTime, BitSet32 idBits, const Posi
|
|||
uint32_t id = iterBits.firstMarkedBit();
|
||||
uint32_t index = idBits.getIndexOfBit(id);
|
||||
iterBits.clearBit(id);
|
||||
float vx, vy;
|
||||
bool available = getVelocity(id, &vx, &vy);
|
||||
if (available) {
|
||||
LOGD(" %d: position (%0.3f, %0.3f), vx=%0.3f, vy=%0.3f, speed=%0.3f",
|
||||
id, positions[index].x, positions[index].y, vx, vy, sqrtf(vx * vx + vy * vy));
|
||||
} else {
|
||||
LOG_ASSERT(vx == 0 && vy == 0);
|
||||
LOGD(" %d: position (%0.3f, %0.3f), velocity not available",
|
||||
id, positions[index].x, positions[index].y);
|
||||
}
|
||||
Estimator estimator;
|
||||
getEstimator(id, DEFAULT_DEGREE, DEFAULT_HORIZON, &estimator);
|
||||
LOGD(" %d: position (%0.3f, %0.3f), "
|
||||
"estimator (degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f)",
|
||||
id, positions[index].x, positions[index].y,
|
||||
int(estimator.degree),
|
||||
vectorToString(estimator.xCoeff, estimator.degree).string(),
|
||||
vectorToString(estimator.yCoeff, estimator.degree).string(),
|
||||
estimator.confidence);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
|
@ -811,47 +865,228 @@ void VelocityTracker::addMovement(const MotionEvent* event) {
|
|||
addMovement(eventTime, idBits, positions);
|
||||
}
|
||||
|
||||
/**
|
||||
* Solves a linear least squares problem to obtain a N degree polynomial that fits
|
||||
* the specified input data as nearly as possible.
|
||||
*
|
||||
* Returns true if a solution is found, false otherwise.
|
||||
*
|
||||
* The input consists of two vectors of data points X and Y with indices 0..m-1.
|
||||
* The output is a vector B with indices 0..n-1 that describes a polynomial
|
||||
* that fits the data, such the sum of abs(Y[i] - (B[0] + B[1] X[i] + B[2] X[i]^2 ... B[n] X[i]^n))
|
||||
* for all i between 0 and m-1 is minimized.
|
||||
*
|
||||
* That is to say, the function that generated the input data can be approximated
|
||||
* by y(x) ~= B[0] + B[1] x + B[2] x^2 + ... + B[n] x^n.
|
||||
*
|
||||
* The coefficient of determination (R^2) is also returned to describe the goodness
|
||||
* of fit of the model for the given data. It is a value between 0 and 1, where 1
|
||||
* indicates perfect correspondence.
|
||||
*
|
||||
* This function first expands the X vector to a m by n matrix A such that
|
||||
* A[i][0] = 1, A[i][1] = X[i], A[i][2] = X[i]^2, ..., A[i][n] = X[i]^n.
|
||||
*
|
||||
* Then it calculates the QR decomposition of A yielding an m by m orthonormal matrix Q
|
||||
* and an m by n upper triangular matrix R. Because R is upper triangular (lower
|
||||
* part is all zeroes), we can simplify the decomposition into an m by n matrix
|
||||
* Q1 and a n by n matrix R1 such that A = Q1 R1.
|
||||
*
|
||||
* Finally we solve the system of linear equations given by R1 B = (Qtranspose Y)
|
||||
* to find B.
|
||||
*
|
||||
* For efficiency, we lay out A and Q column-wise in memory because we frequently
|
||||
* operate on the column vectors. Conversely, we lay out R row-wise.
|
||||
*
|
||||
* http://en.wikipedia.org/wiki/Numerical_methods_for_linear_least_squares
|
||||
* http://en.wikipedia.org/wiki/Gram-Schmidt
|
||||
*/
|
||||
static bool solveLeastSquares(const float* x, const float* y, uint32_t m, uint32_t n,
|
||||
float* outB, float* outDet) {
|
||||
#if DEBUG_LEAST_SQUARES
|
||||
LOGD("solveLeastSquares: m=%d, n=%d, x=%s, y=%s", int(m), int(n),
|
||||
vectorToString(x, m).string(), vectorToString(y, m).string());
|
||||
#endif
|
||||
|
||||
// Expand the X vector to a matrix A.
|
||||
float a[n][m]; // column-major order
|
||||
for (uint32_t h = 0; h < m; h++) {
|
||||
a[0][h] = 1;
|
||||
for (uint32_t i = 1; i < n; i++) {
|
||||
a[i][h] = a[i - 1][h] * x[h];
|
||||
}
|
||||
}
|
||||
#if DEBUG_LEAST_SQUARES
|
||||
LOGD(" - a=%s", matrixToString(&a[0][0], m, n, false /*rowMajor*/).string());
|
||||
#endif
|
||||
|
||||
// Apply the Gram-Schmidt process to A to obtain its QR decomposition.
|
||||
float q[n][m]; // orthonormal basis, column-major order
|
||||
float r[n][n]; // upper triangular matrix, row-major order
|
||||
for (uint32_t j = 0; j < n; j++) {
|
||||
for (uint32_t h = 0; h < m; h++) {
|
||||
q[j][h] = a[j][h];
|
||||
}
|
||||
for (uint32_t i = 0; i < j; i++) {
|
||||
float dot = vectorDot(&q[j][0], &q[i][0], m);
|
||||
for (uint32_t h = 0; h < m; h++) {
|
||||
q[j][h] -= dot * q[i][h];
|
||||
}
|
||||
}
|
||||
|
||||
float norm = vectorNorm(&q[j][0], m);
|
||||
if (norm < 0.000001f) {
|
||||
// vectors are linearly dependent or zero so no solution
|
||||
#if DEBUG_LEAST_SQUARES
|
||||
LOGD(" - no solution, norm=%f", norm);
|
||||
#endif
|
||||
return false;
|
||||
}
|
||||
|
||||
float invNorm = 1.0f / norm;
|
||||
for (uint32_t h = 0; h < m; h++) {
|
||||
q[j][h] *= invNorm;
|
||||
}
|
||||
for (uint32_t i = 0; i < n; i++) {
|
||||
r[j][i] = i < j ? 0 : vectorDot(&q[j][0], &a[i][0], m);
|
||||
}
|
||||
}
|
||||
#if DEBUG_LEAST_SQUARES
|
||||
LOGD(" - q=%s", matrixToString(&q[0][0], m, n, false /*rowMajor*/).string());
|
||||
LOGD(" - r=%s", matrixToString(&r[0][0], n, n, true /*rowMajor*/).string());
|
||||
|
||||
// calculate QR, if we factored A correctly then QR should equal A
|
||||
float qr[n][m];
|
||||
for (uint32_t h = 0; h < m; h++) {
|
||||
for (uint32_t i = 0; i < n; i++) {
|
||||
qr[i][h] = 0;
|
||||
for (uint32_t j = 0; j < n; j++) {
|
||||
qr[i][h] += q[j][h] * r[j][i];
|
||||
}
|
||||
}
|
||||
}
|
||||
LOGD(" - qr=%s", matrixToString(&qr[0][0], m, n, false /*rowMajor*/).string());
|
||||
#endif
|
||||
|
||||
// Solve R B = Qt Y to find B. This is easy because R is upper triangular.
|
||||
// We just work from bottom-right to top-left calculating B's coefficients.
|
||||
for (uint32_t i = n; i-- != 0; ) {
|
||||
outB[i] = vectorDot(&q[i][0], y, m);
|
||||
for (uint32_t j = n - 1; j > i; j--) {
|
||||
outB[i] -= r[i][j] * outB[j];
|
||||
}
|
||||
outB[i] /= r[i][i];
|
||||
}
|
||||
#if DEBUG_LEAST_SQUARES
|
||||
LOGD(" - b=%s", vectorToString(outB, n).string());
|
||||
#endif
|
||||
|
||||
// Calculate the coefficient of determination as 1 - (SSerr / SStot) where
|
||||
// SSerr is the residual sum of squares (squared variance of the error),
|
||||
// and SStot is the total sum of squares (squared variance of the data).
|
||||
float ymean = 0;
|
||||
for (uint32_t h = 0; h < m; h++) {
|
||||
ymean += y[h];
|
||||
}
|
||||
ymean /= m;
|
||||
|
||||
float sserr = 0;
|
||||
float sstot = 0;
|
||||
for (uint32_t h = 0; h < m; h++) {
|
||||
float err = y[h] - outB[0];
|
||||
float term = 1;
|
||||
for (uint32_t i = 1; i < n; i++) {
|
||||
term *= x[h];
|
||||
err -= term * outB[i];
|
||||
}
|
||||
sserr += err * err;
|
||||
float var = y[h] - ymean;
|
||||
sstot += var * var;
|
||||
}
|
||||
*outDet = sstot > 0.000001f ? 1.0f - (sserr / sstot) : 1;
|
||||
#if DEBUG_LEAST_SQUARES
|
||||
LOGD(" - sserr=%f", sserr);
|
||||
LOGD(" - sstot=%f", sstot);
|
||||
LOGD(" - det=%f", *outDet);
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
bool VelocityTracker::getVelocity(uint32_t id, float* outVx, float* outVy) const {
|
||||
Estimator estimator;
|
||||
if (getEstimator(id, DEFAULT_DEGREE, DEFAULT_HORIZON, &estimator)) {
|
||||
if (estimator.degree >= 1) {
|
||||
*outVx = estimator.xCoeff[1];
|
||||
*outVy = estimator.yCoeff[1];
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool VelocityTracker::getEstimator(uint32_t id, uint32_t degree, nsecs_t horizon,
|
||||
Estimator* outEstimator) const {
|
||||
outEstimator->clear();
|
||||
|
||||
// Iterate over movement samples in reverse time order and collect samples.
|
||||
float x[HISTORY_SIZE];
|
||||
float y[HISTORY_SIZE];
|
||||
float time[HISTORY_SIZE];
|
||||
uint32_t m = 0;
|
||||
uint32_t index = mIndex;
|
||||
const Movement& newestMovement = mMovements[mIndex];
|
||||
if (newestMovement.idBits.hasBit(id)) {
|
||||
const Position& newestPosition = newestMovement.getPosition(id);
|
||||
float accumVx = 0;
|
||||
float accumVy = 0;
|
||||
float duration = 0;
|
||||
do {
|
||||
const Movement& movement = mMovements[index];
|
||||
if (!movement.idBits.hasBit(id)) {
|
||||
break;
|
||||
}
|
||||
|
||||
// Iterate over movement samples in reverse time order and accumulate velocity.
|
||||
uint32_t index = mIndex;
|
||||
do {
|
||||
index = (index == 0 ? HISTORY_SIZE : index) - 1;
|
||||
const Movement& movement = mMovements[index];
|
||||
if (!movement.idBits.hasBit(id)) {
|
||||
break;
|
||||
}
|
||||
nsecs_t age = newestMovement.eventTime - movement.eventTime;
|
||||
if (age > horizon) {
|
||||
break;
|
||||
}
|
||||
|
||||
nsecs_t age = newestMovement.eventTime - movement.eventTime;
|
||||
if (age > MAX_AGE) {
|
||||
break;
|
||||
}
|
||||
const Position& position = movement.getPosition(id);
|
||||
x[m] = position.x;
|
||||
y[m] = position.y;
|
||||
time[m] = -age * 0.000000001f;
|
||||
index = (index == 0 ? HISTORY_SIZE : index) - 1;
|
||||
} while (++m < HISTORY_SIZE);
|
||||
|
||||
const Position& position = movement.getPosition(id);
|
||||
accumVx += newestPosition.x - position.x;
|
||||
accumVy += newestPosition.y - position.y;
|
||||
duration += age;
|
||||
} while (index != mIndex);
|
||||
if (m == 0) {
|
||||
return false; // no data
|
||||
}
|
||||
|
||||
// Make sure we used at least one sample.
|
||||
if (duration >= MIN_DURATION) {
|
||||
float scale = 1000000000.0f / duration; // one over time delta in seconds
|
||||
*outVx = accumVx * scale;
|
||||
*outVy = accumVy * scale;
|
||||
// Calculate a least squares polynomial fit.
|
||||
if (degree > Estimator::MAX_DEGREE) {
|
||||
degree = Estimator::MAX_DEGREE;
|
||||
}
|
||||
if (degree > m - 1) {
|
||||
degree = m - 1;
|
||||
}
|
||||
if (degree >= 1) {
|
||||
float xdet, ydet;
|
||||
uint32_t n = degree + 1;
|
||||
if (solveLeastSquares(time, x, m, n, outEstimator->xCoeff, &xdet)
|
||||
&& solveLeastSquares(time, y, m, n, outEstimator->yCoeff, &ydet)) {
|
||||
outEstimator->degree = degree;
|
||||
outEstimator->confidence = xdet * ydet;
|
||||
#if DEBUG_LEAST_SQUARES
|
||||
LOGD("estimate: degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f",
|
||||
int(outEstimator->degree),
|
||||
vectorToString(outEstimator->xCoeff, n).string(),
|
||||
vectorToString(outEstimator->yCoeff, n).string(),
|
||||
outEstimator->confidence);
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// No data available for this pointer.
|
||||
*outVx = 0;
|
||||
*outVy = 0;
|
||||
return false;
|
||||
// No velocity data available for this pointer, but we do have its current position.
|
||||
outEstimator->xCoeff[0] = x[0];
|
||||
outEstimator->yCoeff[0] = y[0];
|
||||
outEstimator->degree = 0;
|
||||
outEstimator->confidence = 1;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue