Video Stabilization

I 'm researching about Video Stabilization field. I implement a application using OpenCV.

My progress such as:

Surf points extraction

Matching

estimateRigidTransform

warpAffine

But the result video is not be stable. Can anyone help me this problem or provide me some source code link to improve?

Sample video: Hippo video

Here is my code [EDIT]

#include "stdafx.h"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <iostream>
#include <stdio.h>
#include <opencv2/nonfree/features2d.hpp>
#include <opencv2/opencv.hpp>

const double smooth_level = 0.7;
using namespace cv;
using namespace std;
struct TransformParam
{
    TransformParam() {}
    TransformParam(double _dx, double _dy, double _da) {
    dx = _dx;
    dy = _dy;
    da = _da;
}
   double dx; // translation x
   double dy; // translation y
   double da; // angle
};
int main( int argc, char** argv )
{
     VideoCapture cap ("test12.avi");  
     Mat cur, cur_grey;
     Mat prev, prev_grey;


     cap >> prev;
     cvtColor(prev, prev_grey, COLOR_BGR2GRAY);

     // Step 1 - Get previous to current frame transformation (dx, dy, da) for all frames
     vector <TransformParam> prev_to_cur_transform; // previous to current

     int k=1;
     int max_frames =  cap.get(CV_CAP_PROP_FRAME_COUNT);
     VideoWriter writeVideo ("stable.avi",0,30,cvSize(prev.cols,prev.rows),true);
     Mat last_T;
     double avg_dx = 0, avg_dy = 0, avg_da = 0;
     Mat smooth_T(2,3,CV_64F);
     while(true) {
        cap >> cur;

        if(cur.data == NULL) {
           break;
        }

        cvtColor(cur, cur_grey, COLOR_BGR2GRAY);

        // vector from prev to cur
        vector <Point2f> prev_corner, cur_corner;
        vector <Point2f> prev_corner2, cur_corner2;
        vector <uchar> status;
        vector <float> err;

        goodFeaturesToTrack(prev_grey, prev_corner, 200, 0.01, 30);
        calcOpticalFlowPyrLK(prev_grey, cur_grey, prev_corner, cur_corner, status, err);

       // weed out bad matches
       for(size_t i=0; i < status.size(); i++) {
           if(status[i]) {
               prev_corner2.push_back(prev_corner[i]);
              cur_corner2.push_back(cur_corner[i]);
           }
       }

       // translation + rotation only
       Mat T = estimateRigidTransform(prev_corner2, cur_corner2, false); 

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

       T.copyTo(last_T);

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

      avg_dx = (avg_dx * smooth_level) + (dx * (1- smooth_level));
      avg_dy = (avg_dy * smooth_level) + (dy * (1- smooth_level));
      avg_da = (avg_da * smooth_level) + (da * (1- smooth_level));

      smooth_T.at<double>(0,0) = cos(avg_da);
      smooth_T.at<double>(0,1) = -sin(avg_da);
      smooth_T.at<double>(1,0) = sin(avg_da);
      smooth_T.at<double>(1,1) = cos(avg_da);

      smooth_T.at<double>(0,2) = avg_dx;
      smooth_T.at<double>(1,2) = avg_dy;

      Mat stable;
      warpAffine(prev,stable,smooth_T,prev.size());

      Mat canvas = Mat::zeros(cur.rows, cur.cols*2+10, cur.type());
      prev.copyTo(canvas(Range::all(), Range(0, prev.cols)));
      stable.copyTo(canvas(Range::all(), Range(prev.cols+10, prev.cols*2+10)));

      imshow("before and after", canvas);
      waitKey(20);
      writeVideo.write(stable);
      cur.copyTo(prev);
      cur_grey.copyTo(prev_grey);
      k++;
   }
 }

First, you can just blur you image. It will helps a bit. Second, you can easily smooth your matrix by simplest implementation of exponential smooth A(t+1) = a*A(t)+(1-a)*A(t+1) and play with a-value in [0;1] range. Third, you can turn off some type of transformations like rotation, shift etc. Here is code example:

t = estimateRigidTransform(new, old, 0); // 0 means not all transformations (5 of 6)
if(!t.empty()){
//  t(Range(0,2), Range(0,2)) = Mat::eye(2, 2, CV_64FC1); // turning off rotation
//  t.at<double>(0,2) = 0; t.at<double>(1,2) = 0; // turning off shift dx and dy
    tAvrg = tAvrg*a + t*(1-a); // a - smooth level in [0;1] range, play with it
    warpAffine(new, stable, tAvrg, Size(new.cols, new.rows));
}