Commit 31bae09e authored by Guillaume CLIVAZ's avatar Guillaume CLIVAZ
Browse files

[calibration/wip] Update calibration.py script

+ Chessboard and charuco board corners detection.
+ Pandas dataframe:
    + filter frames were the board was not detected.
    + get matching ids (+corresponding list of object/images points) per frames extrinsics calibration of camera pairs with charuco.
+ Intrinsics/extrinsics calibration through all stream for the moment.
parent 36ad1b2d
Pipeline #48431 passed with stage
in 8 minutes and 21 seconds
#!/usr/bin/env python3
import os
import argparse
import json
import cv2
import pandas as pd
import numpy as np
from scipy.ndimage.morphology import grey_closing
from matplotlib import pyplot as plt
from bob.io.stream import Stream, StreamFile
def prepocess_image(image, closing_size=15, threshold=201):
"""
"""
grey = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
grey = grey_closing(grey, closing_size)
grey = cv2.adaptiveThreshold(grey,
255,
cv2.ADAPTIVE_THRESH_GAUSSIAN_C,
cv2.THRESH_BINARY, threshold,
1).astype(np.uint8)
return grey
def detect_chessboard_corners(image, pattern_size, verbosity):
"""
"""
ret, corners = cv2.findChessboardCorners(image, pattern_size, 0)
im_points = None
if ret:
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 300, 0.0001)
im_points = cv2.cornerSubPix(image, corners, (11,11), (-1,-1), criteria)
if verbosity > 2:
image = cv2.drawChessboardCorners(image, pattern_size, im_points, True)
plt.imshow(image, cmap='gray')
plt.show()
return ret, im_points
def detect_charuco_corners(image, charuco_board, verbosity):
"""
"""
corners, ids = None, None
markers, ids, rejected_points = cv2.aruco.detectMarkers(image, charuco_board.dictionary)
if ids is not None:
ret, corners, ids = cv2.aruco.interpolateCornersCharuco(markers, ids, image, charuco_board)
else:
ret = False
if verbosity > 2:
image = cv2.aruco.drawDetectedCornersCharuco(np.copy(image), corners, ids)
plt.imshow(image, cmap='gray')
plt.show()
return ret, corners, ids
def detect_patterns(directory_path, data_config_path, pattern_type, pattern_size, cv_flags=None, frames=None):
def compute_intrinsics(im_pts, image_size, obj_pts=None, ids=None, charuco_board=None):
"""
"""
cam_m, dist_coefs = None, None
if not isinstance(im_pts, list):
im_pts = [im_pts]
if charuco_board is not None:
if not isinstance(ids, list):
ids = [ids]
ret, cam_mat, dist_coefs, r, t = \
cv2.aruco.calibrateCameraCharuco(im_pts,
ids,
charuco_board,
image_size,
None,
None,
flags=18
)
else:
if not isinstance(obj_pts, list):
obj_pts = [obj_pts]
ret, cam_mat, dist_coefs, r, t = cv2.calibrateCamera(obj_pts,
im_pts,
image_size,
None,
None,
flags=18
)
return cam_mat, dist_coefs
def compute_relative_extrinsics(obj_pts,
im_pts1, cam_m1, dist_1,
im_pts2, cam_m2, dist_2,
image_size):
"""
"""
R, T, E, F = None, None, None, None
criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 100, 1e-5)
flags = (cv2.CALIB_FIX_INTRINSIC)
(_, _, _, _, R, T, E, F) = cv2.stereoCalibrate(obj_pts,
im_pts1, im_pts2,
cam_m1, dist_1,
cam_m2, dist_2,
image_size,
R, T, E, F,
flags=flags,
criteria=criteria
)[1:]
if R is None or T is None:
print("WARNING: No target pair was detected in the same capture")
return R, T
def detect_patterns(directory_path, data_config_path,
pattern_type, pattern_size, charuco_board = None,
cv_flags=None, frames=None, verbosity=0):
if charuco_board is None and pattern_type == "charuco":
raise ValueError("Charuco board not specified.")
files = [file for file in os.listdir(directory_path) if file.endswith(".h5")]
pattern_points = pd.DataFrame(dtype=object)
capture_dict = {}
image_size = {}
for file_idx, capture in enumerate(files):
f = StreamFile(os.path.join(directory_path, capture), data_config_path)
......@@ -32,8 +142,9 @@ def detect_patterns(directory_path, data_config_path, pattern_type, pattern_size
# add columns for streams, if not already present in dataframe
for stream in streams:
if stream.name not in pattern_points.columns:
pattern_points[stream.name] = None
if stream.name not in capture_dict.keys():
capture_dict[stream.name] = {}
image_size[stream.name] = {}
# Get the indices of frames to use in data files
if frames is None:
......@@ -52,22 +163,192 @@ def detect_patterns(directory_path, data_config_path, pattern_type, pattern_size
if image.shape[0] == 3:
image = image.mean(axis=0, dtype=image.dtype)
image = image.squeeze()
image_size[stream.name] = image.shape
capture_name = capture + ":" + str(frame)
# if detection fails, pattern_points are set to None
if pattern_type == "chessboard":
_, ptrn_pts = cv2.findChessboardCorners(image, pattern_size, cv_flags)
ret , ptrn_pts = detect_chessboard_corners(image, pattern_size, verbosity)
item = ptrn_pts
capture_dict[stream.name][capture_name] = ptrn_pts
elif pattern_type == "charuco":
raise NotImplementedError # TODO: charuco cv2 call
image = np.flip(image, 0)
ret , ptrn_pts, ids = detect_charuco_corners(image, charuco_board, verbosity)
if ids is not None:
capture_dict[stream.name][capture_name] = (ids, ptrn_pts)
else:
print("Capture {} / stream : {} : Failed to detect charuco corners."
.format(capture_name, stream.name))
capture_dict[stream.name][capture_name] = None
else:
raise ValueError("Invalid pattern type: " + pattern_type)
pattern_points.loc[capture + ":" + str(frame), stream.name] = ptrn_pts
return pattern_points
pattern_points = pd.DataFrame(capture_dict)
return pattern_points, image_size
def get_valid_frames_for_intrinsic_calibration(detection_df, camera):
return detection_df[~detection_df[camera].isnull()]
return detection_df[~detection_df[camera].isnull()][camera]
def get_valid_frames_for_extrinsic_calibration(detection_df, camera_1, camera_2, pattern_type):
#df = detection_df.copy()
detection_df = detection_df.loc[:, [camera_1, camera_2]]
detection_df = detection_df[(~detection_df[camera_1].isnull()) & (~detection_df[camera_2].isnull())]
# If charuco board, filter only matching ids for each frame
if pattern_type == "charuco":
for capture_id, capture in detection_df.iterrows():
mask_camera_1 = np.in1d(capture[camera_1][0], capture[camera_2][0])
mask_camera_2 = np.in1d(capture[camera_2][0], capture[camera_1][0])
capture[camera_1] = (capture[camera_1][0][mask_camera_1], capture[camera_1][1][mask_camera_1])
capture[camera_2] = (capture[camera_2][0][mask_camera_2], capture[camera_2][1][mask_camera_2])
return detection_df
def create_3Dpoints(pattern_size, square_size):
# create object points
rows, cols = pattern_size
object_points = np.zeros((rows*cols, 3), np.float32)
for i in range(0, cols):
for j in range(0, rows):
object_points[i*rows+j] = [cols -1 - i, j, 0]
object_points *= square_size
return object_points
def define_calibration_points(df, object_points, pattern_type):
obj_pts, im_pts, ids = [], [] ,[]
for capture_id, capture in df.iteritems():
if pattern_type == "charuco":
ids.append(capture[0])
im_pts.append(capture[1])
else:
im_pts.append(capture)
# If charuco board, mask all 3D points in the board not detected in the ids for each frame
if pattern_type == "charuco":
for frame in range(0,len(im_pts)):
list_id = ids[frame]
obj_pts.append(object_points[list_id, :])
else:
obj_pts = [object_points for _ in range(0,len(im_pts))]
return im_pts, obj_pts, ids
# only for debugging, will be removed further
def display_df(df):
for i, col in df.iteritems():
print(i)
if isinstance(col, pd.Series):
for j, v in col.iteritems():
if isinstance(v, tuple) and v[0] is not None and v[1] is not None:
print(j, v[0].shape, v[1].shape)
elif v is None:
print(j, v)
else:
print(j, v.shape)
else:
if isinstance(col, tuple) and col[0] is not None and col[1] is not None:
print(col[0].shape, col[1].shape)
elif col is None:
print(col)
else:
print(col.shape)
def parse_arguments():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("-c", "--capture-dir", type=str,
help="An absolute name of the directory containing .hdf5 files with calibration board.")
parser.add_argument("-d", "--data-config", type=str,
help="A configuration type used to load streams from hdf5 files.")
parser.add_argument("-P", "--pattern-type", choices=['chessboard','charuco'], help="Board type.")
parser.add_argument("-R", "--pattern-rows", type=int, default=10,
help="the number of rows of the calibration pattern")
parser.add_argument("-C", "--pattern-columns", type=int, default=7,
help="the number of columns of the calibration pattern")
parser.add_argument("-S", "--square-size", type=float, default=0.015,
help="Size [m] of the target squares")
parser.add_argument("-v", "--verbosity", action="count", default=0,
help="Output verbosity: -v output calibration result, -vv output the dataframe, \
-vvv plots the target detection.")
return parser.parse_args()
def main():
args = parse_arguments()
capture_dir_path = args.capture_dir
data_config_path = args.data_config
pattern_type = args.pattern_type
pattern_size = (args.pattern_rows, args.pattern_columns)
square_size = args.square_size
verbosity = args.verbosity
frames = [1, 0, 1] # TODO : find a good way to specify frames
charuco_board = None
if pattern_type == "charuco":
dict_type = cv2.aruco.DICT_4X4_1000
aruco_dict = cv2.aruco.getPredefinedDictionary(dict_type)
charuco_board = cv2.aruco.CharucoBoard_create(pattern_size[0], pattern_size[1], square_size, 0.01125, aruco_dict)
pts, image_data = detect_patterns(capture_dir_path, data_config_path,
pattern_type, pattern_size, charuco_board=charuco_board,
frames=frames, verbosity=verbosity)
# Create 3D object points for 1 frame
object_points = create_3Dpoints(pattern_size, square_size)
np.set_printoptions(formatter={'float': lambda x: "{0:0.3f}".format(x)})
# First all dataframe must be set and intrinsics computed for each camera
stream = [k for k in image_data.keys()]
calibration = dict.fromkeys(stream)
for idx, camera in enumerate(stream):
image_size = image_data[camera]
df_intrinsics = get_valid_frames_for_intrinsic_calibration(pts, camera)
if df_intrinsics.empty:
print("{} dataframe is emtpy, this camera will not be calibrated.".format(camera))
continue
if verbosity > 1:
print("\n=== Intrinsics {} dataframe ===".format(camera))
display_df(df_intrinsics)
calibration[camera] = {}
im_pts, obj_pts, ids = define_calibration_points(df_intrinsics, object_points, pattern_type)
if pattern_type == "charuco":
cam_mat, dist_coefs = compute_intrinsics(im_pts[0], image_size, ids=ids[0], charuco_board=charuco_board)
else:
cam_mat, dist_coefs = compute_intrinsics(im_pts[0], image_size, obj_pts=obj_pts[0])
calibration[camera]["cam_mat"] = cam_mat
calibration[camera]["distortion"] = dist_coefs
if verbosity > 0:
print("\n=== Intrinsics {} ===".format(camera))
print("Camera matrix \n {} \n Distortion coeffs \n {} \n".format(cam_mat, dist_coefs))
for camera_2 in stream[idx+1:]:
df_extrinsics = get_valid_frames_for_extrinsic_calibration(pts, camera, camera_2, pattern_type)
calibration[camera]["df_extrinsics_{}".format(camera_2)] = df_extrinsics
if verbosity > 1:
print("\n=== Extrinsics {}-{} dataframe ===".format(camera, camera_2))
display_df(df_extrinsics)
# Then the extrinsics between camera pair are computed
stream = [k for k, v in calibration.items() if v is not None]
for idx, camera in enumerate(stream):
image_size = image_data[camera]
for camera_2 in stream[idx+1:]:# or with itertools.tee()
if image_data[camera_2] != image_size:
raise ValueError("Image size between {} and {} does not match.".format(camera, camera2))
if not "df_extrinsics_{}".format(camera_2) in calibration[camera].keys():
continue
df = calibration[camera]["df_extrinsics_{}".format(camera_2)]
cam_mat1 = calibration[camera]["cam_mat"]
cam_mat2 = calibration[camera]["cam_mat"]
dist_coefs1 = calibration[camera]["distortion"]
dist_coefs2 = calibration[camera_2]["distortion"]
im_pts1, obj_pts, ids = define_calibration_points(df[camera], object_points, pattern_type)
im_pts2, obj_pts2, ids2 = define_calibration_points(df[camera_2], object_points, pattern_type)
R, T = compute_relative_extrinsics(obj_pts,
im_pts1, cam_mat1, dist_coefs1,
im_pts2, cam_mat2, dist_coefs2,
image_size)
if verbosity > 0:
print("\n=== Extrinsics {}-{} ===".format(camera, camera_2))
print("Rotation \n {} \nTranslation \n {} \n".format(R, T.T))
def get_valid_frames_for_extrinsic_calibration(detection_df, camera_1, camera_2):
return detection_df[(~detection_df[camera_1].isnull()) & (~detection_df[camera_2].isnull())]
if __name__ == "__main__":
main()
......@@ -128,6 +128,7 @@ setup(
# scripts should be declared using this entry:
'console_scripts': [
'stereo_matcher.py = bob.ip.stereo.stereo_matcher:main',
'calibration.py = bob.ip.stereo.calibration:main',
],
},
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment