Added estimMDLT to estimate modified DLT coefficient (higher accuracy and...

Added estimMDLT to estimate modified DLT coefficient (higher accuracy and orthogonal matrix -> needed to estimate quat)
+ Various changes to calib.py and debugging of related test functions

README.md

@@ -124,6 +124,9 @@ Initial body orientation is along x axis. Yaw, pitch and roll are then computed
 ### Post-processing of the kinematics parameters
 ??? `img_process.???`
 
+??? Document how filtering/smoothing is done by default and how to modify it + implement filtering of outlier (example of Cas data)
+
+
 ## Notes
 ### Running the tests
 To run tests locally, type:

camera/cameras.py

@@ -250,7 +250,8 @@ class Camera(Object3D):
     @dlt_coef.setter
     def dlt_coef(self, dlt_coef) -> None:
         self.xp, self.yp, self.fx, self.fy = calib.intrinsic_from_dlt_coef(dlt_coef)
-        self.coords, self.angles = calib.extrinsic_from_dlt_coef(dlt_coef, in_degrees=self.in_degrees)
+        self.coords, self.angles = \
+            calib.extrinsic_from_dlt_coef(dlt_coef, (self.xp, self.yp, self.fx, self.fy), in_degrees=self.in_degrees)
 
     @staticmethod
     def from_dlt_coef(dlt_coef: List[float], in_degrees: bool = True) -> "Camera":
@@ -266,7 +267,7 @@ class Camera(Object3D):
         """
 
         xp, yp, fx, fy = calib.intrinsic_from_dlt_coef(dlt_coef)
-        coords, angles = calib.extrinsic_from_dlt_coef(dlt_coef, in_degrees=in_degrees)
+        coords, angles = calib.extrinsic_from_dlt_coef(dlt_coef, (xp, yp, fx, fy), in_degrees=in_degrees)
 
         X, Y, Z = tuple(coords)
         alpha, beta, gamma = tuple(angles)

camera/utils.py

@@ -68,6 +68,42 @@ def to_homogeneus(X: np.ndarray) -> np.ndarray:
     return np.hstack([X, 1])
 
 
+def is_orthogonal(M, return_error: bool = False):
+    """ Check if matrix is orthogonal
+
+    Args:
+        M: a square matrix (NxN)
+        return_error: Whether to return the rms error for orthogonality
+
+    Return:
+        Whether the matrix is found to be orthogonal
+         OR
+        The rms error for orthogonality
+    """
+
+    assert all(len(row) == len(M) for row in M), "Matrix need to be square (NxN)"
+
+    if not return_error:
+        rtol = 1e-05
+        atol = 1e-08
+        if not np.allclose(np.dot(M, M.conj().transpose()), np.eye(3), rtol=rtol, atol=atol):
+            # Matrix is not orthogonal (i.e. its transpose should be its inverse)
+            return False
+
+        elif not np.isclose(np.linalg.det(M), 1.0, rtol=rtol, atol=atol):
+            # Matrix is not special orthogonal (i.e. its determinant must be +1.0)
+            return False
+
+        else:
+            return True
+
+    else:
+        eye_diff = np.dot(M, M.conj().transpose()) - np.eye(3)
+        ortho_error = np.sqrt(np.sum(eye_diff ** 2))
+
+        return ortho_error
+
+
 def get_rotation_btw_vectors(vec1: np.ndarray, vec2: np.ndarray = np.array([0., 0., 1.])) -> Rotation:
     """ Get rotation matrix between two vectors using scipy.spatial.transform.Rotation.
 
@@ -231,4 +267,48 @@ def quat0123_to_1230(quat: np.ndarray) -> np.ndarray:
     quat = np.asarray(quat)
     assert len(quat) == 4
 
-    return np.append(quat[1:], quat[0])
\ No newline at end of file
+    return np.append(quat[1:], quat[0])
+
+# TODO remove following code?
+# def find_closest_orthogonal_matrix(A):
+#     '''
+#     Find closest orthogonal matrix to *A* using iterative method.
+#
+#     From: https://gist.github.com/dokato/7a997b2a94a0ec6384a5fd0e91e45f8b
+#     Bases on the code from REMOVE_SOURCE_LEAKAGE function from OSL Matlab package.
+#
+#     Args:
+#         A (numpy.array): array shaped k, n, where k is number of channels, n - data points
+#
+#     Returns:
+#         L (numpy.array): orthogonalized matrix with amplitudes preserved
+#
+#     '''
+#
+#     MAX_ITER = 2000
+#
+#     TOLERANCE = np.max((1, np.max(A.shape) * np.linalg.svd(A.T, False, False)[0])) * np.finfo(A.dtype).eps  # TODO
+#     reldiff = lambda a, b: 2 * abs(a - b) / (abs(a) + abs(b))
+#     convergence = lambda rho, prev_rho: reldiff(rho, prev_rho) <= TOLERANCE
+#
+#     A_b = A.conj()
+#     d = np.sqrt(np.sum(A * A_b, axis=1))
+#
+#     rhos = np.zeros(MAX_ITER)
+#
+#     for i in range(MAX_ITER):
+#         scA = A.T * d
+#
+#         u, s, vh = np.linalg.svd(scA, False)
+#
+#         V = np.dot(u, vh)
+#
+#         d = np.sum(A_b * V.T, axis=1)
+#
+#         L = (V * d).T
+#         E = A - L
+#         rhos[i] = np.sqrt(np.sum(E * E.conj()))
+#         if i > 0 and convergence(rhos[i], rhos[i - 1]):
+#             break
+#
+#     return L

tests/test_calib.py

 import os
 import numpy as np
 
+import matplotlib.pyplot as plt
+
 from camera import calib
 
 
@@ -20,7 +22,7 @@ def test_dlt2d_1views():
     nb_cam = 1
 
     # Camera calibration parameters based on view #1 and error of the calibration of view #1 (in pixels):')
-    dlt_coef1, repro_err1 = calib.calib(nb_dim, xy, uv1)
+    dlt_coef1, repro_err1 = calib.estimDLT(nb_dim, xy, uv1)
 
     assert len(dlt_coef1) == 8
     assert repro_err1 <= max_mean_err_px
@@ -55,8 +57,8 @@ def test_dlt2d_2views():
     nb_cam = 2
 
     # Camera calibration parameters based on view #1 and error of the calibration of view #1 (in pixels)
-    dlt_coef1, repro_err1 = calib.calib(nb_dim, xy, uv1)
-    dlt_coef2, repro_err2 = calib.calib(nb_dim, xy, uv2)
+    dlt_coef1, repro_err1 = calib.estimDLT(nb_dim, xy, uv1)
+    dlt_coef2, repro_err2 = calib.estimDLT(nb_dim, xy, uv2)
 
     assert len(dlt_coef1) == 8
     assert repro_err1 <= max_mean_err_px
@@ -97,10 +99,10 @@ def test_dlt3d():
     nb_cam = 4
 
     # Camera calibration parameters based on view #1 and error of the calibration of view #1 (in pixels)
-    dlt_coef1, repro_err1 = calib.calib(nb_dim, xyz, uv1)
-    dlt_coef2, repro_err2 = calib.calib(nb_dim, xyz, uv2)
-    dlt_coef3, repro_err3 = calib.calib(nb_dim, xyz, uv3)
-    dlt_coef4, repro_err4 = calib.calib(nb_dim, xyz, uv4)
+    dlt_coef1, repro_err1 = calib.estimDLT(nb_dim, xyz, uv1)
+    dlt_coef2, repro_err2 = calib.estimDLT(nb_dim, xyz, uv2)
+    dlt_coef3, repro_err3 = calib.estimDLT(nb_dim, xyz, uv3)
+    dlt_coef4, repro_err4 = calib.estimDLT(nb_dim, xyz, uv4)
 
     assert len(dlt_coef1) == 11
     assert repro_err1 <= max_mean_err_px
@@ -141,22 +143,39 @@ def test_dlt3d():
 
 def test_dlt_coef_to_from_intrinsic_extrinsic() -> None:
 
-    # xyz = [[0, 0, 0], [0, 12.3, 0], [14.5, 12.3, 0], [14.5, 0, 0], [0, 0, 14.5], [0, 12.3, 14.5], [14.5, 12.3, 14.5],
-    #        [14.5, 0, 14.5]]
-    # uv = [[1302, 1147], [1110, 976], [1411, 863], [1618, 1012], [1324, 812], [1127, 658], [1433, 564], [1645, 704]]
-    #
-    # # Camera calibration parameters (DLT coefficients)
-    # dlt_coef, repro_err = calib.calib(3, xyz, uv)
+    xyz = [[0, 0, 0], [0, 12.3, 0], [14.5, 12.3, 0], [14.5, 0, 0], [0, 0, 14.5], [0, 12.3, 14.5], [14.5, 12.3, 14.5], [14.5, 0, 14.5]]
+    uv = [[1302, 1147], [1110, 976], [1411, 863], [1618, 1012], [1324, 812], [1127, 658], [1433, 564], [1645, 704]]
+
+    show_plot = True
+    decimal = -1
+
+    # Camera calibration parameters (DLT coefficients)
+    #dlt_coef, repro_err = calib.estimDLT(3, xyz, uv)  # Will no produce dlt_coef from an orthogonal matrix
+    dlt_coef, repro_err = calib.estimMDLT(xyz, uv, show_plot=show_plot)
 
-    dlt_coefs = calib.read_dlt_coefs('../data/mosquito_escapes/calib/to_delete/20200615_DLTcoefs-py.csv')
-    dlt_coef = dlt_coefs[0]
+    # dlt_coefs = calib.read_dlt_coefs('../data/mosquito_escapes/calib/20200306_DLTcoefs-py.csv')
+    # dlt_coef = dlt_coefs[0]
 
     xp, yp, fx, fy = calib.intrinsic_from_dlt_coef(dlt_coef)
-    coords, angles = calib.extrinsic_from_dlt_coef(dlt_coef)
+    coords, quat = calib.extrinsic_from_dlt_coef(dlt_coef, (xp, yp, fx, fy))
+
+    new_dlt_coef = calib.dlt_coef_from_intrinsic_extrinsic(xp, yp, fx, fy, coords, quat)
+
+    uv2 = calib.find2d(1, new_dlt_coef, xyz)
+
+    if show_plot:
+        fig = plt.figure()
+        ax = fig.add_subplot(111)
+        for i in range(0, len(uv)):
+            ax.scatter(uv[i][0], uv[i][1], marker='+', color='g', s=40)
+            ax.scatter(uv2[i][0], uv2[i][1], marker='+', color='r', s=40)
+        ax.set_xlabel('x [m]')
+        ax.set_ylabel('y [m]')
+        plt.show()
 
-    new_dlt_coef = calib.dlt_coef_from_intrinsic_extrinsic(xp, yp, fx, fy, coords, angles)
+    np.testing.assert_almost_equal(uv, uv2, decimal=decimal)
 
-    assert all(dlt_coef == new_dlt_coef)
+    #assert all(dlt_coef == new_dlt_coef)
 
 
 def test_gen_dlt_coefs_and_save_in_csv() -> None:

tests/test_camera.py

@@ -15,7 +15,15 @@ def test_pinhole_cameras():
 
     in_degrees = True
     norm_vect_init = [0., 0., 1.]
+
+    # Without noize
+    sigma = 0
     max_mean_err_px = 10e-5
+    decimal = 4
+    # # With noize (std = 2 pixels)
+    # sigma = 1
+    # max_mean_err_px = 3
+    # decimal = -1
 
     pixel_size = 10e-5
 
@@ -57,6 +65,7 @@ def test_pinhole_cameras():
     # Generate pixel coordinates of a set of 3d points for each camera
     xyz = list(itertools.combinations_with_replacement([-0.45, -0.23, 0, 0.23, 0.45], 3))
     xyz = [list(coord) for coord in xyz]
+    # xyz = [xyz[i] for i in np.random.choice(len(xyz), 6)]  # To reduce number of 3d points to min needed for dlt_coefs
 
     if show_plot:
         fig = plt.figure()
@@ -71,7 +80,7 @@ def test_pinhole_cameras():
         ax.set_zlabel('z [m]')
         plt.show()
 
-    dlt_coefs = []
+    dlt_coefs, dlt_coefs2 = [], []
     for cam in cams.cameras:
         quat_back_to_init = cam.quat.inverse
 
@@ -113,19 +122,23 @@ def test_pinhole_cameras():
         # pixel_size = np.amax(uv) / np.min(cam.sensor_size)) / 2
         uv = np.array(uv) / pixel_size + cam.sensor_size / 2  # scale meters to pixels
 
+        # Add noize to 2d coordinates
+        uv = uv + np.reshape(np.random.normal(0, sigma, len(uv)*2), [len(uv), 2])
+
         # --------------------------------------------------------------------------------------------------------------
         # Generate DLT coefficients for each camera
-        dlt_coef, repro_err = calib.calib(3, xyz, uv)
+        #dlt_coef, repro_err = calib.estimDLT(3, xyz, uv)  # Will no produce dlt_coef from an orthogonal matrix
+        dlt_coef, repro_err = calib.estimMDLT(xyz, uv, show_plot=show_plot)
         assert repro_err <= max_mean_err_px
         dlt_coefs.append(dlt_coef)
 
         # --------------------------------------------------------------------------------------------------------------
         # Test dlt_coef_from_intrinsic_extrinsic
-        # Here I use minus fx and fy because the image is upside down
-        dlt_coef2 = calib.dlt_coef_from_intrinsic_extrinsic(cam.xp, cam.yp, - cam.fx, - cam.fy,
+        dlt_coef2 = calib.dlt_coef_from_intrinsic_extrinsic(cam.xp, cam.yp, cam.fx, cam.fy,
                                                             cam.coords, cam.quat, in_degrees=in_degrees)
 
         uv2 = calib.find2d(1, dlt_coef2, xyz)
+        dlt_coefs2.append(dlt_coef2)
 
         if show_plot:
             fig = plt.figure()
@@ -137,7 +150,7 @@ def test_pinhole_cameras():
             ax.set_ylabel('y [m]')
             plt.show()
 
-        np.testing.assert_almost_equal(uv, uv2, decimal=4)
+        np.testing.assert_almost_equal(uv, uv2, decimal=decimal)
 
     # ------------------------------------------------------------------------------------------------------------------
     # Generate intrinsic and extrinsic parameters from DLT coefficients
@@ -146,12 +159,23 @@ def test_pinhole_cameras():
     for i, dlt_coef in enumerate(dlt_coefs):
         cam = cams.get(i)
 
+        # --------------------------------------------------------------------------------------------------------------
+        # Estimate intrinsic parameters
+        xp, yp, fx, fy = calib.intrinsic_from_dlt_coef(dlt_coef)
+
+        np.testing.assert_almost_equal(cam.xp, xp, decimal=decimal)
+        np.testing.assert_almost_equal(cam.yp, yp, decimal=decimal)
+        np.testing.assert_almost_equal(cam.fx, fx, decimal=decimal)
+        np.testing.assert_almost_equal(cam.fy, fy, decimal=decimal)
+
+        new_cams.append(cameras.Camera([xp, yp], [fx, fy], coords, quat, in_degrees=in_degrees))
+
         # --------------------------------------------------------------------------------------------------------------
         # Estimate extrinsic parameters
-        coords, quat = calib.extrinsic_from_dlt_coef(dlt_coef)
+        coords, quat = calib.extrinsic_from_dlt_coef(dlt_coef, (xp, yp, fx, fy))
 
-        np.testing.assert_almost_equal(cam.coords, coords)
-        np.testing.assert_almost_equal(cam.quat.rotate(norm_vect_init), quat.rotate(norm_vect_init))
+        np.testing.assert_almost_equal(cam.coords, coords, decimal=decimal)
+        np.testing.assert_almost_equal(cam.quat.rotate(norm_vect_init), quat.rotate(norm_vect_init), decimal=decimal)
 
         if show_plot:
             norm_vect = quat.rotate(norm_vect_init)
@@ -166,17 +190,6 @@ def test_pinhole_cameras():
             ax3.set_zlabel('z [m]')
             plt.show()
 
-        # --------------------------------------------------------------------------------------------------------------
-        # Estimate intrinsic parameters
-        xp, yp, fx, fy = calib.intrinsic_from_dlt_coef(dlt_coef)
-
-        np.testing.assert_almost_equal(cam.xp, xp)
-        np.testing.assert_almost_equal(cam.yp, yp)
-        np.testing.assert_almost_equal(cam.fx, fx)
-        np.testing.assert_almost_equal(cam.fy, fy)
-
-        new_cams.append(cameras.Camera([xp, yp], [fx, fy], coords, quat, in_degrees=in_degrees))
-
     if show_plot:
         fig = plt.figure()
         ax = Axes3D(fig)