C++/OpenCV - Kalman filter for video stabilization

edit

I try to Stabilize video with a Kalman filter for smoothing . But i have some problems

Each time, i have two frames: one current and another one. Here my workflow:

• Compute goodFeaturesToTrack()
• Compute Optical Flow using calcOpticalFlowPyrLK()
• Keep only good points
• Estimate a rigid transformation
• Smoothing using Kalman filter
• Warping of the picture.

But i think there is something wrong with Kalman because at the end my video is still not stabilized and it's not smooth at all, it even worse than the original...

Here my code of Kalman

    cv::Mat StabilizationTestSimple2::computeMask(Mat& frame,int lh,int lw){
    int height=frame.rows;
    int width=frame.cols;

    /// Creation du masque pour le calcul de point d'interet
    int limitH=lh;
    int limitW=lw;
    cv::Mat mask = cv::Mat::zeros(frame.size(), CV_8UC1);  //NOTE: using the type explicitly
    cv::Mat roi1=frame(Range(0,limitH),Range(0,limitW));//Coin haut à gauche
    cv::Mat roi2=frame(Range(height-limitH,height),Range(0,limitW));//Coinbas à gauche
    cv::Mat roi3=frame(Range(0,limitH),Range(width-limitW,width));//Coin haut à droite
    cv::Mat roi4=frame(Range(height-limitH,height),Range(width-limitW,width));//Coin haut à droite

    roi1.copyTo(mask(Range(0,limitH),Range(0,limitW)));
    roi2.copyTo(mask(Range(height-limitH,height),Range(0,limitW)));
    roi3.copyTo(mask(Range(0,limitH),Range(width-limitW,width)));
    roi4.copyTo(mask(Range(height-limitH,height),Range(width-limitW,width)));

    return mask;

}

void StabilizationTestSimple2::init_kalman(double x, double y)
{

    KF.statePre.at<float>(0) = x;
    KF.statePre.at<float>(1) = y;
    KF.statePre.at<float>(2) = 0;
    KF.statePre.at<float>(3) = 0;

    KF.transitionMatrix = *(Mat_<float>(4,4) << 1,0,1,0,    0,1,0,1,     0,0,1,0,   0,0,0,1);
    KF.processNoiseCov = *(Mat_<float>(4,4) << 0.2,0,0.2,0,  0,0.2,0,0.2,  0,0,0.3,0,
                           0,0,0,0.3);
    setIdentity(KF.measurementMatrix);
    setIdentity(KF.processNoiseCov,Scalar::all(1e-6));
    setIdentity(KF.measurementNoiseCov,Scalar::all(1e-1));
    setIdentity(KF.errorCovPost, Scalar::all(.1));
}

and here how i use it. I put only the interesting part. All this code is inside a flor loop. cornerPrev2 and cornerCurr2 contains all the features points detected just before (with calcOpticalFlowPyrLK())

    /// Transformation
    Mat transformMatrix = estimateRigidTransform(cornersPrev2,cornersCurr2 ,false);

    // in rare cases no transform is found. We'll just use the last known good transform.
    if(transformMatrix.data == NULL) {
        last_transformationmatrix.copyTo(transformMatrix);
    }

    transformMatrix.copyTo(last_transformationmatrix);

    // decompose T
    double dx = transformMatrix.at<double>(0,2);
    double dy = transformMatrix.at<double>(1,2);
    double da = atan2(transformMatrix.at<double>(1,0), transformMatrix.at<double>(0,0));

    // Accumulated frame to frame transform
    x += dx;
    y += dy;
    a += da;
    std::cout << "accumulated x,y: (" << x << "," << y << ")" << endl;

    if (compteur==0){
        init_kalman(x,y);
    }
    else {


          vector<Point2f> smooth_feature_point;
          Point2f smooth_feature=kalman_predict_correct(x,y);
          smooth_feature_point.push_back(smooth_feature);
          std::cout << "smooth x,y: (" << smooth_feature.x << "," << smooth_feature.y << ")" << endl;

          // target - current
          double diff_x = smooth_feature.x - x;//
          double diff_y = smooth_feature.y - y;

          dx = dx + diff_x;
          dy = dy + diff_y;

          transformMatrix.at<double>(0,0) = cos(da ...