#retval, corners = cv2.findChessboardCorners(image,patternSize, flags)

import cv2
import numpy as np

# Define the size of the chessboard
chessboard_size = (8,5)

# Define the object points of the chessboard
object_points = np.zeros((np.prod(chessboard_size), 3), dtype=np.float32)
object_points[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2)

# Create arrays to store the object points and image points from all the images
object_points_array = []
image_points_array1 = []
image_points_array2 = []

# Load the images
images = []
images.append(cv2.imread("/home/ros/Bureau/ca_ur5/1.jpg"))
# Add more images as needed

# Loop through each image and find the chessboard corners
for image in images:
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)

    # Find the chessboard corners
    found, corners = cv2.findChessboardCorners(gray, chessboard_size, None)

    # If the corners are found, add the object points and image points to the arrays
    if found:
        object_points_array.append(object_points)
        image_points_array1.append(corners)

# Calibrate the camera using the object points and image points
ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors = cv2.calibrateCamera(
    object_points_array, image_points_array1, gray.shape[::-1], None, None)

# Print the camera matrix and distortion coefficients
print("Camera matrix:")
print(camera_matrix)
print("Distortion coefficients:")
print(distortion_coefficients)


# Load the images
images = []
images.append(cv2.imread("/home/ros/Bureau/ca_ur5/2.jpg"))
# Add more images as needed

# Loop through each image and find the chessboard corners
for image in images:
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)

    # Find the chessboard corners
    found, corners = cv2.findChessboardCorners(gray, chessboard_size, None)

    # If the corners are found, add the object points and image points to the arrays
    if found:
        object_points_array.append(object_points)
        image_points_array2.append(corners)

# Calibrate the camera using the object points and image points
ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors = cv2.calibrateCamera(
    object_points_array, image_points_array2, gray.shape[::-1], None, None)

# Print the camera matrix and distortion coefficients
print("Camera matrix:")
print(camera_matrix)
print("Distortion coefficients:")
print(distortion_coefficients)

print("Stereo calib")

flags = 0
flags |= cv2.CALIB_FIX_INTRINSIC
# Here we fix the intrinsic camara matrixes so that only Rot, Trns, Emat and Fmat are calculated.
# Hence intrinsic parameters are the same 
 
criteria_stereo= (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
 
 
# This step is performed to transformation between the two cameras and calculate Essential and Fundamenatl matrix
retS, new_mtxL, distL, new_mtxR, distR, Rot, Trns, Emat, Fmat = cv2.stereoCalibrate(object_points_array, image_points_array1, image_points_array2, new_mtxL, distL, new_mtxR, distR, imgL_gray.shape[::-1], criteria_stereo, flags)