I am trying to implement an obstacle avoidance algorithm using monocular camera. I have tried out the approach mentioned in this paper. But I am unable to estimate Focus of expansion accurately. I am suing formula mentioned in this paper at page 13-14 to calculate FOE. Is there any other method or easy implementation that I can readily use as I am not a computer vision expert and just need this application. Here is my code in case it helps.
void calculate_FOE(vector<Point2f> prev_pts, vector<Point2f> next_pts)
MatrixXf A(next_pts.size(),2);
MatrixXf b(next_pts.size(),1);
Point2f tmp;
for(int i=0;i<next_pts.size();i++)
{
tmp= prev_pts[i]-next_pts[i];
A.row(i)<<prev_pts[i].x-next_pts[i].x,prev_pts[i].y-next_pts[i].y;
b.row(i)<<(prev_pts[i].x*tmp.x)-(prev_pts[i].y*tmp.y);
}
Matrix<float,2,1> FOE;
FOE=((A.transpose()*A).inverse())*A.transpose()*b;