I am trying to estimate focus of expansion for a moving camera mounted in a mobile robot. I am using the method described in this method page 13-14. My aim is to use it to avoid obstacle using optical flow. I have attached the code below. Calculating the terms when I display the calculated point it keeps jumping around and isnt correct. Any advice would be appreciated.
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;