import numpy as np

import cv2
import os
import glob
from tqdm import tqdm
import math
from numpy import matrix
from numpy import linalg
"""def check_path(path): #function to confirm whether the given path
exists or not
dir = os.path.dirname(path) #if it doesn't exist this function will create
if not os.path.exists(dir):
os.makedirs(dir)
cap = cv2.VideoCapture(0)
camera=cv2.VideoCapture(1)
anh = 1
dem = 1
face_id = 1 # For each person,there will be one face id
count = 0 # Initialize sample face image

check_path("dataset/")
check_path("dataset1/")
# Define the codec and create VideoWriter object.The output is stored in
'outpy.avi' file.
while(cap.isOpened()):
ret, frame = cap.read()
ret1,frame1=camera.read()
if ret==True:
count += 1
cv2.imwrite("dataset/User." + str(face_id) + '.' + str(count) + ".jpg",
frame)
cv2.imshow('Creating Dataset!!!', frame)
cv2.waitKey(1000)
if ret1 == True:
dem += 1
cv2.imwrite("dataset1/User." + str(anh) + '.' + str(dem) + ".jpg", frame1)
cv2.imshow('Creating Dataset 1!!!', frame1)
cv2.waitKey(1000)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
if (count > 29) and (dem>29): # If image taken reach 100, stop taking video
break
"""
img_mask = 'dataset/*.jpg'
img_names = glob.glob(img_mask)
img_mask1 = 'dataset1/*.jpg'
img_names1 = glob.glob(img_mask1)
pattern_size = (9, 6)

pattern_points = np.zeros((np.prod(pattern_size), 3), np.float32)

pattern_points[:, :2] = np.mgrid[0:pattern_size[0],0:pattern_size[1]].T.reshape(-1,
2)
pattern_points1 = np.zeros((np.prod(pattern_size), 3), np.float32)
pattern_points1[:, :2] = np.mgrid[0:pattern_size[0],0:pattern_size[1]].T.reshape(-
1, 2)
obj_points = []
obj_points1 =[]
img_points = []
img_points1 = []
for fn in tqdm(img_names):
print('processing %s... ' % fn, end='')
img = cv2.imread(fn)
gray= cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
#h, w = img.shape[:2]
#cv2.imshow('hinh',gray)
found, corners = cv2.findChessboardCorners(gray, pattern_size)
if found:
#dem +=1
term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1)
obj_points.append(pattern_points)
corners2=cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), term)
img_points.append(corners2.reshape(-1, 2))
else:
#print('chessboard not found')
continue
for fn1 in tqdm(img_names1):
print('processing %s... ' % fn, end='')
img1 = cv2.imread(fn1)
gray1= cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
#h, w = img.shape[:2]
#cv2.imshow('hinh',gray)
found1, corners1 = cv2.findChessboardCorners(gray1, pattern_size)
if found1:
#dem +=1
term1 = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1)
obj_points1.append(pattern_points)
corners3=cv2.cornerSubPix(gray1, corners1, (5, 5), (-1, -1), term1)
img_points1.append(corners3.reshape(-1, 2))
else:
#print('chessboard not found')
continue
rt, M1, d1, r1, t1 = cv2.calibrateCamera(obj_points, img_points, gray.shape[::-1],
None, None)
rt, M2, d2, r2, t2 = cv2.calibrateCamera(obj_points1, img_points1, gray1.shape[::-
1], None, None)

print(M1)
print(M2)
print(d1)
print(d2)
#print(t1)
flags = (cv2.CALIB_FIX_K5 + cv2.CALIB_FIX_K6)
stereocalib_criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 100,
1e-5)
flags = (cv2.CALIB_FIX_PRINCIPAL_POINT | cv2.CALIB_FIX_ASPECT_RATIO |
cv2.CALIB_FIX_FOCAL_LENGTH |
cv2.CALIB_FIX_INTRINSIC | cv2.CALIB_FIX_K3 | cv2.CALIB_FIX_K4 |
cv2.CALIB_FIX_K5 |
cv2.CALIB_FIX_K6)
T = np.zeros((3, 1), dtype=np.float64)
R = np.eye(3, dtype=np.float64)

#stereocalibration_flags = 0
#stereocalibration_flags= cv2.CALIB_FIX_INTRINSIC
#stereocalibration_flags= cv2.CALIB_USE_INTRINSIC_GUESS
#stereocalibration_flags= cv2.CALIB_FIX_FOCAL_LENGTH
#stereocalibration_flags= cv2.CALIB_ZERO_TANGENT_DIST
ret, M_1, d_1, M_2, d_2, R, T, E, F = cv2.stereoCalibrate(obj_points,
img_points,img_points1, M1, d1, M2,d2, gray1.shape[::-
1],criteria=stereocalib_criteria, flags=flags)

#print('Intrinsic_mtx_1', M_1)
#print('dist_1', d_1)
#print('Intrinsic_mtx_2', M_2)
#print('dist_2', d_2)
#print('R', R)
#print('T', T)
#print('E', E)
#print('F', F)

#camera_model = dict([('M1', M1), ('M_2', M_2), ('dist1', d_1),

# ('dist2', d_2), ('rvecs1', r1),
# ('rvecs2', r2), ('R', R), ('T', T),
# ('E', E), ('F', F)])
#print(camera_model)