Video Stabilization - c++

Question

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++;
}
}

Answer 1

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));
}