Wrote a test function test_calib_multi_focus to test calibration of one camera...

Wrote a test function test_calib_multi_focus to test calibration of one camera for multiple focus distances and focal lentghs

camera/calib_gimbals.py

-
+import copy
 from dataclasses import dataclass
 
 from typing import List
@@ -6,6 +6,8 @@ from typing import List
 import numpy as np
 from scipy.interpolate import interp1d
 
+from camera import calib
+
 # Code for calibration mirrors mounted on gimbals + virtual cameras
 
 # TODO: Read calibration data (April tag position in 3D vs size and position in pixels vs orientation of mirror (in steps of stepper motors)
@@ -17,23 +19,32 @@ from scipy.interpolate import interp1d
 
 @dataclass
 class CameraCalibMultiFocuses:
-    """ Class for keeping track of the intrinsic parameters of a camera for multiple focus positions.
+    """ Class for keeping track of the intrinsic parameters of a camera for multiple distances of focus plan.
     """
 
-    distances = List[float] = None  # List of distance between camera and multiple focus points
+    def __init__(self, xps: List[float], yps: List[float], fxs: List[float], fys: List[float], distances: List[float]):
+        """
+
+        Args:
+            xps: List of multiple x coordinate of camera principal point (pixels) for each distances of focus plan
+            yps: List of multiple y coordinate of camera principal point (pixels) for each distances of focus plan
+            fxs: List of multiple camera focal length along x (pixels) for each distances of focus plan
+            fys: List of multiple camera focal length along y (pixels) for each distances of focus plan
+            distances: List of distances between camera and focus plan
+        """
+
+        # Coordinates of principal points (if a camera sensor is N*M pixels, xp ~ N/2 and yp ~ M/2
+        self.xps = xps  # type: List[float]
+        self.yps = yps  # type: List[float]
 
-    # ------------------------------------------------------------------------------------------------------------------
-    # Intrinsic parameters:
-    # Coordinates of principal points (if a camera sensor is N*M pixels, xp ~ N/2 and yp ~ M/2
-    xps = List[float] = None  # List of xp for multiple focus points (pixels)
-    yps = List[float] = None  # List of yp
+        # Focal lengths of the camera
+        self.fxs = fxs  # type: List[float]
+        self.fys = fys  # type: List[float]
 
-    # Focal lengths of the camera
-    fxs = List[float] = None  # List of fx for multiple focus points (m)
-    fys = List[float] = None  # List of fy
+        self.distances = distances  # type: List[float]
 
-    assert len(distances) == len(xps) and len(distances) == len(fxs)
-    assert len(xps) == len(yps) and len(fxs) == len(fys)
+        assert len(distances) == len(xps) and len(distances) == len(fxs)
+        assert len(xps) == len(yps) and len(fxs) == len(fys)
 
     def get_intrinsic(self, distance):
         """ Get intrinsic parameters of the camera for a given focus distance.
@@ -64,29 +75,37 @@ class CameraCalibMultiFocuses:
             return fxp(distance), fyp(distance), ffx(distance), ffy(distance)
 
     @staticmethod
-    def from_dict(dict):
-        """Creates a CameraCalibMultiFocuses dataclass
+    def from_dlt_coefs(dlt_coefs: List[List[float]], x_focus: List[float]):
+        """ Creates a CameraCalibMultiFocuses dataclass from
 
         Args:
-            dict: Dictionary to use.
+            dlt_coefs: List of DLT coefficients of one camera for multiple focus distances (nb_distance*11).
+            x_focus: List of x coordinates of the focus plan
 
         Returns:
             CameraCalibMultiFocuses
         """
 
-        return CameraCalibMultiFocuses(**dict)
+        assert len(dlt_coefs) == len(x_focus)
 
-    @staticmethod
-    def from_csv(csv_path):
-        """Creates a CameraCalibMultiFocuses dataclass
+        # Generate intrinsic and extrinsic parameters from dlt_coefs
+        coords, quats = np.zeros((len(x_focus), 3)), np.zeros((len(x_focus), 4))
+        xps, yps, fxs, fys = np.zeros(len(x_focus)), np.zeros(len(x_focus)), np.zeros(len(x_focus)), np.zeros(len(x_focus))
+        for i in range(0, len(x_focus)):
+            xps[i], yps[i], fxs[i], fys[i] = calib.intrinsic_from_dlt_coef(dlt_coefs[i])
+            coords[i], quats[i] = calib.extrinsic_from_dlt_coef(dlt_coefs[i], (xps[i], yps[i], fxs[i], fys[i]))
 
-        Args:
-            csv_path: path to the csv file that contains calibration parameters for multiple focus distances
+        x_cams = [coord[0] for coord in coords]
+        # y_cams = [coord[1] for coord in coords]
+        # z_cams = [coord[2] for coord in coords]
+        #
+        # # To check that camera didn't move during the calibration
+        # np.testing.assert_almost_equal(0.0, np.nanstd(x_cams), decimal=4)
+        # np.testing.assert_almost_equal(0.0, np.nanstd(y_cams), decimal=4)
+        # np.testing.assert_almost_equal(0.0, np.nanstd(z_cams), decimal=4)
 
-        Returns:
-            CameraCalibMultiFocuses
-        """
+        # Generate CameraCalibMultiFocuses class
+        distances = x_focus - x_cams
 
-        dict = np.genfromtxt(csv_path, delimiter=',', skip_header=0, names=True)
+        return CameraCalibMultiFocuses(xps, yps, fxs, fys, distances)
 
-        return CameraCalibMultiFocuses.from_dict(dict)

tests/test_calib_gimbal.py

+import copy
+import itertools
+
+import numpy as np
+from matplotlib import pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+from pyquaternion import Quaternion
+
+from camera import cameras, calib, calib_gimbals
+
+
+def test_calib_multi_focus():
+
+    show_plot = False
+    ind_to_plot = 0  # Index of focus distance to plot
+
+    in_degrees = True
+    norm_vect_init = [0., 0., 1.]
+
+    with_cst_focal_length = False  # Will adjust focal length (zoom in/out) to keep all the 3d points in the full image
+
+    # # 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
+    width_sensor = 0.2
+
+    # Camera #1
+    coords_principal_pt = [1024, 1024]  # So image size is 2024 x 2024 px
+    # distance_camera_to_focus_point / width_field_view_around_focus_point * width_sensor (m)
+    focal_length = 1 / 0.1 * width_sensor / pixel_size  # (pixels) for an angle of view = 5.7°
+    focal_lengths = [focal_length, focal_length]
+    coords = [0, 0, 0]
+    quat = Quaternion(axis=[0, 1, 0], degrees=90)
+
+    cam1 = cameras.Camera(coords_principal_pt, focal_lengths, coords, quat, in_degrees=in_degrees)
+    cam1.sensor_size = np.array(coords_principal_pt) * 2
+
+    # ------------------------------------------------------------------------------------------------------------------
+    # Generate fake calibration (dlt_coefs vs various focus distances)
+    x_range = np.linspace(0.5, 2.5, 10)
+    x_focus_offset = 0.02
+    yz = list(itertools.combinations_with_replacement([-0.2, -0.07, 0, 0.12, 0.2], 2))
+    yz = [list(coord) for coord in yz]
+    # yz = [yz[i] for i in np.random.choice(len(yz), 4)]  # To reduce number of 2d points to min needed for dlt_coefs
+
+    quat_back_to_init = cam1.quat.inverse
+
+    dlt_coefs_vs_focus, repro_errs = [], []
+    for x_focus in x_range:
+
+        if not with_cst_focal_length:
+            # Correct focal length of the camera to fit all the 3d points in angle of view
+            cam1.f = x_focus / np.max(yz) * width_sensor / pixel_size
+
+        xyz = np.array([[np.nan, np.nan, np.nan]])
+        for factor_offset in [-1, 1]:  # At each focus distance, offset plan with yz along x axis (towards/away from cam)
+            x_offsetted = x_focus + factor_offset*x_focus_offset
+
+            xyz = np.append(xyz, [[x_offsetted, coords[0], coords[1]] for coords in yz], axis=0)
+
+        xyz = xyz[1:]
+        if show_plot and x_focus == x_range[ind_to_plot]:
+            fig = plt.figure()
+            ax = Axes3D(fig)
+            for coords in xyz:
+                ax.scatter(coords[0], coords[1], coords[2], marker='*', color='k', s=10)
+            ax.scatter(cam1.X, cam1.Y, cam1.Z, marker='+', color='r', s=40)
+            ax.quiver(cam1.X, cam1.Y, cam1.Z, cam1.norm_vect[0], cam1.norm_vect[1], cam1.norm_vect[2], length=0.2,
+                      color='r')
+            ax.set_xlabel('x [m]')
+            ax.set_ylabel('y [m]')
+            ax.set_zlabel('z [m]')
+
+            fig2 = plt.figure()
+            ax2 = Axes3D(fig2)
+
+        uv = []
+        for coords in xyz:
+            obj = cameras.Object3D(coords, [0, 0, 0], in_degrees=in_degrees)
+            obj.transpose(- cam1.coords)
+            obj.rotate(quat_back_to_init)
+
+            # Coordinates in image plan: with Z axis normal to image plan, need initial normal vector to be [0, 0, 1]
+            cur_uv = - cam1.f / obj.Z * np.array([obj.X, obj.Y])  # (m)
+            cur_uv = cur_uv * pixel_size  # (pixels)
+            # cur_uv = - cur_uv  # rotate image to avoid it being upside down
+            uv.append(cur_uv)
+
+            if show_plot and x_focus == x_range[ind_to_plot]:
+                ax2.scatter(obj.X, obj.Y, obj.Z, marker='*', color='k', s=10)
+                ax2.scatter(cur_uv[0], cur_uv[1], - cam1.f * pixel_size, marker='*', color='g', s=10)
+                ax2.plot([obj.X, cur_uv[0]], [obj.Y, cur_uv[1]], [obj.Z, - cam1.f * pixel_size], marker='',
+                         color='g')
+
+        if show_plot and x_focus == x_range[ind_to_plot]:
+            cam_p = copy.deepcopy(cam1)
+            cam_p.transpose(- cam_p.coords)
+            cam_p.rotate(quat_back_to_init)
+            ax2.scatter(cam_p.X, cam_p.Y, cam_p.Z, marker='+', color='r', s=40)
+            ax2.quiver(cam_p.X, cam_p.Y, cam_p.Z,
+                       cam_p.norm_vect[0], cam_p.norm_vect[1], cam_p.norm_vect[2], length=0.2, color='r')
+            ax2.set_xlabel('x [m]')
+            ax2.set_ylabel('y [m]')
+            ax2.set_zlabel('z [m]')
+            plt.show()
+
+        # --------------------------------------------------------------------------------------------------------------
+        # Convert to pixel coordinates
+        # pixel_size = np.amax(uv) / np.min(cam.sensor_size)) / 2
+        uv = np.array(uv) / pixel_size + cam1.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.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 and x_focus == x_range[ind_to_plot]))
+        assert repro_err <= max_mean_err_px
+        dlt_coefs_vs_focus.append(dlt_coef)
+        repro_errs.append(repro_err)
+
+    cam_calib = calib_gimbals.CameraCalibMultiFocuses.from_dlt_coefs(dlt_coefs_vs_focus, x_range)
+
+    if show_plot:
+        fig = plt.figure()
+        ax = fig.add_subplot(111)
+        ax.plot(repro_errs, marker='+', color='g')
+        ax.set_xlabel('# iteration')
+        ax.set_ylabel('reprojection error')
+        ax.set_yscale('log')
+
+        fig2, axs = plt.subplots(4)
+        fig2.suptitle('Intrinsic parameters vs focus distances')
+        axs[0].plot(x_range, cam_calib.xps)
+        axs[0].set_ylabel('x principal point [px]')
+        axs[1].plot(x_range, cam_calib.yps)
+        axs[1].set_ylabel('y principal point [px]')
+        axs[2].plot(x_range, cam_calib.fxs)
+        axs[3].set_ylabel('x focal length [px]')
+        axs[3].plot(x_range, cam_calib.fys)
+        axs[3].set_xlabel('x focus length [m]')
+        axs[3].set_ylabel('y focal length [px]')
+
+        plt.show()
+
+    # ------------------------------------------------------------------------------------------------------------------
+    # Compare dlt_coef gotten from intrinsic parameters interpolated from calib to coef gotten from uv/xyz
+    distance = 0.86
+
+    xyz = np.array([[np.nan, np.nan, np.nan]])
+    for factor_offset in [-1, 1]:  # At each focus distance, offset plan with yz along x axis (towards/away from cam)
+        x_offsetted = distance + factor_offset * x_focus_offset
+        xyz = np.append(xyz, [[x_offsetted, coords[0], coords[1]] for coords in yz], axis=0)
+
+    xyz = xyz[1:]
+    uv = []
+    for coords in xyz:
+        obj = cameras.Object3D(coords, [0, 0, 0], in_degrees=in_degrees)
+        obj.transpose(- cam1.coords)
+        obj.rotate(quat_back_to_init)
+
+        cur_uv = - cam1.f / obj.Z * np.array([obj.X, obj.Y])  # (m)
+        cur_uv = cur_uv * pixel_size  # (pixels)
+        uv.append(cur_uv)
+
+    uv = np.array(uv) / pixel_size + cam1.sensor_size / 2  # scale meters to pixels
+    uv = uv + np.reshape(np.random.normal(0, sigma, len(uv) * 2), [len(uv), 2])
+    dlt_coef, repro_err = calib.estimMDLT(xyz, uv, show_plot=(show_plot and x_focus == x_range[ind_to_plot]))
+
+    xp, yp, fx, fy = cam_calib.get_intrinsic(distance)
+    dlt_coef2 = calib.dlt_coef_from_intrinsic_extrinsic(xp, yp, fx, fy, cam1.coords, cam1.quat)
+
+    np.testing.assert_almost_equal(dlt_coef, dlt_coef2, decimal=decimal)
+
+
+def test_calib_gimbal_system():
+    # TODO write test functions to test calibration of gimbal system
+
+    assert 1 == 0
+
+
+def test_virtual_cameras():
+    # TODO write test functions to test virtual camera when the mirror is moving, etc ...
+
+    assert 1 == 0

tests/test_camera.py

@@ -28,7 +28,7 @@ def test_pinhole_cameras():
     pixel_size = 10e-5
 
     # Camera #1
-    coords_principal_pt = [1024, 1024]
+    coords_principal_pt = [1024, 1024]  # So image size is 2024 x 2024 px
     # distance_camera_to_focus_point / width_field_view_around_focus_point * width_sensor (m)
     focal_length = 1/0.5 * 0.02 / pixel_size  # (pixels) for an angle of view = 28°
     focal_lengths = [focal_length, focal_length]
@@ -153,7 +153,7 @@ def test_pinhole_cameras():
         np.testing.assert_almost_equal(uv, uv2, decimal=decimal)
 
     # ------------------------------------------------------------------------------------------------------------------
-    # Generate intrinsic and extrinsic parameters from DLT coefficients
+    # re-Generate intrinsic and extrinsic parameters from DLT coefficients
 
     new_cams = []
     for i, dlt_coef in enumerate(dlt_coefs):
@@ -202,11 +202,3 @@ def test_pinhole_cameras():
         ax.set_ylabel('y [m]')
         ax.set_zlabel('z [m]')
         plt.show()
-
-
-def test_virtual_pinhole_cameras():
-    # TODO write test functions to test virtual camera calibration when the mirror is moving, etc ...
-    assert 0 == 1
-
-
-