camera-flir/distortion_correct.py at master · dregner/camera-flir · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
from include.CameraController import CameraController
import PySpin
import cv2
import numpy as np
import time
import os
import argparse

import yaml


# Load camera calibration parameters from a YAML file
def load_calibration_file(calibration_file):
    with open(calibration_file, 'r') as file:
        calib_data = yaml.safe_load(file)
        camera_matrix = np.array(calib_data['camera_matrix'])
        dist_coeffs = np.array(calib_data['dist_coeffs'])
    return camera_matrix, dist_coeffs


def correct_distortion(frame, camera_matrix, dist_coef):
    newcameramtx, roi = cv2.getOptimalNewCameraMatrix(camera_matrix, dist_coef, (1600, 1200), 1, (1600, 1200))

    undistorted_image = cv2.undistort(frame, camera_matrix, dist_coef, None)
    return undistorted_image


def show_concat_images(left, right, name='Rectified Images'):
    combined_image = np.concatenate((left, right), axis=1)
    # combined_image = cv2.line(combined_image, (0,1460), (8000, 1460), (0, 255, 0))
    # combined_image = cv2.line(combined_image, (0,1431), (8000, 1431), (0, 255, 0))
    cv2.namedWindow(name, cv2.WINDOW_NORMAL)
    cv2.resizeWindow(name, 2560, 1080)
    # cv2.resizeWindow(name, cv2.WND_PROP_FULLSCREEN, cv2.WND_PROP_FULLSCREEN)
    cv2.imshow(name, combined_image)


if __name__ == "__main__":

    print("Distortion")
    cam_ctrl = CameraController(SN=16378749)

    try:
        # Set camera parametersh
        cam_ctrl.set_exposure_time(16660.0)  # Set exposure time to 10000 microseconds
        cam_ctrl.set_exposure_mode(PySpin.ExposureAuto_Continuous)  # Set exposure mode to auto continuous
        cam_ctrl.set_gain(1.0)  # Set gain to 5 dB
        cam_ctrl.set_image_format(PySpin.PixelFormat_Mono8)  # Set image format to Mono8
        pixel_formats = cam_ctrl.get_available_pixel_formats(cam_ctrl.cam)
        cam_ctrl.set_frame_rate(10)
        print(f" Camera Available Pixel Formats: {pixel_formats}")
        print("Seria: {}".format(cam_ctrl.get_model()))
        # Get and print the serial number
        serial_number = cam_ctrl.get_serial_number()
        print(f"Camera Serial Number: {serial_number}")
        cv2.namedWindow('distortion', cv2.WINDOW_NORMAL)
        cv2.resizeWindow('distortion', 2560, 1080)
        # Start acquisition
        cam_ctrl.start_acquisition()
        start_time = time.time()
        frames = 0
        fps = 0
        k = 0
        # Capture synchronized images
        while k != 27:
            k = cv2.waitKey(1)
            image = cam_ctrl.capture_images()
            image = cv2.rotate(image, cv2.ROTATE_180)
            K, kc = load_calibration_file(calibration_file='cfg/fisheye.yaml')
            undistort_image = correct_distortion(image, camera_matrix=K, dist_coef=kc)
            # show_concat_images(image, undistort_image, 'Undistort')
            concat = np.hstack((image, undistort_image))

            cv2.imshow('distortion', concat)
            # cv2.imshow('image', image)
            frames += 1
            if (time.time() - start_time) > 1:
                fps = round(frames / (time.time() - start_time), 1)
                print(f"FPS: {fps}")
                frames = 0
                start_time = time.time()

        # Process images as needed...
    finally:
        # Cleanup resources
        cam_ctrl.stop_acquisition()
        cam_ctrl.cleanup()