|| || || || || || || ||

---

FunctorKalman.h

Go to the documentation of this file.

#ifndef Impala_Core_Array_Trait_FuncKalman_h
#define Impala_Core_Array_Trait_FuncKalman_h

#include <deque>

namespace Impala
{
namespace Core
{
namespace Array
{
namespace Element
{

inline double EuclidDistSquare(const Vec3Real64& p1, const Vec3Real64& p2)
{
    double t,d;
    t = p1.X() - p2.X();
    d = t*t;
    t = p1.Y() - p2.Y();
    d += t*t;
    t = p1.Z() - p2.Z();
    d += t*t;
    return d;
}

} // namespace Element

namespace Trait
{

class FuncKalman
{
private:
    Array2dScalarUInt8* mOutCounter;
    std::deque<double> mPastResiduals;
    double mOutlierTreshold;
public:
    double mSigmaI, mSigmaW, mSigmaG;
    double mAlpha, mBeta, mGamma, mN_omax, mK, mPixelCount;
    int mOutliers;
    double mResidualThisFrame,mResidualAvrg;
    bool mInitialised,mOcclusion;

    typedef Pattern::TagTransVar TransVarianceCategory;
    typedef Pattern::TagCallValue CallCategory;
        typedef Vec3Int32 DstArithT;
        typedef Vec3Int32 SrcArithT;

    FuncKalman(int w, int h)
    {
        // 'static' properties
        mAlpha = 2.5;
        mBeta = 1.1;
        mGamma = 0.4;
        mN_omax = 20;
        mK = 20;

        mOutCounter = 0;
        Reset(w,h);
    }

    void Reset(int w, int h)
    {
        if(mOutCounter)
            delete mOutCounter;
        mOutCounter = new Array2dScalarUInt8(w,h,0,0);
        int i;
        for(i=0 ; i<w*h ; i++)
            mOutCounter->mData[i] = 0;

        mOutlierTreshold = 196608;
        mSigmaI = mSigmaW = 0;
        mSigmaG = 1;
        mPastResiduals.clear();
        mResidualThisFrame = 0;
        mPixelCount = 0;
        mOutliers = 0;
        mInitialised = false;
        mOcclusion = false;
    }

    Vec3Real64
        doIt(const Vec3Real64& p1, const Vec3Real64& p2, int x, int y)
    {
        double r2;
        r2 = EuclidDistSquare(p1,p2);

        //(part of)eq.14
        mResidualThisFrame += r2;
        mPixelCount++;
        //eq.12
        if(r2 < mOutlierTreshold)
        {
            mOutCounter->mData[y*mOutCounter->CW() + x] = 0;
            //eq.10
            return (p1*mSigmaI + p2*mSigmaG) / (mSigmaI+mSigmaG);
        }
        else
        {
            mOutliers++;
            if(mOcclusion) // this extra check to leave the outlier counters intact during occlusion
                return p1; 
            else
            {
                if(mOutCounter->mData[y*mOutCounter->CW() + x] >= mN_omax)
                {
                    mOutCounter->mData[y*mOutCounter->CW() + x] = 0;
                    return p2;
                }
                else
                {
                    mOutCounter->mData[y*mOutCounter->CW() + x]++;
                    return p1;
                }
            }
        }
    }

    void InitialiseSigma()
    {
        //eq.16
        mSigmaW = 0;
        mSigmaI = 0.5*mResidualAvrg;
        mSigmaG = 0.5*mResidualAvrg;
        mInitialised = true;
    }

    void UpdateKalman()
    {
        //eq.14
        mResidualThisFrame /= mPixelCount;
        mResidualThisFrame *= mBeta;
        if(mResidualThisFrame == 0)
            mResidualThisFrame = 1; 

        if(mPixelCount * mGamma > mOutliers) // if no occlusion
        {
            mOcclusion = false;
            ComputeResiduals();
            if(!mInitialised)
                InitialiseSigma();
            //eq.11
            mSigmaG = (mSigmaG*mSigmaI)/(mSigmaG+mSigmaI);
            //eq.15
            mSigmaW = mResidualAvrg - mSigmaI - mSigmaG;
            // following found in Hieus code, makes some sense, but only because the
            // assumption about residual and sigmas are incorrect (I can't define them, sorry)
            if(mSigmaW<0)
                mSigmaW = 0;
            //eq.9 (prediction of sigmaG for the next time step)
            mSigmaG += mSigmaW;
        }
        else
        {
            if(!mOcclusion)
            {
                //undo prediction of sigmaG (eq.9) (only if the last time sigmaG was predicted)
                mSigmaG -= mSigmaW;
            }
            mOcclusion = true;
        }
        mPixelCount = 0;
        mOutliers = 0;
    }

    void ComputeResiduals()
    {
        //eq.13
        mPastResiduals.push_back(mResidualThisFrame);
        mResidualThisFrame = 0;
        mResidualAvrg = 0;
        std::deque<double>::iterator i;
        for(i=mPastResiduals.begin() ; i!=mPastResiduals.end() ; i++)
        {
            mResidualAvrg += *i;
        }
        mResidualAvrg /= mPastResiduals.size();
        mOutlierTreshold = mResidualAvrg * mAlpha;
        while(mPastResiduals.size() > mK)
            mPastResiduals.pop_front();
    }

};

/**************************** FuncKalmanColor ****************************/
class FuncKalmanColor
{
private:
    Array2dScalarUInt8* mOutCounter;
    std::deque<double> mPastResiduals[3];
    double mOutlierTreshold;
public:
    double mSigmaI[3], mSigmaW[3], mSigmaG[3];
    double mChi2, mGamma, mN_omax, mK, mPixelCount;
    int mOutliers;
    double mResidualThisFrame[3], mResidualAvrg[3];
    bool mInitialised, mOcclusion;

    typedef Pattern::TagTransVar TransVarianceCategory;
    typedef Pattern::TagCallPointer CallCategory;
        typedef Vec3Int32 DstArithT;
        typedef Vec3Int32 SrcArithT;

    FuncKalmanColor()
    {
        CmdOptions& options = CmdOptions::GetInstance();
        // 'static' properties
        mChi2 = options.GetDouble("appearance_kalman.kalman.chi^2", 13); //is (Chi_d,delta)^2 where d=3 and delta=0.99
        mGamma = options.GetDouble("appearance_kalman.kalman.gamma", 0.3);
        mN_omax = options.GetDouble("appearance_kalman.kalman.N_omax", 40);
        mK = options.GetDouble("appearance_kalman.kalman.K", 20);

        mOutCounter = 0;
        Reset(0,0);
    }

    void Reset(int w, int h)
    {
        if(mOutCounter)
            delete mOutCounter;
        mOutCounter = new Array2dScalarUInt8(w,h,0,0);
        int i;
        for(i=0 ; i<w*h ; i++)
            mOutCounter->mData[i] = 0;

        mOutlierTreshold = 196608; //todo make a paramater out of this!
        for(i=0 ; i<3 ; i++)
        {
            mSigmaI[i] = 0;
            mSigmaW[i] = 0;
            mSigmaG[i] = 1;
            mPastResiduals[i].clear();
            mResidualThisFrame[i] = 0;
        }
        mPixelCount = 0;
        mOutliers = 0;
        mInitialised = false;
        mOcclusion = false;
    }

    double MahalanobisDist(double* p1, double* p2)
    {
        double r, err=0;
        for(int i=0 ; i<3 ; i++)
        {
            r = p1[i] - p2[i];
            err += r*r/(mSigmaG[i]+mSigmaI[i]);
            mResidualThisFrame[i] += r*r;
        }
        return err;
    }

    void
        DoIt(double* out, double* p1, double* p2, int x, int y)
    {
        double err;
        err = MahalanobisDist(p1,p2); // mahalanobis dist

        mPixelCount++;
        //eq.11
        if(err < mChi2 || !mInitialised)
        {
            mOutCounter->mData[y*mOutCounter->CW() + x] = 0;
            //eq.9
            for(int i=0 ; i<3 ; i++)
                out[i] = (p1[i] * mSigmaI[i] + p2[i] * mSigmaG[i]) / (mSigmaI[i]+mSigmaG[i]);
            return;
        }
        else
        {
            mOutliers++;
            if(mOcclusion)
            {
                for(int i=0 ; i<3 ; i++)
                    out[i] = p1[i];
                return;
            }
            else
            {
                if(mOutCounter->mData[y*mOutCounter->CW() + x] >= mN_omax)
                {
                    mOutCounter->mData[y*mOutCounter->CW() + x] = 0;
                    for(int i=0 ; i<3 ; i++)
                        out[i] = p2[i];
                    return;
                }
                else
                {
                    mOutCounter->mData[y*mOutCounter->CW() + x]++;
                    for(int i=0 ; i<3 ; i++)
                        out[i] = p1[i];
                    return;
                }
            }
        }
    }

    void InitialiseSigma()
    {
        for(int i=0 ; i<3 ; i++)
        {
            //eq.15
            mSigmaW[i] = 0;
            mSigmaI[i] = 0.5*mResidualAvrg[i];
            mSigmaG[i] = 0.5*mResidualAvrg[i];
        }
        mInitialised = true;
    }

    void UpdateKalman()
    {
        int i;
        for(i=0 ; i<3 ; i++)
        {
            //eq.12
            mResidualThisFrame[i] /= mPixelCount;
            if(mResidualThisFrame[i] == 0)
                mResidualThisFrame[i] = 1; 
        }

        if(mPixelCount * mGamma > mOutliers) // if no occlusion
        {
            mOcclusion = false;
            ComputeResiduals();
            if(!mInitialised)
                InitialiseSigma();
            for(i=0 ; i<3 ; i++)
            {
                //eq.10
                mSigmaG[i] = (mSigmaG[i]*mSigmaI[i])/(mSigmaG[i]+mSigmaI[i]);
                //eq.14
                mSigmaW[i] = mResidualAvrg[i] - mSigmaI[i] - mSigmaG[i];
                // following found in Hieus code, makes some sense, but only because the
                // assumption about residual and sigmas are incorrect (I can't define them, sorry)
                if(mSigmaW[i]<0)
                    mSigmaW[i] = 0;
                //eq.8 (prediction of sigmaG for the next time step)
                mSigmaG[i] += mSigmaW[i];
            }
        }
        else
        {
            if(!mOcclusion)
            {
                //undo prediction of sigmaG (eq.8) (only if the last time sigmaG was predicted)
                for(i=0 ; i<3 ; i++)
                    mSigmaG[i] -= mSigmaW[i];
            }
            mOcclusion = true;

            /* *
                \note augmentation to hieus algorithm: increment sigma G so that the algorithm
                leaves occlusion mode with increasing probability 
                */
            
            for(i=0 ; i<3 ; i++)
                mSigmaG[i] *= 1.15;
            
        }
        mPixelCount = 0;
        mOutliers = 0;
    }

    void ComputeResiduals()
    {
        //eq.13
        for(int i=0 ; i<3 ; i++)
        {
            mPastResiduals[i].push_back(mResidualThisFrame[i]);
            mResidualThisFrame[i] = 0;
            mResidualAvrg[i] = 0;
            std::deque<double>::iterator it;
            for(it=mPastResiduals[i].begin() ; it!=mPastResiduals[i].end() ; it++)
            {
                mResidualAvrg[i] += *it;
            }
            mResidualAvrg[i] /= mPastResiduals[i].size();
            mOutlierTreshold = mResidualAvrg[i];
            while(mPastResiduals[i].size() > mK)
                mPastResiduals[i].pop_front();
        }
    }
};


/**************************** FuncBpoRobustMahalanobis ****************************/
class FuncBpoRobustMahalanobis
{
public:
    typedef Pattern::TagTransInVar TransVarianceCategory;
    typedef Pattern::TagCallPointer CallCategory;

        typedef Vec3Int32 DstArithT;
        typedef Vec3Int32 Src1ArithT;
        typedef Vec3Int32 Src2ArithT;

    double mSigma[3];
    double mError;
    double mChi;
    
    FuncBpoRobustMahalanobis() 
    {
        mSigma[0] = mSigma[1] = mSigma[2] = 1;
        mError = 0;
        mChi = 3.5;
    }

    void
        DoIt(double* res, double* p1, double* p2)
        {
        double r, err=0;
        for(int i=0 ; i<3 ; i++)
        {
            r = p1[i] - p2[i];
            err += r*r/(mSigma[i]);
        }

        // implements Hubers functon (eq.3)
        if(err < mChi)
        {
            double r = 0.5 * err * err;
            res[0] = res[1] = res[2] = r;
            mError += r;
        }
        else
        {
            double r = mChi*(err - mChi/2);
            res[0] = res[1] = res[2] = r;
            mError += r;
        }
        }
};

} // namespace Element
} // namespace Array
} // namespace Core
} // namespace Impala

#endif //Impala_Core_Array_Trait_FuncKalman_h

---