ConDensation algo

edit

i've been trying to come up with a 2.0 replacement for the old ConDensation algorithm that went into legacy, based on [tom sgouros patch] (http://code.opencv.org/issues/2826).

his main approach there seems to be to re-weight the random distribution in every step according to the sample-ranges, instead of keeping it fixed like in the original code.

trying this, i found, that the prediction still gets unstable, when the measurement stays still, does not change any more.

my "fix" for that was to pull the measurement into the random-distribution code, make the particles congeal, when we actually reached the target, but i have some bad feelings about it. is this the right way ? how should condension behave at all at that point ?

looking for ideas / comments on how to address the stability problem here.

if you feel like trying the original legacy code, here's a demo

the 'offending' piece of code is this (from ConDensation::updateByTime()):

if ( 0 ) // Tom's modification
{
    // This line may not be strictly necessary, but it prevents
    // the particles from congealing into a single particle in the
    // event of a poor choice of fitness (weighting) function.
    diff = max(diff, 0.02f * newSamples(0,d));
} else {  // my approach:
    // Rule 1 : reaching the target is the goal here, right ? 
    // * if we lost it         : swarm out  
    // * if target was reached : hog it .
    diff = min(diff, flocking * (measure(d) - newSamples(0,d)));
}

and here's the whole wall of code (337 lines, love it or leave it):

#include <opencv2/core/core.hpp>
#include <opencv2/core/utility.hpp>

using namespace cv;

class ConDensation 
{
public:

    //
    //! All Matrices interfaced here are expected to have dp cols, and are float.
    //
    ConDensation( int dp, int numSamples, float flocking=0.9f );

    //! Reset. call at least once before correct()
    void initSampleSet( const Mat & lowerBound, const Mat & upperBound, const Mat & dyna=Mat() );

    //! Update the state and return prediction.
    const Mat & correct( const Mat & measurement );

    //! Access single samples(read only).
    int   sampleCount()       { return samples.rows; }
    float sample(int j,int i) { return samples(j,i); }

private:

    int DP;                      //! Sample dimension
    int numSamples;              //! Number of the Samples                 
    float flocking;              //! flocking/congealing factor
    Mat_<float> range;           //! Scaling factor for correction, the upper bound from the orig. samples
    Mat_<float> dynamMatr;       //! Matrix of the linear Dynamics system  
    Mat_<float> samples;         //! Arr of the Sample Vectors             
    Mat_<float> newSamples;      //! Temporary array of the Sample Vectors 
    Mat_<float> confidence;      //! Confidence for each Sample            
    Mat_<float> cumulative;      //! Cumulative confidence                 
    Mat_<float> randomSample;    //! RandomVector to update sample set     
    Mat_<float> state;           //! Predicted state vector
    Mat_<float> mean;            //! Internal mean vector
    Mat_<float> measure;         //! Cached measurement vector

    struct Rand                  //! CvRandState replacement
    {
        RNG r;
        float lo, hi;
        Rand(float l=0.0f,float h=1.0f) { set(getTickCount(),l,h); }
        void set(int64 s=0, float l=0.0f, float h=1.0f) { r.state=s; lo=l; hi=h;}
        float uni() { return r.uniform(lo,hi); }
    };
    std::vector<Rand> rng;       //! One rng for each dimension.

    void updateByTime();
};


ConDensation::ConDensation( int dp, int ...