def _video_capture(self, img):
"""
Desc
Args:
returns:
"""
self._img_src = self._write_text(
image=img,
text="Press 'S' to save the current frame.",
coords=(25, 45))
self._img_src = self._write_text(image=self._img_src,
text="Press 'Q' to exit.",
coords=(25, 80))
cv2.imshow(self._win_name, self._img_src)
key = cv2.waitKey(self._win_time)
if key == -1:
pass
# If Q key pressed then close the window
elif key == 113 or key == 81:
cv2.destroyAllWindows()
exit()
# If S key pressed then save current frame
elif key == 115:
self._save_frame()
# Invalid key
else:
printlog(msg="key {} command no found".format(key),
msg_type="WARN")
def _win_bird_mouse_event(self, event, x, y, flags, param):
"""
Callback for mouse events in the bird/sky view window
Args:
event: 'int' mouse event
x: 'int' mouse event x axis position
y: 'int' mouse event y axis position
flags: 'list[int]' flags given by mouse event
param: 'XXXX' XXXX
returns:
"""
self._win_bird_event = True
if event == cv2.EVENT_RBUTTONDOWN:
self.bird_pts = []
self.bird_mouse_current_pos = [0, 0]
return
# Update bird/sky view window components
if self.img_sky is not None:
x = int((x / self.img_sky.shape[1]) * self._win_bird_width)
y = int((y / self.img_sky.shape[0]) * self._win_bird_height)
# Re-scale x and y axis to fit original image size
self.bird_mouse_current_pos = [x, y]
if len(self.bird_pts) == 1:
dv = abs(self.bird_pts[0][1] - y)
dh = abs(self.bird_pts[0][0] - x)
if dv >= dh:
self.bird_mouse_current_pos[0] = self.bird_pts[0][0]
else:
self.bird_mouse_current_pos[1] = self.bird_pts[0][1]
# Add point
if event == cv2.EVENT_LBUTTONDOWN:
self.bird_pts.append((self.bird_mouse_current_pos[0],
self.bird_mouse_current_pos[1]))
if len(self.bird_pts) == 2:
dp = int(math.sqrt((dv)**2 + (dh)**2))
printlog(
msg="Please set distance in meters [m] for {} {}"
" pixels".format(dp,
"vertical" if dv >= dh else "horizontal"),
msg_type="USER",
)
dm = input("input [m]: ")
self._update_pix_relation(dm=float(dm),
dp=int(dp),
relation="dv" if dv >= dh else "dh")
self.bird_pts = []
def _create_objPts(self):
"""
Function to create the chessboard matrix points for calibration
Args: NaN
Returns: NaN
"""
printlog(msg="Creating object points", msg_type="INFO")
self._obj_pts = np.zeros((self._nCornX * self._nCornY, 3), np.float32)
self._obj_pts[:, :2] = np.mgrid[0:self._nCornX,
0:self._nCornY].T.reshape(-1, 2)
def _save_frame(self):
"""
Function to save the current frame of the video
Args: NaN
Returns: NaN
"""
cv2.imwrite(
os.path.join(self._imgs_path,
"chessboard_{}.jpg".format(self._frame)),
self._img_src,
)
self._frame += 1
printlog(msg="Frame saved", msg_type="INFO")
def _load_frames(self, imgs_path):
"""
Function to load the chessboard images for calibration
Args: NaN
Returns: NaN
"""
# Defining images path files
self._imgs_path = imgs_path
printlog(msg="Loading calibration images ...", msg_type="INFO")
self._cal_imgs = glob.glob(self._imgs_path + "/*.jpg")
if len(self._cal_imgs) == 0:
printlog(
msg="{} folder is empty,"
" please allocate the calibration images there.".format(
self._imgs_path),
msg_type="ERROR",
)
exit()
else:
printlog(
msg="{} Images were loaded.".format(len(self._cal_imgs)),
msg_type="OKGREEN",
)
def _update_pix_relation(self, dm, dp, relation):
"""
updates pixels per meter relation
Args:
dm: 'float' measure in meters
dp: 'int' measure in pixels
relation: 'string' relation
dv: for vertical measurement
dh: for horizontal measurement
returns:
"""
if relation == "dv":
self.ppmy = float(dm / dp)
printlog(
msg="Vertical relation updated, new values are "
"ppmx={:.2f}[pix/m], ppmy={:.2f}[pix/m]".format(
self.ppmx if self.ppmx is not None else 0.0,
self.ppmy if self.ppmy is not None else 0.0,
),
msg_type="USER",
)
elif relation == "dh":
self.ppmx = float(dm / dp)
printlog(
msg="Horizontal relation updated, new values are "
"ppmx={:.2f}[pix/m], ppmy={:.2f}[pix/m]".format(
self.ppmx if self.ppmx is not None else 0.0,
self.ppmy if self.ppmy is not None else 0.0,
),
msg_type="USER",
)
else:
printlog(msg="No valid relation to update", msg_type="ERROR")
def _save_calibration(self):
"""
Function to save the calibration file in .yaml format
Args: NaN
Returns: NaN
"""
# Dictionary containing the data
data = {
"image_height": self._img_size[0],
"image_width": self._img_size[1],
"camera_matrix": {
"rows": 3,
"cols": 3,
"data": list(self._camMat.tolist()),
},
"distortion_model": "rational_polynomial",
"distortion_coefficients": {
"rows": 1,
"cols": 5,
"data": list(self._distCoef.tolist()),
},
}
file_path = os.path.join(self._int_path, self._int_file)
with open(file_path, "w") as output_file:
yaml.dump(data,
output_file,
default_flow_style=False,
sort_keys=False)
printlog(
msg="Intrinsic calibration saved successfuly at {}".format(
file_path),
msg_type="OKGREEN",
)
def _load_extrinsic(self):
"""
Load extrinsic calibration parameters from yaml file
Args:
returns:
"""
file_path = os.path.join(self._ext_path, self._ext_file)
if os.path.isfile(file_path):
try:
with open(file_path, "r") as stream:
data = yaml.safe_load(stream)
# Cast variables to word with python variables
self._Mpts = {
"p1": tuple(data["Mpt_src"]["p1"]),
"p2": tuple(data["Mpt_src"]["p2"]),
"p3": tuple(data["Mpt_src"]["p3"]),
"p4": tuple(data["Mpt_src"]["p4"]),
}
self.pts = [tuple(pt) for pt in self._Mpts.values()]
self._VISION_CAL_WARPED_SIZE = tuple(data["M_warped_size"])
self.img_scl = data["img_scl"]
# self.img_width = data["src_img_size"][0]
# self.img_height = data["src_img_size"][1]
self.ppmx = data["ppmx"]
self.ppmy = data["ppmy"]
self.warp_width = data["dst_warp_size"][0]
self.warp_height = data["dst_warp_size"][1]
self._Mdst_pts = {
"p1": tuple(data["Mdst_pts"]["p1"]),
"p2": tuple(data["Mdst_pts"]["p2"]),
"p3": tuple(data["Mdst_pts"]["p3"]),
"p4": tuple(data["Mdst_pts"]["p4"]),
}
self._in_gerion = data["in_region"]
self._calibrate_extrinsic()
# Update window elements
self._win_bird_event = True
self._win_event = True
printlog(
msg="Extrinsic file {} loaded".format(self._ext_file),
msg_type="INFO",
)
except IOError as e: # Report any error saving de data
printlog(msg="Error loading extrinsic, {}".format(e),
msg_type="ERROR")
else:
printlog(msg="No extrinsic file {}".format(self._ext_file),
msg_type="WARN")
def _save_extrinsic(self):
"""
Save extrinsic calibration parameters in a yaml file in extrinsic
calibration folder and path defined in constructor
Args:
returns:
"""
# Don't proceed if there's no calibration matrix
if self._M is None:
printlog(msg="No transformation matrix yet to save",
msg_type="WARN")
return
# Get dictionary of extrinsic parameters to save in yaml file
ext_params = {
"M": self._M.tolist(),
"Mpt_src": {key: list(pt)
for key, pt in self._Mpts.items()},
"Mdst_pts": {key: list(pt)
for key, pt in self._Mdst_pts.items()},
"ppmx": self.ppmx,
"ppmy": self.ppmy,
"M_warped_size": list(self._VISION_CAL_WARPED_SIZE),
"src_img_size": [self.img_width, self.img_height],
"dst_warp_size": [self.warp_width, self.warp_height],
"img_scl": self.img_scl,
"intrinsic": False if self._intrinsic is None else True,
"in_region": self._in_gerion,
}
# Absolute path to save file
file_path = os.path.join(self._ext_path, self._ext_file)
try: # Save the calibration data in file
with open(file_path, "w") as outfile:
yaml.dump(ext_params, outfile, default_flow_style=False)
printlog(msg="Extrinsic parameters saved {}".format(file_path),
msg_type="INFO")
except IOError as e: # Report any error saving de data
printlog(msg="Error saving extrinsic, {}".format(e),
msg_type="ERROR")
def main(argv):
# Reading input arguments
try:
opts, args = getopt.getopt(argv, "hi:", ["path="])
except getopt.GetoptError:
printlog("calibrate_extrinsic.py -i <intrinsic_action>")
sys.exit(2)
for opt, arg in opts:
if opt == "-h":
printlog("calibrate_extrinsic.py -i "
"<video name, imgs path or video device>")
sys.exit()
elif opt in ("-i", "--intrinsic"):
intrinsic_file = arg
# -------------------------------------------------------------------------
# Calibrator instantiation
intrinsic = calibrator()
# Get source to calibrate extrinsic
fwd = os.path.dirname(os.path.abspath(__file__))
fwd = os.path.abspath(os.path.join(fwd, os.pardir))
if len(intrinsic_file) < 4:
videoDevice = int(intrinsic_file)
cap = cv2.VideoCapture(videoDevice)
if not cap.isOpened():
printlog(
msg="Error opening video device, "
"please try a different one",
msg_type="ERROR",
)
sys.exit()
while True:
ret, frame = cap.read()
if not ret:
printlog(msg="Error getting video frame", msg_type="ERROR")
sys.exit()
intrinsic._video_capture(frame)
elif len(intrinsic_file) >= 4:
file_ext = str(intrinsic_file).split(".")
file_name = str(intrinsic_file)
if len(file_ext) == 1:
imgs_path = os.path.join(fwd, "calibration", file_name)
intrinsic.calibrate(imgs_path)
elif len(file_ext) == 2:
if (file_ext[1] == "mp4") or (file_ext[1] == "avi"):
file_path = os.path.join(fwd, "media", file_name)
# Openning video file
cap = cv2.VideoCapture(file_path)
# Check if camera opened successfully
if not cap.isOpened():
printlog(msg="Error opening video stream or file",
msg_type="ERROR")
sys.exit()
while True:
ret, frame = cap.read()
if not ret:
printlog(msg="Error getting video frame",
msg_type="ERROR")
sys.exit()
intrinsic._video_capture(frame)
else:
printlog(
msg="Invalid format. A .mp4 or .avi file is required",
msg_type="ERROR",
)
sys.exit()
else:
printlog(msg="Not valid argument", msg_type="ERROR")
sys.exit()
def calibrate(self, imgs_path):
"""
Function to perform the intrinsic camera calibration
Args: NaN
Returns: NaN
"""
# Loading images for calibration
self._load_frames(imgs_path)
# Object points for chessboard pattern
self._create_objPts()
object_pts = []
image_pts = []
n_frame = 0
wrong_cal = 0
printlog(msg="Starting calibration.", msg_type="INFO")
for frame in self._cal_imgs:
# Frame reading
src_img = cv2.imread(filename=frame)
# Color transformation
gray_img = cv2.cvtColor(src=src_img, code=cv2.COLOR_RGB2GRAY)
# Finding chessboard inner corners
ret, corners = cv2.findChessboardCorners(
image=gray_img, patternSize=(self._nCornX, self._nCornY))
if ret == True:
self._curr_frame += 1
# Points appending for calibration
object_pts.append(self._obj_pts)
image_pts.append(corners)
# Drawing the chessboard corners in the current frame
img_chess = cv2.drawChessboardCorners(
image=src_img,
patternSize=(self._nCornX, self._nCornY),
corners=corners,
patternWasFound=ret,
)
img_chess = self._write_text(
image=img_chess,
text="Frame {}".format(self._curr_frame),
coords=(25, 45),
)
cv2.imshow("Chessboard detections", img_chess)
key = cv2.waitKey(100)
if key == 113 or key == 81:
cv2.destroyAllWindows()
exit()
else:
wrong_cal += 1
n_frame += 1
self._img_size = (src_img.shape[1], src_img.shape[0])
# Performing calibration
(
self._rep_error,
self._camMat,
self._distCoef,
self._rotVect,
self._transVect,
) = cv2.calibrateCamera(object_pts, image_pts, self._img_size, None,
None)
printlog(msg="Resulting reprojection error: %.5f" % self._rep_error,
msg_type="INFO")
if self._rep_error >= 0.5:
printlog(
msg=
"Consider adding more images of the pattern, to reduce the Reprojection Error",
msg_type="WARN",
)
if wrong_cal != 0:
printlog(
msg="Calibration not found in {} images.".format(wrong_cal),
msg_type="WARN",
)
printlog(msg="Calibration completed.", msg_type="OKGREEN")
self._save_calibration()
def main(argv):
# -------------------------------------------------------------------------
extrinsic_file = None
intrinsic_file = None
source_file = None
intrinsic = None
# -------------------------------------------------------------------------
# Read user input
try:
opts, _ = getopt.getopt(argv, "hv:e:i:",
["video=", "extfile=", "intfile="])
except getopt.GetoptError:
printlog("calibrate_extrinsic.py -v <video_source_file> -e "
"<extrinsic_file> -i <intrinsic_file>")
sys.exit(2)
for opt, arg in opts:
if opt == "-h":
printlog("calibrate_extrinsic.py -v <video_source_file> -e "
"<extrinsic_file> -i <intrinsic_file>")
sys.exit()
elif opt in ("-e", "--extrinsic"):
extrinsic_file = arg
elif opt in ("-i", "--intrinsic"):
intrinsic_file = arg
elif opt in ("-v", "--video"):
source_file = arg
# -------------------------------------------------------------------------
# Get source to calibrate extrinsic
fwd = os.path.dirname(os.path.abspath(__file__))
fwd = os.path.abspath(os.path.join(fwd, os.pardir))
# Take first file in media folder if and input was not defined
if source_file is None:
files = glob.glob("./media/*.mp4")
if not len(files):
printlog(msg="File {} no found", msg_type="ERROR")
return 2
file_default = os.path.basename(files[0])
file_name = file_default if source_file is None else source_file
file_ext = file_name.split(".")[-1]
file_path = os.path.join(fwd, "media", file_name)
if not os.path.isfile(file_path):
printlog(msg="File {} no found".format(file_name), msg_type="ERROR")
return 2
# If source file is a video
if file_ext == "mp4":
# Create a VideoCapture object and read from input file
# If the input is the camera, pass 0 instead of the video file name
cap = cv2.VideoCapture(file_path)
# Check if camera opened successfully
if not cap.isOpened():
printlog(msg="Error opening video stream or file",
msg_type="ERROR")
# Get video frame
ret, frame = cap.read()
# Check if camera opened successfully
if not ret:
printlog(msg="Error getting video frame", msg_type="ERROR")
# If source file is an image
elif file_ext == "jpg" or file_ext == "png":
frame = cv2.imread(file_path)
# If file has a invalid format
else:
printlog(msg="File {} with invalid".format(file_name),
msg_type="ERROR")
return 2
# -------------------------------------------------------------------------
# Load intrinsic parameters
if intrinsic_file is not None:
intrinsic_path = os.path.join(fwd, "configs", intrinsic_file)
intrinsic = read_intrinsic_params(CONF_PATH=os.path.join(
fwd, "configs"),
FILE_NAME=intrinsic_file)
else:
printlog(msg="No using intrinsic file", msg_type="WARN")
# -------------------------------------------------------------------------
# Create and run extrinsic calibrator object
extrinsic = calibrator(extrinsic=extrinsic_file,
intrinsic=intrinsic,
img=frame)
while True:
extrinsic.draw_gui()
def draw_gui(self):
"""
Draw extrinsic calibration window elements
Args:
returns:
"""
if self._show_menu:
cv2.imshow(self._win_name, self._img_menu)
else:
if self._win_event:
self.img_gui = self.img_src.copy()
# Draw gui window elements
self._draw_polygon(img=self.img_gui)
self._draw_points(img=self.img_gui)
self._draw_rulers(img=self.img_gui,
pose=self.mouse_current_pos)
self._win_event = False
if self._win_bird_event:
if self._M is not None:
self._draw_skyview()
self._win_bird_event = False
cv2.imshow(self._win_name, self.img_gui)
key = cv2.waitKey(self._win_time)
# If no key pressed do nothing
if key == -1:
pass
# If Q key pressed then save extrinsic
elif key == 113 or key == 81:
self._save_extrinsic()
exit()
# If S key pressed then save extrinsic
elif key == 115:
self._save_extrinsic()
# If L key pressed then save extrinsic
elif key == 108:
self._load_extrinsic()
# If A key pressed then go previous point
elif key == 97:
self._win_event = True
if self.pts_idx + 1 <= len(self.pts) - 1:
self.pts_idx += 1
elif self.pts_idx == len(self.pts) - 1:
self.pts_idx = 0
# If D key pressed then next previous point
elif key == 100:
self._win_event = True
if self.pts_idx > 0:
self.pts_idx -= 1
elif self.pts_idx == 0:
self.pts_idx = len(self.pts) - 1
# If R key pressed then rotate space
elif key == 114:
self._rotate_space()
# If U key pressed then move current point idx up
elif key == 117:
self._move_pt(direction="up")
# If J key pressed then move current point idx down
elif key == 106:
self._move_pt(direction="down")
# If K key pressed then move current point idx to the right
elif key == 107:
self._move_pt(direction="right")
# If TAB key pressed then show menu/help
elif key == 9:
self._show_menu = not self._show_menu
# If H key pressed then move current point idx to the left
elif key == 104:
self._move_pt(direction="left")
# If 1, 2, 3, or 4 key pressed then assing point to pt
elif key == 49 or key == 50 or key == 51 or key == 52:
self._assing_mpt(pt_idx=chr(key))
self._win_event = True
# If E key pressed then performe extrinsic calibration
elif key == 101 or self._win_bird_event:
self._calibrate_extrinsic()
# If I key pressed then analyse in region
elif key == 105:
self._in_gerion = not self._in_gerion
self._calibrate_extrinsic()
# If no key action found
else:
printlog(msg="key {} command no found".format(key),
msg_type="WARN")
def _calibrate_extrinsic(self):
"""
Description
Args:
variable: 'type' description
returns:
variable: 'type' description
"""
# Check if there're all user's points assigned to perform extrinsic
# calibration
for pt in self._Mpts.values():
if pt is None:
printlog(msg="No transformation matrix yet", msg_type="WARN")
return
# Get space transformation matrix
M = get_rot_matrix(
self._Mpts["p1"],
self._Mpts["p2"],
self._Mpts["p3"],
self._Mpts["p4"],
self._VISION_CAL_WARPED_SIZE,
)
# Get image size coordinates in warped space
dst_pt_ltop = get_projection_point_dst(coords_src=(0, 0), M=M)
dst_pt_rtop = get_projection_point_dst(coords_src=(self.img_width, 0),
M=M)
dst_pt_lbottom = get_projection_point_dst(coords_src=(0,
self.img_height),
M=M)
dst_pt_rbottom = get_projection_point_dst(coords_src=(self.img_width,
self.img_height),
M=M)
self._MOriginpts = [
dst_pt_ltop, dst_pt_rtop, dst_pt_rbottom, dst_pt_lbottom
]
if not self._in_gerion:
# Get bounding rect with the new geometry
x, y, self.warp_width, self.warp_height = cv2.boundingRect(
np.array((dst_pt_ltop, dst_pt_rtop, dst_pt_lbottom,
dst_pt_rbottom)))
# Apply translation component to rotation matrix
Mt = np.float32([[1.0, 0, -float(x)], [0, 1.0, -float(y)],
[0, 0, 1.0]])
self._M = np.matmul(Mt, M)
# Get image size coordinates in warped space
dst_pt_ltop = get_projection_point_dst(coords_src=(0, 0),
M=self._M)
dst_pt_rtop = get_projection_point_dst(coords_src=(self.img_width,
0),
M=self._M)
dst_pt_lbottom = get_projection_point_dst(
coords_src=(0, self.img_height), M=self._M)
dst_pt_rbottom = get_projection_point_dst(
coords_src=(self.img_width, self.img_height), M=self._M)
self._MOriginpts = [
dst_pt_ltop,
dst_pt_rtop,
dst_pt_rbottom,
dst_pt_lbottom,
]
else:
self.warp_width = self._VISION_CAL_WARPED_SIZE[0]
self.warp_height = self._VISION_CAL_WARPED_SIZE[1]
self._M = M
# Get image size coordinates in warped space
dst_p1 = get_projection_point_dst(coords_src=self._Mpts["p1"],
M=self._M)
dst_p2 = get_projection_point_dst(coords_src=self._Mpts["p2"],
M=self._M)
dst_p3 = get_projection_point_dst(coords_src=self._Mpts["p3"],
M=self._M)
dst_p4 = get_projection_point_dst(coords_src=self._Mpts["p4"],
M=self._M)
self._Mdst_pts = {
"p1": dst_p1,
"p2": dst_p2,
"p3": dst_p3,
"p4": dst_p4
}
# print(self.img_src.shape, self.img_width, self.img_height)
self._win_bird_event = True