Search code examples
pythonopencvcomputer-visionstereo-3d

Python OpenCV stereo camera position

I'd like to determine the position and orientation of a stereo camera relative to its previous position in world coordinates. I'm using a bumblebee XB3 camera and the motion between stereo pairs is on the order of a couple feet.

Would this be on the correct track?

  1. Obtain rectified image for each pair
  2. Detect/match feature points rectified images
  3. Compute Fundamental Matrix
  4. Compute Essential Matrix

Thanks for any help!

Solution

  • Well, it sounds like you have a fair understanding of what you want to do! Having a pre-calibrated stereo camera (like the Bumblebee) will then deliver up point-cloud data when you need it - but it also sounds like you basically want to also use the same images to perform visual odometry (certainly the correct term) and provide absolute orientation from a last known GPS position, when the GPS breaks down.

    First things first - I wonder if you've had a look at the literature for some more ideas: As ever, it's often just about knowing what to google for. The whole idea of "sensor fusion" for navigation - especially in built up areas where GPS is lost - has prompted a whole body of research. So perhaps the following (intersecting) areas of research might be helpful to you:

    Issues you are going to encounter with all these methods include:

    • Handling static vs. dynamic scenes (i.e. ones that change purely based on the camera motion - c.f. others that change as a result of independent motion occurring in the scene: trees moving, cars driving past, etc.).
    • Relating amount of visual motion to real-world motion (the other form of "calibration" I referred to - are objects small or far away? This is where the stereo information could prove extremely handy, as we will see...)
    • Factorisation/optimisation of the problem - especially with handling accumulated error along the path of the camera over time and with outlier features (all the tricks of the trade: bundle adjustment, ransac, etc.)

    So, anyway, pragmatically speaking, you want to do this in python (via the OpenCV bindings)?

    If you are using OpenCV 2.4 the (combined C/C++ and Python) new API documentation is here.

    As a starting point I would suggest looking at the following sample:

    /OpenCV-2.4.2/samples/python2/lk_homography.py

    Which provides a nice instance of basic ego-motion estimation from optic flow using the function cv2.findHomography.

    Of course, this homography H only applies if the points are co-planar (i.e. lying on the same plane under the same projective transform - so it'll work on videos of nice flat roads). BUT - by the same principal we could use the Fundamental matrix F to represent motion in epipolar geometry instead. This can be calculated by the very similar function cv2.findFundamentalMat.

    Ultimately, as you correctly specify above in your question, you want the Essential matrix E - since this is the one that operates in actual physical coordinates (not just mapping between pixels along epipoles). I always think of the Fundamental matrix as a generalisation of the Essential matrix by which the (inessential) knowledge of the camera intrinsic calibration (K) is omitted, and vise versa.

    Thus, the relationships can be formally expressed as:

    E =  K'^T F K

    So, you'll need to know something of your stereo camera calibration K after all! See the famous Hartley & Zisserman book for more info.

    You could then, for example, use the function cv2.decomposeProjectionMatrix to decompose the Essential matrix and recover your R orientation and t displacement.

    Hope this helps! One final word of warning: this is by no means a "solved problem" for the complexities of real world data - hence the ongoing research!