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