Estimation of Focus of expansion

Question

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;

Answer 1

The focus of expansion (FOE) is the point in the image that has the smallest cross product with all of the optical flow vectors. Each optical flow vector has a previous point and a delta. Let $p_x$ and $p_y$ be the x and y positions of the previous position of an optical flow vector. Let $dx$ and $dy$ be the change in x and y position between the previous location and the current location.

The cross product between a single optical flow vector and the FOE is \begin{equation} M = \begin{bmatrix} \hat i & \hat j & \hat k \\ p_x-foe_x & p_y - foe_y &0\\ dx & dy &0 \end{bmatrix} \end{equation}

\begin{equation} M = \hat k \left( (p_x - foe_x)dy - (p_y - foe_y)dx \right) \end{equation}

The moment, $M$, is what we want to minimize. So setting $M$ equal to zero we can solve for the FOE.

\begin{equation} \begin{bmatrix} -dy & dx \\ ... & ... \\ \end{bmatrix} \begin{bmatrix} foe_x \\ foe_y\\ \end{bmatrix} = \begin{bmatrix} p_ydx-p_x dy \\ ...\\ \end{bmatrix} \end{equation}

Note that my matrices do not match the matrices in the paper; their $v$ term is positive while I believe it should be negative.

Answer 2

Have you tried reversing your derivatives so that velocities are specified forward in time? i.e., do next_pts - prev_pts instead of prev_pts - next_pts. Negating all the constraint equations could result in the crazy instability you describe.

It also looks like A.row(i) can be simplified to just tmp.

If you haven't already, you should also try visualizing the optical flow results that are feeding into this estimation, to make sure that they look good enough for you, as a human, to identify FOE from them.