From 74ab63fc06bb4c9522872e6f32e00fe7209ae23c Mon Sep 17 00:00:00 2001 From: Satja Sivcev <46567886+SatjaSivcev@users.noreply.github.com> Date: Mon, 22 Nov 2021 08:46:50 +0100 Subject: [PATCH] Add convert_intrinsics_opencv_to_halcon.py This script reads Zivid camera intrinsic parameters (OpenCV model) from .yml file, converts them to internal camera parameters (Halcon model), and save them to .dat file. --- .../convert_intrinsics_opencv_to_halcon.py | 303 ++++++++++++++++++ 1 file changed, 303 insertions(+) create mode 100644 source/python/convert_intrinsics_opencv_to_halcon.py diff --git a/source/python/convert_intrinsics_opencv_to_halcon.py b/source/python/convert_intrinsics_opencv_to_halcon.py new file mode 100644 index 0000000..10640f1 --- /dev/null +++ b/source/python/convert_intrinsics_opencv_to_halcon.py @@ -0,0 +1,303 @@ +""" +Read Zivid camera intrinsic parameters (OpenCV model) from .yml file, convert them to +internal camera parameters (Halcon model), and save them to .dat file. + +Example: python convert_intrinsics_opencv_to_halcon.py --input-file IntrinsicsOpenCV.yml + --model-name "zivid two" --output-file "IntrinsicsHalcon" + +The .dat file can be read with read_cam_par HALCON operator which creates a camera parameter +tuple (CameraParam). Alternatively, gen_cam_par_area_scan_polynomial HALCON procedure can be +used to generate a camera parameter tuple (CameraParam). + +To generate Zivid camera intrinsic parameters (OpenCV model) for your Zivid camera, run +GetCameraIntrinsics.cpp code sample from https://github.com/zivid/zivid-cpp-samples. An +example YML file for this sample can be found under the main instructions for Zivid samples. +""" + +import argparse +from dataclasses import dataclass +from pathlib import Path + +import cv2 +import numpy as np +import yaml +from numpy.typing import NDArray +from scipy.optimize import minimize + + +@dataclass +class CameraIntrinsics: + cx: float + cy: float + fx: float + fy: float + k1: float + k2: float + k3: float + p1: float + p2: float + + def __init__(self, filepath: Path): + data = yaml.safe_load(filepath.read_text(encoding="utf-8")) + self.cx = data["CameraIntrinsics"]["CameraMatrix"]["CX"] + self.cy = data["CameraIntrinsics"]["CameraMatrix"]["CY"] + self.fx = data["CameraIntrinsics"]["CameraMatrix"]["FX"] + self.fy = data["CameraIntrinsics"]["CameraMatrix"]["FY"] + self.k1 = data["CameraIntrinsics"]["Distortion"]["K1"] + self.k2 = data["CameraIntrinsics"]["Distortion"]["K2"] + self.k3 = data["CameraIntrinsics"]["Distortion"]["K3"] + self.p1 = data["CameraIntrinsics"]["Distortion"]["P1"] + self.p2 = data["CameraIntrinsics"]["Distortion"]["P2"] + + def _camera_matrix(self) -> NDArray[np.float64]: + K = np.eye(3) + K[0, 0] = self.fx + K[1, 1] = self.fy + K[0, 2] = self.cx + K[1, 2] = self.cy + return K + + def _distortion_coefficients(self) -> NDArray[np.float64]: + return np.array([self.k1, self.k2, self.p1, self.p2, self.k3]) + + def undistorted_pixels(self, pixels: NDArray) -> NDArray[np.float64]: + undistorted_normalized = cv2.undistortPoints( + pixels, + cameraMatrix=self._camera_matrix(), + distCoeffs=self._distortion_coefficients(), + ) + return undistorted_normalized * np.array([[[self.fx, self.fy]]]) + np.array([[[self.cx, self.cy]]]) + + +@dataclass +class HalconInternalCameraParameters: + cx: float + cy: float + sx: float + sy: float + focus: float + poly: NDArray + image_size: NDArray + + def __init__( + self, + intrinsics: CameraIntrinsics, + pixel_size: NDArray[np.float64], + image_size: NDArray[np.int32], + ): + self.cx = intrinsics.cx + self.cy = intrinsics.cy + self.sx = float(pixel_size) + self.sy = float(pixel_size) + self.focus = float(pixel_size) * intrinsics.fx + self.poly = np.zeros((5)) + self.image_size = image_size + + def undistorted_pixels(self, pixels: NDArray) -> NDArray: + x = (pixels[:, :, 0] - self.cx) * self.sx + y = (pixels[:, :, 1] - self.cy) * self.sy + + r2 = x ** 2 + y ** 2 + r4 = r2 ** 2 + r6 = r2 * r4 + + x_u = ( + x * (1 + self.poly[0] * r2 + self.poly[1] * r4 + self.poly[2] * r6) + + self.poly[3] * (r2 + 2 * x ** 2) + + 2 * self.poly[4] * x * y + ) + y_u = ( + y * (1 + self.poly[0] * r2 + self.poly[1] * r4 + self.poly[2] * r6) + + self.poly[4] * (r2 + 2 * y ** 2) + + 2 * self.poly[3] * x * y + ) + + undistorted_pixels = np.zeros(pixels.shape) + undistorted_pixels[:, :, 0] = x_u / self.sx + self.cx + undistorted_pixels[:, :, 1] = y_u / self.sy + self.cy + return undistorted_pixels + + def _dat_text(self) -> str: + return f"""\ +# INTERNAL CAMERA PARAMETERS + +ParGroup: Camera: Parameter; + "Internal camera parameters"; + +Focus:foc: {self.focus:.06e}; + DOUBLE:0.0:; + "Focal length of the lens [meter]"; + +Poly1:poly1: {self.poly[0]:.06e}; + DOUBLE::; + "1st polynomial distortion coefficient"; + +Poly2:poly2: {self.poly[1]:.06e}; + DOUBLE::; + "2nd polynomial distortion coefficient"; + +Poly3:poly3: {self.poly[2]:.06e}; + DOUBLE::; + "3rd polynomial distortion coefficient"; + +Poly4:poly4: {self.poly[3]:.06e}; + DOUBLE::; + "4th polynomial distortion coefficient"; + +Poly5:poly5: {self.poly[4]:.06e}; + DOUBLE::; + "5th polynomial distortion coefficient"; + +Sx:sx: {self.sx:.06e}; + DOUBLE:0.0:; + "Width of a cell on the sensor"; + +Sy:sy: {self.sx:.06e}; + DOUBLE:0.0:; + "Height of a cell on the sensor"; + +Cx:cx: {self.cx:.06e}; + DOUBLE::; + "X-coordinate of the image center"; + +Cy:cy: {self.cy:.06e}; + DOUBLE::; + "Y-coordinate of the image center"; + +ImageWidth:imgw: {self.image_size[0]}; + INT:1:32768; + "Width of the images"; + +ImageHeight:imgh: {self.image_size[1]}; + INT:1:32768; + "Height of the images";""" + + def write_to_dat_file(self, filepath: Path) -> None: + """Write internal camera parameters (Halcon model) to .dat file. + + Args: + filepath: Path to .dat file to save HALCON internal camera parameters + + Returns None + + Raises: + FileExistsError: File path already exists + """ + if filepath.exists(): + raise FileExistsError(f"{filepath} already exists") + filepath.write_text(self._dat_text(), encoding="utf-8") + + +@dataclass +class CameraParameters: + pixel_size: NDArray[np.float64] + image_size: NDArray[np.int32] + + def __init__(self, model_name: str): + if model_name in [ + "zivid one", + "zivid one plus", + ]: + self.pixel_size = np.array([np.float64(5.86e-6)]) + self.image_size = np.array([np.int32(1920), np.int32(1200)]) + elif model_name == "zivid two": + self.pixel_size = np.array([np.float64(4.50e-6)]) + self.image_size = np.array([np.int32(1944), np.int32(1200)]) + else: + raise Exception(f"{model_name} not a valid model name") + + +def _cost_function( + poly: NDArray, + halcon_parameters: HalconInternalCameraParameters, + intrinsics: CameraIntrinsics, + pixels: NDArray, +) -> float: + halcon_parameters.poly = poly + halcon_pixels_undistorted = halcon_parameters.undistorted_pixels(pixels=pixels) + opencv_pixels_undistorted = intrinsics.undistorted_pixels(pixels=pixels) + return np.sqrt(np.mean(np.linalg.norm(halcon_pixels_undistorted - opencv_pixels_undistorted, axis=2) ** 2)) + + +def _create_pixels( + camera_parameters: CameraParameters, +) -> NDArray: + samples_x = 25 + x = np.linspace(0, camera_parameters.image_size[0] - 1, samples_x) + y = np.linspace( + 0, + camera_parameters.image_size[1] - 1, + round(samples_x * camera_parameters.image_size[1] / camera_parameters.image_size[0]), + ) + xx, yy = np.meshgrid(x, y) + return np.array([[xx.flatten(), yy.flatten()]]).transpose((2, 0, 1)) + + +def _estimate_halcon_internal_camera_parameters_from_opencv( + zivid_yaml_path: Path, model_name: str +) -> HalconInternalCameraParameters: + """Estimate internal camera parameters (Halcon model) from intrinsic parameters (OpenCV model). + + Args: + zivid_yaml_path: Path to Zivid camera intrinsic parameters (OpenCV model) .yml file + model_name: Zivid camera model + + Returns: + Estimated internal camera parameters (Halcon model) + + """ + + intrinsics = CameraIntrinsics(filepath=zivid_yaml_path) + camera_parameters = CameraParameters(model_name=model_name) + halcon_parameters = HalconInternalCameraParameters( + intrinsics=intrinsics, + pixel_size=camera_parameters.pixel_size, + image_size=camera_parameters.image_size, + ) + pixels = _create_pixels(camera_parameters=camera_parameters) + + optimizer_result = minimize( + _cost_function, + np.zeros(5), + args=(halcon_parameters, intrinsics, pixels), + method="nelder-mead", + ) + halcon_parameters.poly = optimizer_result.x + return halcon_parameters + + +def _args() -> argparse.Namespace: + parser = argparse.ArgumentParser() + parser.add_argument( + "--input-file", + type=Path, + required=True, + help="Path to Zivid camera intrinsic parameters (openCV model) as .yml file", + ) + parser.add_argument( + "--model-name", + type=str, + choices=["zivid one", "zivid one plus", "zivid two"], + required=True, + help="Zivid camera model", + ) + parser.add_argument( + "--output-file", + type=Path, + required=True, + help="Path to .dat file to save HALCON internal camera parameters", + ) + return parser.parse_args() + + +def _main(): + args = _args() + halcon_internal_camera_parameters = _estimate_halcon_internal_camera_parameters_from_opencv( + args.input_file, args.model_name + ) + halcon_internal_camera_parameters.write_to_dat_file(args.output_file) + print(f"Halcon internal camera parameters saved to {args.output_file}") + + +if __name__ == "__main__": + _main()