Tracking using PTU

edit

Hi guys.. I am trying to track object (head) and based on the position move a PTU (Pan-tilt unit) based on where the head is located.

So i am using Lukas kanade optical flow to track the movement of the face.. I quickly realized that it has some issues with points drifting away, for which i tried to come up with a fix. So my idea is to look at the goodFeatures as a distribution, with a µ and the standard deviation.
So to prevent outliers, i try to keep my goodFeatures within 1 standardDeviation (or close to µ), such that I Only keep points which is closest to my head. When i come to a time where the amount of GoodFeatures, which is used to track, is becomes below a threshold value, I re initiate the goodFeaturesToTrack function to get new points which will be used for tracking, from a mask which keeps getting updated as long the standard deviation is acceptable.

Problem is though that the reinitiating does not occur as expected since i am not able to erase all points without the program crashes, which I hoped you would be able to help..

I am thinking of adding a template matching scheme such that when the mean begins to drift, i still will be able to just detect the template, and track based on that.

My code is on Github: http://tinyurl.com/q4w9njx

Furthermore what do you guys think of the way of tackling the problem, It's simple, maybe too simple??..

Feedback and improvement is always appreciated :)