def load_stereo_calib(filename):
stereo_calib = s_cal.StereoCal()
try:
os.chdir(func.make_path(root_p, c.DATA_DIR, c.STEREO_CALIB_DIR))
stereo_calib.load_params(filename)
os.chdir(root_p)
return stereo_calib
except ValueError as e:
print(e)
return None
def rename():
old = request.args["old"]
new = request.args["new"]
oldPath = funcs.get_file_by_name(old)['path']
newPath = funcs.make_path(new)
print(oldPath, newPath)
if not os.path.exists(newPath):
os.rename(oldPath, newPath)
return redirect(url_for('index'))
def load_calibs():
os.chdir(
func.make_path(root_p, c.DATA_DIR, c.CALIB_DIR, c.ACTIVE_CALIB_DIR))
names = os.listdir()
left_cal = CamCal()
right_cal = CamCal()
for name in names:
if c.LEFT_CALIB_F in name:
left_cal.load_params(name)
elif c.RIGHT_CALIB_F in name:
right_cal.load_params(name)
return left_cal, right_cal
def load_params(self, filename):
try:
os.chdir(
func.make_path(root_p, c.DATA_DIR, c.CALIB_DIR,
c.ACTIVE_CALIB_DIR))
with open(filename, 'rb') as f:
myfile = np.load(f)
print(f"{filename} loaded successfully")
# cam_cal = CamCal(myfile['rms'],myfile['camera_matrix'],myfile['dist_coefs'],myfile['rvecs'],myfile['tvecs'])
self.rms = myfile['rms']
self.camera_matrix = myfile['camera_matrix']
self.dist_coefs = myfile['dist_coefs']
self.rvecs = myfile['rvecs']
self.tvecs = myfile['tvecs']
return True
except OSError:
raise ValueError(f"{filename} does not exist")
def calibrate_stereo_local():
os.chdir(func.make_path(root_p, c.IMG_DIR, c.CALIB_IMG_S_DIR))
img_list = os.listdir()
left_img_data = []
right_img_data = []
size = 0
## -- Load all images in for testing -- ##
for img_name in img_list:
if 'l' in img_name or 'left' in img_name:
img = cv2.imread(img_name, cv2.IMREAD_GRAYSCALE)
frame_n = img_name[-8:-4]
left_img_data.append((frame_n, img))
elif 'r' in img_name or 'right' in img_name:
img = cv2.imread(img_name, cv2.IMREAD_GRAYSCALE)
frame_n = img_name[-8:-4]
right_img_data.append((frame_n, img))
## -- Find chessboards in images -- ##
print('left')
left_chessboards = find_chessboards(left_img_data)
print('right')
right_chessboards = find_chessboards(right_img_data)
left_chessboards, right_chessboards = validate_chessboards(
left_chessboards, right_chessboards)
## -- Load camera data -- ##
left_cal, right_cal = load_calibs()
w, h = c.RESOLUTION
RMS, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = calibrate_stereo(
left_chessboards, right_chessboards, left_cal, right_cal, (h, w))
## -- Obtain stereo rectification projection matrices -- ##
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, (h, w), R, T)
s_cal = StereoCal(RMS, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F, \
R1, R2, P1, P2, Q, validPixROI1, validPixROI2)
save_stereo_calib(s_cal)
# Lx,Ly = (253.0, 218.0)
# Rx,Ry = (49.5, 202.8)
# bottle of sunscreen [0,0.05,3]
Lx, Ly = (378.5, 304.5)
Rx, Ry = (259.6, 298.6)
# Lx,Ly = (320+20, 240)
# Rx,Ry = (320-20, 240)
LPointsd = np.array([[Lx, Ly]], dtype=np.float32).reshape(-1, 1, 2)
RPointsd = np.array([[Rx, Ry]], dtype=np.float32).reshape(-1, 1, 2)
## -- Undistort points based on camera matrix and rectification projection -- ##
LPointsu = cv2.undistortPoints(LPointsd,
cameraMatrix1,
distCoeffs1,
R=R1,
P=P1)
RPointsu = cv2.undistortPoints(RPointsd,
cameraMatrix2,
distCoeffs2,
R=R2,
P=P2)
## -- Triangulate points in 3D space -- ##
points4d = cv2.triangulatePoints(P1, P2, LPointsu, RPointsu)
## -- Convert homogeneous coordinates to Euclidean space -- ##
points3d = np.array([i / points4d[3] for i in points4d[:3]])
print(points3d)
return True
def save_stereo_calib(stereo_calib):
os.chdir(func.make_path(root_p, c.DATA_DIR, c.STEREO_CALIB_DIR))
stereo_calib.save_params(f"{stereo_calib.rms:0.4f}{c.STEREO_CALIB_F}")
def newfile():
name = request.args["name"]
path = funcs.make_path(name)
open(path, "wb")
return redirect(url_for('index'))
time.sleep(0.001)
def shutdown(self):
'''
Attempts to gracefully close all processes that are currently running then closes the script
'''
print('Shutting down...')
self.camera_manager.event_manager.shutdown.set()
self.camera_manager.shutdown.wait(1)
self.camera_process.kill()
print("Shutdown (client)")
sys.exit()
if __name__ == "__main__":
func.clean_dir(func.make_path(root_p, c.IMG_DIR, c.RECORD_DIR))
client_name = l_r_consts.CLIENT_NAME
if len(sys.argv) > 1:
client_name = sys.argv[1]
client = Client(client_name)
while True:
try:
message_list = client.read_server_messages()
for message in message_list:
client.cmd_func(message['data'].type, message['data'].message)
time.sleep(1E-6)
except (sf.CommError, KeyboardInterrupt) as e:
print('Server disconnected, closing client')
def save_img(message):
os.chdir(func.make_path(root_p, c.IMG_DIR, c.STREAM_DIR))
n_frame, img = message['data'].message
cv2.imwrite(f"{message['client']}_{n_frame:04d}.png", img)
return True
def rectify_points(frame, camera_matrix, dist_coeffs, R_matrix, P_matrix,
*args):
for candidate in frame:
if candidate is not []:
candidate[c.X_COORD:c.Y_COORD + 1] = cv2.undistortPoints(
candidate[c.X_COORD:c.Y_COORD + 1],
camera_matrix,
dist_coeffs,
R=R_matrix,
P=P_matrix)
if __name__ == "__main__":
print("Server started")
func.clean_dir(func.make_path(root_p, c.IMG_DIR, c.STREAM_DIR))
server = Server()
server.initialise()
server.initialise_picamera()
while True:
try:
message_list = server.read_client_messages(read_all=True)
for message in message_list:
print(message['data'].message)
cmd = input("cmd: ")
server.cmd_func(cmd)
except sf.CommError as e:
print("Connection to client closed unexpectedly, shutting down...")
# make the above the same length as left_cands
if __name__ == "__main__":
## -- Test setup -- ##
import stereo_calibration as s_cal
import funcs as func
import os
ball_candidate_dict = {'left': {}, 'right': {}}
for i in range(1,11):
ball_candidate_dict['left'][i-1] = np.float32([[i*2,i*2,i*12,i*40,i*40]])
ball_candidate_dict['right'][i-1] = np.float32([[i*2,i*2,i*12,i*40-i*15,i*40]])
del ball_candidate_dict['left'][5]
del ball_candidate_dict['right'][5]
del ball_candidate_dict['left'][8]
del ball_candidate_dict['right'][2]
stereo_calib = s_cal.StereoCal()
root_p = os.getcwd()
os.chdir(func.make_path(root_p, c.DATA_DIR, c.STEREO_CALIB_DIR))
stereo_calib.load_params(c.ACTIVE_STEREO_F)
os.chdir(root_p)
## -- ## -- ##
points_3d = triangulate_points(ball_candidate_dict, stereo_calib)
for point in points_3d:
print(point)
