Merge branch 'cvFindFundamentalMat'

DEPENDS.txt

@@ -17,6 +17,9 @@ functionality is only available with the following installed:
 `Open CV <http://opencv.willowgarage.com/wiki/>`_ (version 2.1). 
    Required for functions in ``scikits.image.opencv``.
 
+   Note that, under Ubuntu, due to problems with Atlas + OpenCV,
+   you may have to rebuild your own libcv.
+
 `PyQt4 <http://wiki.python.org/moin/PyQt>`_
    The `qt` plugin that provides `imshow` and `scivy`.
 

scikits/image/opencv/opencv_constants.py

@@ -147,6 +147,14 @@ CV_CALIB_CB_ADAPTIVE_THRESH = 1
 CV_CALIB_CB_NORMALIZE_IMAGE = 2
 CV_CALIB_CB_FILTER_QUADS = 4
 
+################################
+# Fundamental Matrix Constants #
+################################
+CV_FM_7POINT = 1
+CV_FM_8POINT = 2
+CV_FM_LMEDS = 4
+CV_FM_RANSAC = 8
+
 ####################
 # cvMat TypeValues #
 ####################

scikits/image/opencv/opencv_cv.pyx

@@ -294,6 +294,14 @@ c_cvFindExtrinsicCameraParams2 = \
                         (<cvFindExtrinsicCameraParams2Ptr*><size_t>
                          ctypes.addressof(cv.cvFindExtrinsicCameraParams2))[0]
 
+# cvFindFundamentalMat
+ctypedef int (*cvFindFundamentalMatPtr)(CvMat*, CvMat*, CvMat*, int, double,
+                                        double, CvMat*)
+cdef cvFindFundamentalMatPtr c_cvFindFundamentalMat
+c_cvFindFundamentalMat = \
+                       (<cvFindFundamentalMatPtr*><size_t>
+                        ctypes.addressof(cv.cvFindFundamentalMat))[0]
+
 # cvDrawChessboardCorners
 ctypedef void (*cvDrawChessboardCornersPtr)(IplImage*, CvSize, CvPoint2D32f*,
                                             int, int)
@@ -2474,6 +2482,118 @@ def cvFindExtrinsicCameraParams2(object_points, image_points, intrinsic_matrix,
 
     return (rvec, tvec)
 
+#---------------------
+# cvFindFundamentalMat
+#---------------------
+
+@cvdoc(package='cv', group='calibration', doc=\
+'''cvFindFundamentalMat(points1, points2, int method=CV_FM_RANSAC,
+                        double param1=1, double param2=0.99)
+
+Calculates the fundamental matrix from the corresponding points in two images.
+
+Parameters
+----------
+points1 : ndarray, Nx2 or Nx3, dtype=float
+    Points from the first image.
+points2 : ndarray, Nx3 or Nx3, dtype=float
+    Points from the second image (same length as ``points1``).
+method : integer
+    CV_FM_7POINT - use 7-point algorithm (N = 7)
+    CV_FM_8POINT - use 8-point algorithm (N = 8)
+    CV_FM_RANSAC - use RANSAC algorithm (N >= 8)
+    CV_FM_LMEDS - use LMedS algorithm (N >= 8)
+param1 : float
+    In RANSAC, the maximum distance from point to epipolar lines
+    (in pixels) beyond which the point is considered an outlier.
+param2 : float
+    In RANSAC and LMedS, the level of confidence (probability)
+    that the estimated matrix is correct.
+
+Returns
+-------
+fundamental_matrix : ndarray, 3x3 or 3x3x3
+    Fundamental matrix. The 7-point method may return up to three
+    matrices, stored as a 3x3x3 array. If no matrix could be found,
+    None is returned.
+status : ndarray, length N, dtype=bool
+    Currently, this is only a placeholder for future use.
+
+    In future, should indicate whether a data-point is an inlier
+    (True) or outlier (False). Only used by RANSAC and MLedS; other
+    methods set all True.''')
+def cvFindFundamentalMat(points1, points2, int method=CV_FM_RANSAC,
+                         double param1=1, double param2=0.99):
+    validate_array(points1)
+    validate_array(points2)
+
+    assert_ndims(points1, [2])
+    assert_ndims(points2, [2])
+    assert_dtype(points1, [FLOAT32, FLOAT64])
+    assert_dtype(points2, [FLOAT32, FLOAT64])
+
+    if not points1.shape[1] in (2, 3):
+        raise ValueError("Points should be Nx2 or Nx3 arrays")
+
+    if not method in (CV_FM_7POINT, CV_FM_8POINT, CV_FM_RANSAC, CV_FM_LMEDS):
+        raise ValueError("Invalid method specified")
+
+    if not points1.shape[0] == points2.shape[0]:
+        raise ValueError("Points1 and points2 should be of equal length.")
+
+    # allocate the numpy return arrays
+    cdef np.npy_intp fundamental_shape[2]
+
+    if (method == CV_FM_7POINT):
+        fundamental_shape[0] = <np.npy_intp> 9
+    else:
+        fundamental_shape[0] = <np.npy_intp> 3
+    fundamental_shape[1] = <np.npy_intp> 3
+
+    cdef np.ndarray F = new_array(2, fundamental_shape, FLOAT64)
+
+    ## The code snippet below creates the ``status`` matrix
+    ## that may optionally be provided.  I could not get this
+    ## to work with OpenCV 2.1.
+
+    cdef np.npy_intp status_shape[2]
+    status_shape[0] = <np.npy_intp> points1.shape[0]
+    status_shape[1] = <np.npy_intp> 1
+    cdef np.ndarray status = new_array(2, status_shape, UINT8)
+    status.fill(0)
+
+    ## cdef IplImage status_img
+    ## populate_iplimage(status, &status_img)
+    ## cdef CvMat* cvstatus = cvmat_ptr_from_iplimage(&status_img)
+
+    # Allocate cv images
+    cdef IplImage points1_img
+    cdef IplImage points2_img
+    cdef IplImage F_img
+
+    populate_iplimage(points1, &points1_img)
+    populate_iplimage(points2, &points2_img)
+    populate_iplimage(F, &F_img)
+
+    # Allocate cv matrices
+    cdef CvMat* cvpoints1 = cvmat_ptr_from_iplimage(&points1_img)
+    cdef CvMat* cvpoints2 = cvmat_ptr_from_iplimage(&points2_img)
+    cdef CvMat* cvF = cvmat_ptr_from_iplimage(&F_img)
+
+    cdef int m = c_cvFindFundamentalMat(cvpoints1, cvpoints2, cvF, method,
+                                        param1, param2, NULL)
+
+    PyMem_Free(cvpoints1)
+    PyMem_Free(cvpoints2)
+    PyMem_Free(cvF)
+    # PyMem_Free(cvstatus)
+
+    if m == 0:
+        return (None, status)
+    else:
+        return (F.reshape((m, 3, 3)).squeeze(), status)
+
+
 #------------------------
 # cvFindChessboardCorners
 #------------------------

scikits/image/opencv/tests/test_opencv_cv.py

@@ -320,7 +320,89 @@ class TestUndistort2(OpenCVTest):
         assert_array_almost_equal(undistg, self.lena_GRAY_U8)
 
 
+@opencv_skip
+def test_cvFindFundamentalMat():
+    #
+    #  c2--->*        * = Data Cloud
+    #        ^
+    #        |           ^ z-direction
+    #        c1       <--|
+    #                 x
+    #
+    # Experimental setup: camera 1 at the origin, random cube data set in front,
+    # camera two watching from the side (position [10, 0, 10])
 
+    # Set up projection matrices
+
+    def build_proj_mat(K, R, C):
+        """
+        Construct a projection matrix.
+
+        Parameters
+        ----------
+        K : ndarray, 3x3
+            Camera matrix, intrinsic parameters.
+        R : ndarray, 3x3
+            Rotation, world to camera.
+        C : ndarray, (3,)
+            Location of camera center in world coordinates.
+
+        """
+        C = np.reshape(C, (3, 1))
+
+        KR = np.dot(K, R)
+        P = np.zeros((3, 4))
+        P[:3, :3] = KR
+        P[:, 3].flat = np.dot(KR, -C)
+
+        return P
+
+    def cross_matrix(v):
+        a = v[0]
+        b = v[1]
+        c = v[2]
+
+        return np.array([[ 0, -c,  b],
+                         [ c,  0, -a],
+                         [-b,  a,  0]])
+
+    # Camera one, at origin of world coordinates, looking down the z-axis
+    K = np.array([[100., 0,   100],
+                  [0,    100, 100],
+                  [0,    0,   1]])
+    R = np.eye(3)
+    C = np.zeros((3,))
+    P = build_proj_mat(K, R, C)
+
+    # Camera two
+    K_ = K
+    R_ = np.array([[0., 0, -1],
+                   [0,  1,  0],
+                   [1,  0,  0]]) # Rotation of 90 degrees around y-axis
+    C_ = np.array([[10., 0, 10]]).T
+    P_ = build_proj_mat(K_, R_, C_)
+
+    data = np.random.random((100, 4)) * 5 - 2.5
+    data[:, 2] += 10 # Offset data in the z direction
+    data[:, 3] = 1 # 4D homogeneous version of 3D coords
+
+    points1 = np.dot(data, P.T)
+    points2 = np.dot(data, P_.T)
+
+    # See Hartley & Zisserman, Multiple View Geometry (2nd ed), p. 244
+    t = -np.dot(R_, C_)
+    K_t = np.dot(K_, t)
+
+    # Under numpy >= 1.5, this would be:
+    #F = cross_matrix(K_t).dot(K_).dot(R).dot(np.linalg.inv(K))
+
+    F = np.dot(np.dot(np.dot(cross_matrix(K_t), K_), R_), np.linalg.inv(K))
+    F /= F[2, 2]
+
+    F_est, status = cvFindFundamentalMat(points1, points2)
+
+    # Compare
+    assert_array_almost_equal(F, F_est)
 
 if __name__ == '__main__':
     run_module_suite()