Structure From Motion using Farneback’s Optical Flow Part 2

Standard

Here’s another example of structure from motion from 2 camera using Farneback’s optical flow, sample codes included.

Basically, rather than using SIFT to extract key points, or to use Lucas Kanade’s sparse optical flow, we use a Farneback’s dense optical flow to show the transformation of key points between two images.

Then we use these key points to find essential matrix, and finally the rigid body transformation between the points. And finally to use triangulation to find the 3d coordinate of the points in world space.

Below is the sample code that operates on 2 images. For more images, you might need bundle adjustment, which I’m still trying to find out how to do it on OpenCV.

**
 * Structure from motion from 2 cameras, using farneback optical flow as the 'features'
 * No, this doesn't work on more than 2 cams, because that requires bundle adjustment, which
 * I'm still searching if there's an OpenCV implementation of it
 */
Mat sfm( Mat& img1, Mat& img2, Mat& cam_matrix, Mat& dist_coeff ) {
    Mat gray1, gray2;
    cvtColor( img1, gray1, CV_BGR2GRAY );
    cvtColor( img2, gray2, CV_BGR2GRAY );

    /*  Find the optical flow using farneback dense algorithm
        Note that you might need to tune the parameters, especially window size.
        Smaller window size param, means more ambiguity when calculating the flow.
     */
    Mat flow_mat;
    calcOpticalFlowFarneback( gray1, gray2, flow_mat, 0.5, 3, 12, 3, 5, 1.2, 0 );

    vector left_points, right_points;
    for ( int y = 0; y < img1.rows; y+=6 ) {
        for ( int x = 0; x < img1.cols; x+=6 ) {
            /* Flow is basically the delta between left and right points */
            Point2f flow = flow_mat.at(y, x);

            /*  There's no need to calculate for every single point,
                if there's not much change, just ignore it
             */
            if( fabs(flow.x) < 0.1 && fabs(flow.y) < 0.1 )
                continue;

            left_points.push_back(  Point2f( x, y ) );
            right_points.push_back( Point2f( x + flow.x, y + flow.y ) );
        }
    }

    /* Undistort the points based on intrinsic params and dist coeff */
    undistortPoints( left_points, left_points, cam_matrix, dist_coeff );
    undistortPoints( right_points, right_points, cam_matrix, dist_coeff );

    /* Try to find essential matrix from the points */
    Mat fundamental = findFundamentalMat( left_points, right_points, FM_RANSAC, 3.0, 0.99 );
    Mat essential   = cam_matrix.t() * fundamental * cam_matrix;

    /* Find the projection matrix between those two images */
    SVD svd( essential );
    static const Mat W = (Mat_(3, 3) <<
                         0, -1, 0,
                         1, 0, 0,
                         0, 0, 1);

    static const Mat W_inv = W.inv();

    Mat_ R1 = svd.u * W * svd.vt;
    Mat_ T1 = svd.u.col( 2 );

    Mat_ R2 = svd.u * W_inv * svd.vt;
    Mat_ T2 = -svd.u.col( 2 );

    static const Mat P1 = Mat::eye(3, 4, CV_64FC1 );
    Mat P2 =( Mat_(3, 4) <<
             R1(0, 0), R1(0, 1), R1(0, 2), T1(0),
             R1(1, 0), R1(1, 1), R1(1, 2), T1(1),
             R1(2, 0), R1(2, 1), R1(2, 2), T1(2));

    /*  Triangulate the points to find the 3D homogenous points in the world space
        Note that each column of the 'out' matrix corresponds to the 3d homogenous point
     */
    Mat out;
    triangulatePoints( P1, P2, left_points, right_points, out );

    /* Since it's homogenous (x, y, z, w) coord, divide by w to get (x, y, z, 1) */
    vector splitted = {
        out.row(0) / out.row(3),
        out.row(1) / out.row(3),
        out.row(2) / out.row(3)
    };

    merge( splitted, out );

    return out;
}

 

Here’s a pair of images used in our example: Screen Shot 2014-03-26 at 12.32.42

And here’s the resulting video, notice that how the 3d model has 2 tails. This is partly because I made the optical flow’s search window too small, while the rotational changes between image is too large, thus the optical flow failed to recognise that the tail has been moved :

The 3D dataset images along with calibration parameters used in our example can be found here: http://www-cvr.ai.uiuc.edu/ponce_grp/data/mview/

You can get the source code of this example over here (mainly in main.cpp): https://github.com/subokita/Sandbox/tree/master/SFM/SFM.

Please note that the example requires OpenCV, glm, glfw, and maybe FreeImage. The glv library is just a simple OpenGL helper (viewer, basic shapes, arc ball) that I created for rapid prototyping, please don’t use it for production stuff, very buggy and might be wrong

Advertisements

One thought on “Structure From Motion using Farneback’s Optical Flow Part 2

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s