ClockEstimator.cpp revision aa8b569eb652c22821b93a6e543449a52ad21158 
1/*
2**
3** Copyright 2014, The Android Open Source Project
4**
5** Licensed under the Apache License, Version 2.0 (the "License");
6** you may not use this file except in compliance with the License.
7** You may obtain a copy of the License at
8**
9**     http://www.apache.org/licenses/LICENSE-2.0
10**
11** Unless required by applicable law or agreed to in writing, software
12** distributed under the License is distributed on an "AS IS" BASIS,
13** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14** See the License for the specific language governing permissions and
15** limitations under the License.
16*/
17
18//#define LOG_NDEBUG 0
19#define LOG_TAG "ClockEstimator"
20#include <utils/Log.h>
21
22#include <math.h>
23#include <media/stagefright/ClockEstimator.h>
24
25#include <media/stagefright/foundation/ADebug.h>
26
27namespace android {
28
29WindowedLinearFitEstimator::WindowedLinearFitEstimator(
30        size_t headLength, double headFactor, size_t mainLength, double tailFactor)
31    : mHeadFactorInv(1. / headFactor),
32      mTailFactor(tailFactor),
33      mHistoryLength(mainLength + headLength),
34      mHeadLength(headLength) {
35    reset();
36    mXHistory.resize(mHistoryLength);
37    mYHistory.resize(mHistoryLength);
38    mFirstWeight = pow(headFactor, mHeadLength);
39}
40
41WindowedLinearFitEstimator::LinearFit::LinearFit() {
42    reset();
43}
44
45void WindowedLinearFitEstimator::LinearFit::reset() {
46    mX = mXX = mY = mYY = mXY = mW = 0.;
47}
48
49double WindowedLinearFitEstimator::LinearFit::size() const {
50    double s = mW * mW + mX * mX + mY * mY + mXX * mXX + mXY * mXY + mYY * mYY;
51    if (s > 1e72) {
52        // 1e72 corresponds to clock monotonic time of about 8 years
53        ALOGW("estimator is overflowing: w=%g x=%g y=%g xx=%g xy=%g yy=%g",
54              mW, mX, mY, mXX, mXY, mYY);
55    }
56    return s;
57}
58
59void WindowedLinearFitEstimator::LinearFit::add(double x, double y, double w) {
60    mW += w;
61    mX += w * x;
62    mY += w * y;
63    mXX += w * x * x;
64    mXY += w * x * y;
65    mYY += w * y * y;
66}
67
68void WindowedLinearFitEstimator::LinearFit::combine(const LinearFit &lf) {
69    mW += lf.mW;
70    mX += lf.mX;
71    mY += lf.mY;
72    mXX += lf.mXX;
73    mXY += lf.mXY;
74    mYY += lf.mYY;
75}
76
77void WindowedLinearFitEstimator::LinearFit::scale(double w) {
78    mW *= w;
79    mX *= w;
80    mY *= w;
81    mXX *= w;
82    mXY *= w;
83    mYY *= w;
84}
85
86double WindowedLinearFitEstimator::LinearFit::interpolate(double x) {
87    double div = mW * mXX - mX * mX;
88    if (fabs(div) < 1e-5 * mW * mW) {
89        // this only should happen on the first value
90        return x;
91        // assuming a = 1, we could also return x + (mY - mX) / mW;
92    }
93    double a_div = (mW * mXY - mX * mY);
94    double b_div = (mXX * mY - mX * mXY);
95    ALOGV("a=%.4g b=%.4g in=%g out=%g",
96            a_div / div, b_div / div, x, (a_div * x + b_div) / div);
97    return (a_div * x + b_div) / div;
98}
99
100double WindowedLinearFitEstimator::estimate(double x, double y) {
101    /*
102     * TODO: We could update the head by adding the new sample to it
103     * and amplifying it, but this approach can lead to unbounded
104     * error. Instead, we recalculate the head at each step, which
105     * is computationally more expensive. We could balance the two
106     * methods by recalculating just before the error becomes
107     * significant.
108     */
109    const bool update_head = false;
110    if (update_head) {
111        // add new sample to the head
112        mHead.scale(mHeadFactorInv); // amplify head
113        mHead.add(x, y, mFirstWeight);
114    }
115
116    /*
117     * TRICKY: place elements into the circular buffer at decreasing
118     * indices, so that we can access past elements by addition
119     * (thereby avoiding potentially negative indices.)
120     */
121    if (mNumSamples >= mHeadLength) {
122        // move last head sample from head to the main window
123        size_t lastHeadIx = (mSampleIx + mHeadLength) % mHistoryLength;
124        if (update_head) {
125            mHead.add(mXHistory[lastHeadIx], mYHistory[lastHeadIx], -1.); // remove
126        }
127        mMain.add(mXHistory[lastHeadIx], mYHistory[lastHeadIx], 1.);
128        if (mNumSamples >= mHistoryLength) {
129            // move last main sample from main window to tail
130            mMain.add(mXHistory[mSampleIx], mYHistory[mSampleIx], -1.); // remove
131            mTail.add(mXHistory[mSampleIx], mYHistory[mSampleIx], 1.);
132            mTail.scale(mTailFactor); // attenuate tail
133        }
134    }
135
136    mXHistory.editItemAt(mSampleIx) = x;
137    mYHistory.editItemAt(mSampleIx) = y;
138    if (mNumSamples < mHistoryLength) {
139        ++mNumSamples;
140    }
141
142    // recalculate head unless we were using the update method
143    if (!update_head) {
144        mHead.reset();
145        double w = mFirstWeight;
146        for (size_t headIx = 0; headIx < mHeadLength && headIx < mNumSamples; ++headIx) {
147            size_t ix = (mSampleIx + headIx) % mHistoryLength;
148            mHead.add(mXHistory[ix], mYHistory[ix], w);
149            w *= mHeadFactorInv;
150        }
151    }
152
153    if (mSampleIx > 0) {
154        --mSampleIx;
155    } else {
156        mSampleIx = mHistoryLength - 1;
157    }
158
159    // return estimation result
160    LinearFit total;
161    total.combine(mHead);
162    total.combine(mMain);
163    total.combine(mTail);
164    return total.interpolate(x);
165}
166
167void WindowedLinearFitEstimator::reset() {
168    mHead.reset();
169    mMain.reset();
170    mTail.reset();
171    mNumSamples = 0;
172    mSampleIx = mHistoryLength - 1;
173}
174
175}; // namespace android
176
177
178