Merge "Velocity Tracker II: The Revenge of Velocity Tracker Bug: 5265529"
This commit is contained in:
@@ -59,6 +59,8 @@ public final class VelocityTracker implements Poolable<VelocityTracker> {
|
||||
private static native void nativeComputeCurrentVelocity(int ptr, int units, float maxVelocity);
|
||||
private static native float nativeGetXVelocity(int ptr, int id);
|
||||
private static native float nativeGetYVelocity(int ptr, int id);
|
||||
private static native boolean nativeGetEstimator(int ptr, int id,
|
||||
int degree, int horizonMillis, Estimator outEstimator);
|
||||
|
||||
/**
|
||||
* Retrieve a new VelocityTracker object to watch the velocity of a
|
||||
@@ -215,4 +217,94 @@ public final class VelocityTracker implements Poolable<VelocityTracker> {
|
||||
public float getYVelocity(int id) {
|
||||
return nativeGetYVelocity(mPtr, id);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get an estimator for the movements of a pointer using past movements of the
|
||||
* pointer to predict future movements.
|
||||
*
|
||||
* It is not necessary to call {@link #computeCurrentVelocity(int)} before calling
|
||||
* this method.
|
||||
*
|
||||
* @param id Which pointer's velocity to return.
|
||||
* @param degree The desired polynomial degree. The actual estimator may have
|
||||
* a lower degree than what is requested here. If -1, uses the default degree.
|
||||
* @param horizonMillis The maximum age of the oldest sample to consider, in milliseconds.
|
||||
* If -1, uses the default horizon.
|
||||
* @param outEstimator The estimator to populate.
|
||||
* @return True if an estimator was obtained, false if there is no information
|
||||
* available about the pointer.
|
||||
*
|
||||
* @hide For internal use only. Not a final API.
|
||||
*/
|
||||
public boolean getEstimator(int id, int degree, int horizonMillis, Estimator outEstimator) {
|
||||
if (outEstimator == null) {
|
||||
throw new IllegalArgumentException("outEstimator must not be null");
|
||||
}
|
||||
return nativeGetEstimator(mPtr, id, degree, horizonMillis, outEstimator);
|
||||
}
|
||||
|
||||
/**
|
||||
* An estimator for the movements of a pointer based on a polynomial model.
|
||||
*
|
||||
* The last recorded position of the pointer is at time zero seconds.
|
||||
* Past estimated positions are at negative times and future estimated positions
|
||||
* are at positive times.
|
||||
*
|
||||
* First coefficient is position (in pixels), second is velocity (in pixels per second),
|
||||
* third is acceleration (in pixels per second squared).
|
||||
*
|
||||
* @hide For internal use only. Not a final API.
|
||||
*/
|
||||
public static final class Estimator {
|
||||
// Must match VelocityTracker::Estimator::MAX_DEGREE
|
||||
private static final int MAX_DEGREE = 2;
|
||||
|
||||
/**
|
||||
* Polynomial coefficients describing motion in X.
|
||||
*/
|
||||
public final float[] xCoeff = new float[MAX_DEGREE + 1];
|
||||
|
||||
/**
|
||||
* Polynomial coefficients describing motion in Y.
|
||||
*/
|
||||
public final float[] yCoeff = new float[MAX_DEGREE + 1];
|
||||
|
||||
/**
|
||||
* Polynomial degree, or zero if only position information is available.
|
||||
*/
|
||||
public int degree;
|
||||
|
||||
/**
|
||||
* Confidence (coefficient of determination), between 0 (no fit) and 1 (perfect fit).
|
||||
*/
|
||||
public float confidence;
|
||||
|
||||
/**
|
||||
* Gets an estimate of the X position of the pointer at the specified time point.
|
||||
* @param time The time point in seconds, 0 is the last recorded time.
|
||||
* @return The estimated X coordinate.
|
||||
*/
|
||||
public float estimateX(float time) {
|
||||
return estimate(time, xCoeff);
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets an estimate of the Y position of the pointer at the specified time point.
|
||||
* @param time The time point in seconds, 0 is the last recorded time.
|
||||
* @return The estimated Y coordinate.
|
||||
*/
|
||||
public float estimateY(float time) {
|
||||
return estimate(time, yCoeff);
|
||||
}
|
||||
|
||||
private float estimate(float time, float[] c) {
|
||||
float a = 0;
|
||||
float scale = 1;
|
||||
for (int i = 0; i <= degree; i++) {
|
||||
a += c[i] * scale;
|
||||
scale *= time;
|
||||
}
|
||||
return a;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -51,7 +51,10 @@ public class PointerLocationView extends View {
|
||||
// Most recent velocity.
|
||||
private float mXVelocity;
|
||||
private float mYVelocity;
|
||||
|
||||
|
||||
// Position estimator.
|
||||
private VelocityTracker.Estimator mEstimator = new VelocityTracker.Estimator();
|
||||
|
||||
public void clearTrace() {
|
||||
mTraceCount = 0;
|
||||
}
|
||||
@@ -75,6 +78,10 @@ public class PointerLocationView extends View {
|
||||
}
|
||||
}
|
||||
|
||||
private final int ESTIMATE_PAST_POINTS = 4;
|
||||
private final int ESTIMATE_FUTURE_POINTS = 2;
|
||||
private final float ESTIMATE_INTERVAL = 0.02f;
|
||||
|
||||
private final ViewConfiguration mVC;
|
||||
private final Paint mTextPaint;
|
||||
private final Paint mTextBackgroundPaint;
|
||||
@@ -278,8 +285,20 @@ public class PointerLocationView extends View {
|
||||
haveLast = true;
|
||||
}
|
||||
|
||||
// Draw velocity vector.
|
||||
if (drawn) {
|
||||
// Draw movement estimate curve.
|
||||
mPaint.setARGB(128, 128, 0, 128);
|
||||
float lx = ps.mEstimator.estimateX(-ESTIMATE_PAST_POINTS * ESTIMATE_INTERVAL);
|
||||
float ly = ps.mEstimator.estimateY(-ESTIMATE_PAST_POINTS * ESTIMATE_INTERVAL);
|
||||
for (int i = -ESTIMATE_PAST_POINTS + 1; i <= ESTIMATE_FUTURE_POINTS; i++) {
|
||||
float x = ps.mEstimator.estimateX(i * ESTIMATE_INTERVAL);
|
||||
float y = ps.mEstimator.estimateY(i * ESTIMATE_INTERVAL);
|
||||
canvas.drawLine(lx, ly, x, y, mPaint);
|
||||
lx = x;
|
||||
ly = y;
|
||||
}
|
||||
|
||||
// Draw velocity vector.
|
||||
mPaint.setARGB(255, 255, 64, 128);
|
||||
float xVel = ps.mXVelocity * (1000 / 60);
|
||||
float yVel = ps.mYVelocity * (1000 / 60);
|
||||
@@ -517,6 +536,7 @@ public class PointerLocationView extends View {
|
||||
ps.addTrace(coords.x, coords.y);
|
||||
ps.mXVelocity = mVelocity.getXVelocity(id);
|
||||
ps.mYVelocity = mVelocity.getYVelocity(id);
|
||||
mVelocity.getEstimator(id, -1, -1, ps.mEstimator);
|
||||
ps.mToolType = event.getToolType(i);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -29,6 +29,14 @@ namespace android {
|
||||
// Special constant to request the velocity of the active pointer.
|
||||
static const int ACTIVE_POINTER_ID = -1;
|
||||
|
||||
static struct {
|
||||
jfieldID xCoeff;
|
||||
jfieldID yCoeff;
|
||||
jfieldID degree;
|
||||
jfieldID confidence;
|
||||
} gEstimatorClassInfo;
|
||||
|
||||
|
||||
// --- VelocityTrackerState ---
|
||||
|
||||
class VelocityTrackerState {
|
||||
@@ -39,6 +47,8 @@ public:
|
||||
void addMovement(const MotionEvent* event);
|
||||
void computeCurrentVelocity(int32_t units, float maxVelocity);
|
||||
void getVelocity(int32_t id, float* outVx, float* outVy);
|
||||
bool getEstimator(int32_t id, uint32_t degree, nsecs_t horizon,
|
||||
VelocityTracker::Estimator* outEstimator);
|
||||
|
||||
private:
|
||||
struct Velocity {
|
||||
@@ -118,6 +128,11 @@ void VelocityTrackerState::getVelocity(int32_t id, float* outVx, float* outVy) {
|
||||
}
|
||||
}
|
||||
|
||||
bool VelocityTrackerState::getEstimator(int32_t id, uint32_t degree, nsecs_t horizon,
|
||||
VelocityTracker::Estimator* outEstimator) {
|
||||
return mVelocityTracker.getEstimator(id, degree, horizon, outEstimator);
|
||||
}
|
||||
|
||||
|
||||
// --- JNI Methods ---
|
||||
|
||||
@@ -169,6 +184,30 @@ static jfloat android_view_VelocityTracker_nativeGetYVelocity(JNIEnv* env, jclas
|
||||
return vy;
|
||||
}
|
||||
|
||||
static jboolean android_view_VelocityTracker_nativeGetEstimator(JNIEnv* env, jclass clazz,
|
||||
jint ptr, jint id, jint degree, jint horizonMillis, jobject outEstimatorObj) {
|
||||
VelocityTrackerState* state = reinterpret_cast<VelocityTrackerState*>(ptr);
|
||||
VelocityTracker::Estimator estimator;
|
||||
bool result = state->getEstimator(id,
|
||||
degree < 0 ? VelocityTracker::DEFAULT_DEGREE : uint32_t(degree),
|
||||
horizonMillis < 0 ? VelocityTracker::DEFAULT_HORIZON :
|
||||
nsecs_t(horizonMillis) * 1000000L,
|
||||
&estimator);
|
||||
|
||||
jfloatArray xCoeffObj = jfloatArray(env->GetObjectField(outEstimatorObj,
|
||||
gEstimatorClassInfo.xCoeff));
|
||||
jfloatArray yCoeffObj = jfloatArray(env->GetObjectField(outEstimatorObj,
|
||||
gEstimatorClassInfo.yCoeff));
|
||||
|
||||
env->SetFloatArrayRegion(xCoeffObj, 0, VelocityTracker::Estimator::MAX_DEGREE + 1,
|
||||
estimator.xCoeff);
|
||||
env->SetFloatArrayRegion(yCoeffObj, 0, VelocityTracker::Estimator::MAX_DEGREE + 1,
|
||||
estimator.yCoeff);
|
||||
env->SetIntField(outEstimatorObj, gEstimatorClassInfo.degree, estimator.degree);
|
||||
env->SetFloatField(outEstimatorObj, gEstimatorClassInfo.confidence, estimator.confidence);
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
// --- JNI Registration ---
|
||||
|
||||
@@ -195,12 +234,35 @@ static JNINativeMethod gVelocityTrackerMethods[] = {
|
||||
{ "nativeGetYVelocity",
|
||||
"(II)F",
|
||||
(void*)android_view_VelocityTracker_nativeGetYVelocity },
|
||||
{ "nativeGetEstimator",
|
||||
"(IIIILandroid/view/VelocityTracker$Estimator;)Z",
|
||||
(void*)android_view_VelocityTracker_nativeGetEstimator },
|
||||
};
|
||||
|
||||
#define FIND_CLASS(var, className) \
|
||||
var = env->FindClass(className); \
|
||||
LOG_FATAL_IF(! var, "Unable to find class " className);
|
||||
|
||||
#define GET_FIELD_ID(var, clazz, fieldName, fieldDescriptor) \
|
||||
var = env->GetFieldID(clazz, fieldName, fieldDescriptor); \
|
||||
LOG_FATAL_IF(! var, "Unable to find field " fieldName);
|
||||
|
||||
int register_android_view_VelocityTracker(JNIEnv* env) {
|
||||
int res = jniRegisterNativeMethods(env, "android/view/VelocityTracker",
|
||||
gVelocityTrackerMethods, NELEM(gVelocityTrackerMethods));
|
||||
LOG_FATAL_IF(res < 0, "Unable to register native methods.");
|
||||
|
||||
jclass clazz;
|
||||
FIND_CLASS(clazz, "android/view/VelocityTracker$Estimator");
|
||||
|
||||
GET_FIELD_ID(gEstimatorClassInfo.xCoeff, clazz,
|
||||
"xCoeff", "[F");
|
||||
GET_FIELD_ID(gEstimatorClassInfo.yCoeff, clazz,
|
||||
"yCoeff", "[F");
|
||||
GET_FIELD_ID(gEstimatorClassInfo.degree, clazz,
|
||||
"degree", "I");
|
||||
GET_FIELD_ID(gEstimatorClassInfo.confidence, clazz,
|
||||
"confidence", "F");
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user