Commit 2ab28288 authored by Jay's avatar Jay Committed by Jason R Laura
Browse files

Added MLE estimation of F and necessary support camera functionality.

parent 3fa912f6

autocnet/camera/__init__.py

0 → 100644
+0 −0

Empty file added.

autocnet/camera/camera.py

0 → 100644
+163 −0
Original line number Diff line number Diff line 
import numpy as np

import pandas as pd





def compute_epipoles(f):

    """

    Compute the epipole and epipolar prime



    Parameters

    ----------

    f : ndarray

        (3,3) fundamental matrix or autocnet Fundamental Matrix object



    Returns

    -------

    e : ndarray

        (3,1) epipole



    e1 : ndarray

         (3,3) epipolar prime matrix

    """

    u, _, _ = np.linalg.svd(f)

    e = u[:, -1]

    e1 = np.array([[0, -e[2], e[1]],

                   [e[2], 0, -e[0]],

                   [-e[1], e[0], 0]])



    return e, e1





def idealized_camera():

    """

    Create an idealized camera transformation matrix



    Returns

    -------

     : ndarray

       (3,4) with diagonal 1

    """

    return np.eye(3, 4)





def estimated_camera_from_f(f):

    """

    Estimate a camera matrix using a fundamental matrix.





    Parameters

    ----------

    f : ndarray

        (3,3) fundamental matrix or autocnet Fundamental Matrix object



    Returns

    -------

    p1 : ndarray

         Estimated camera matrix

    """



    e, e1 = compute_epipoles(f)

    p1 = np.empty((3, 4))

    p1[:, :3] = e1.dot(f)

    p1[:, 3] = e



    return p1





def triangulate(pt, pt1, p, p1):

    """

    Given two sets of homogeneous coordinates and two camera matrices,

    triangulate the 3D coordinates.  The image correspondences are

    assumed to be implicitly ordered.



    References

    ----------

    .. [Hartley2003]



    Parameters

    ----------

    pt : ndarray

         (n, 3) array of homogeneous correspondences



    pt1 : ndarray

          (n, 3) array of homogeneous correspondences



    p : ndarray

        (3, 4) camera matrix



    p1 : ndarray

         (3, 4) camera matrix



    Returns

    -------

    coords : ndarray

             (n, 4) array of triangulated coordinates in the form (x, y, z, a)



    """

    npts = len(pt)

    a = np.zeros((4, 4))

    coords = np.empty((npts, 4))



    if isinstance(pt, pd.DataFrame):

        pt = pt.values

    if isinstance(pt1, pd.DataFrame):

        pt1 = pt.values



    for i in range(npts):

        # Compute AX = 0

        a[0] = pt[i][0] * p[2] - p[0]

        a[1] = pt[i][1] * p[2] - p[1]

        a[2] = pt1[i][0] * p1[2] - p1[0]

        a[3] = pt1[i][1] * p1[2] - p1[1]

        # v.T is a least squares solution that minimizes the error residual

        u, s, vh = np.linalg.svd(a)

        v = vh.T

        coords[i] = v[:, 3] / (v[:, 3][-1])

    return coords.T





def projection_error(p1, p, pt, pt1):

    """

    Based on Hartley and Zisserman p.285 this function triangulates

    image correspondences and computes the reprojection error

    by back-projecting the points into the image.



    References

    ----------

    .. [Hartley2003]



    Parameters

    -----------

    p1 : ndarray

         (3,4) camera matrix



    p : ndarray

        (3,4) idealized camera matrix in the form np.eye(3,4)



    pt : dataframe or ndarray

         of homogeneous coordinates in the form (x_{i}, y_{i}, 1)



    pt1 : dataframe or ndarray

          of homogeneous coordinates in the form (x_{i}, y_{i}, 1)



    Returns

    -------

    reproj_error : ndarray

                   (n, 1) residuals for each correspondence



    """

    if p1.shape != (3,4):

        p1 = p1.reshape(3,4)

    # Triangulate the correspondences

    xw_est = triangulate(pt, pt1, p, p1)



    xhat = p.dot(xw_est).T

    xhat /= xhat[:, -1][:, np.newaxis]

    x2hat = p1.dot(xw_est).T

    x2hat /= x2hat[:, -1][:, np.newaxis]



    # Compute residuals

    dist = (pt - xhat)**2 + (pt1 - x2hat)**2

    reproj_error = np.sqrt(np.sum(dist, axis=1) / len(pt))



    return reproj_error

autocnet/camera/tests/__init__.py

0 → 100644
+0 −0

Empty file added.

autocnet/camera/tests/test_camera.py

0 → 100644
+65 −0
Original line number Diff line number Diff line 
import os

import sys

import unittest



import numpy as np



sys.path.append(os.path.abspath('..'))



from .. import camera





class TestCamera(unittest.TestCase):



    @classmethod

    def setUpClass(cls):

        cls.f = np.array([[3.27429392e-07, 8.31555374e-06, -1.37175583e-05],

                          [2.33349759e-07, 5.99315297e-07,  1.15196332e-01],

                          [-4.78065797e-03, -1.21448858e-01, 1.00000000e+00]])



    def test_compute_epipoles(self):

        e, e1 = camera.compute_epipoles(self.f)

        np.testing.assert_array_almost_equal(e, np.array([9.99999885e-01, -4.75272787e-04, 6.84672384e-05]))



        np.testing.assert_array_almost_equal(e1, np.array([[0.00000000e+00,  -6.84672384e-05,  -4.75272787e-04],

                                                    [6.84672384e-05,   0.00000000e+00,  -9.99999885e-01],

                                                    [4.75272787e-04,   9.99999885e-01,   0.00000000e+00]]))



    def test_idealized_camera(self):

        np.testing.assert_array_equal(np.eye(3,4), camera.idealized_camera())



    def test_estimated_camera_from_f(self):

        p1 = camera.estimated_camera_from_f(self.f)

        np.testing.assert_array_almost_equal(p1, np.array([[2.27210066e-06, 5.77212964e-05, -4.83159962e-04, 9.99999885e-01],

                                                    [4.78065745e-03,   1.21448845e-01,  -9.99999886e-01, -4.75272787e-04],

                                                    [2.33505351e-07,   6.03267385e-07,   1.15196312e-01, 6.84672384e-05]]))



    def test_triangulation_and_reprojection_error(self):

        p = np.eye(3,4)

        p1 = np.array([[2.27210066e-06, 5.77212964e-05, -4.83159962e-04, 9.99999885e-01],

                       [4.78065745e-03,   1.21448845e-01,  -9.99999886e-01, -4.75272787e-04],

                       [2.33505351e-07,   6.03267385e-07,   1.15196312e-01, 6.84672384e-05]])



        coords1 = np.array([[260.12573242, 6.37760448, 1.],

                            [539.05926514, 7.0553031 , 1.],

                            [465.07751465, 16.02966881, 1.],

                            [46.39139938, 16.96884346, 1.],

                            [456.28939819, 23.12134743, 1.]])



        coords2 = np.array([[707.23968506, 8.2479744, 1.],

                            [971.61566162, 18.7211895, 1.],

                            [905.67974854, 25.06698608, 1.],

                            [487.46420288, 11.28651524, 1.],

                            [897.73425293, 32.06435013, 1.]])



        truth = np.array([[3.03655751, 4.49076221, 4.17705934, 0.7984432 , 4.13673958],

                          [0.07447866, 0.05871216, 0.14393111, 0.29223435, 0.20962208],

                          [0.01167342, 0.00833074, 0.00898143, 0.01721084, 0.00906604],

                          [1., 1., 1., 1., 1. ]])



        c = camera.triangulate(coords1, coords2, p, p1)

        np.testing.assert_array_almost_equal(c, truth)



        truth = np.array([ 0.078723,  0.228164,  0.127505,  0.333851,  0.009718])

        c = camera.projection_error(p1, p, coords1, coords2)

        np.testing.assert_array_almost_equal(c, truth)
 
 No newline at end of file

autocnet/graph/edge.py

+5 −3
Original line number Diff line number Diff line
@@ -102,7 +102,7 @@ class Edge(dict, MutableMapping): 
        else:

            raise AttributeError('No matches have been computed for this edge.')



    def compute_fundamental_matrix(self, clean_keys=[], **kwargs):

    def compute_fundamental_matrix(self, clean_keys=[], method='linear', **kwargs):



        if hasattr(self, 'matches'):

            matches = self.matches
@@ -127,11 +127,13 @@ class Edge(dict, MutableMapping): 


        # Convert the truncated RANSAC mask back into a full length mask

        mask[mask] = fundam_mask



        # Pass in the truncated mask to the fundamental matrix.  These are

        # only those points that have pased previous outlier checks

        self.fundamental_matrix = FundamentalMatrix(transformation_matrix,

                                                    s_keypoints,

                                                    d_keypoints,

                                                    mask=mask)

                                                    mask=mask,

                                                    local_mask=fundam_mask)



        # Subscribe the health watcher to the fundamental matrix observable

        self.fundamental_matrix.subscribe(self._health.update)