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