add calibration

Author: cjh1995-ros

calibration/board.jpg

@@ -0,0 +1,85 @@
+import cv2
+import numpy as np
+import copy
+import yaml
+
+input = "data/chessboard.avi"
+
+board_pattern = (10, 7)
+select_images = True
+
+capture = cv2.VideoCapture(input)
+
+if capture.isOpened() == False: raise Exception("No video")
+
+images = []
+
+while True:
+    ret1, image = capture.read()
+    if not ret1: break
+    if select_images:
+        cv2.imshow("3DV Tutorial: Camera Calibration", image)
+        key = cv2.waitKey(1)
+        if key == 27: break # 'ESC' key: Exit
+        elif key == 32:     # 'Space' key: Pause
+            ret2, pts = cv2.findChessboardCorners(image, board_pattern, None) # No flags
+            
+            # display = image.clone()
+            display = copy.deepcopy(image)
+            display = cv2.drawChessboardCorners(display, board_pattern, pts, ret2)
+            cv2.imshow("3DV Tutorial: Camera Calibration", display)
+            key = cv2.waitKey()
+            if key == 27: break
+            elif key == 13: images.append(image) # 'Enter' key: Save
+    else: 
+        images.append(image)
+
+capture.release()
+
+if(len(images)) == 0:
+    print("no images")
+    raise Exception("There is no captured images!")
+
+# Find 2D corner points from given images
+img_points = []
+h, w = 0,0
+for image in images:
+    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
+    h, w = gray.shape
+    ret3, corners = cv2.findChessboardCorners(gray, board_pattern) # No flags
+    if ret3 == True:
+        img_points.append(corners)
+
+if len(img_points) == 0:
+    raise Exception("No 2d Corner pts")
+
+# Prepare 3D points of the chess board
+objp = np.zeros((10*7, 3), np.float32)
+objp[:, :2] = np.mgrid[0:7, 0:10].T.reshape(-1, 2)
+obj_points = []
+for _ in images:
+    obj_points.append(objp)
+
+# Calibrate Camera
+K = np.eye(3,3, dtype=np.float32)
+dist_coeff = np.zeros((4,1))
+rms, K, dist_coeff, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, (h,w), None, None)
+
+# Report calibration results
+print("## Camera Calibration Results")
+print(f"* The number of applied images = {w}x{h}")
+print(f"* RMS error = {rms}")
+print(f"* Camera matrix (K) = \n{K}")
+print(f"* Distortion coefficient (k1, k2, p1, p2, k3, ...) = {dist_coeff}")
+
+# Save as cam_config.yaml
+file_name = "cfg/cam_config.yaml"
+cam_dict = {
+    "Intrinsic": K.flatten().tolist(),
+    "Distortion": dist_coeff.flatten().tolist(),
+    "RMS": rms,
+}
+with open(file_name, 'w') as f:
+    yaml.dump(cam_dict, f, sort_keys=False, default_flow_style=False)
+
+print("End!")

calibration/camera_calibration_v1.py

@@ -0,0 +1,84 @@
+import cv2
+import numpy as np
+import copy
+import yaml
+import time
+
+help_msg = "At least 40 images is needed for calibrating camera. We Take picture in every 2 sec."
+board_pattern = (10, 7)
+
+capture = cv2.VideoCapture(0)
+if capture.isOpened() == False: raise Exception("No video")
+
+images = []
+count = 0
+TIME_LIMIT = 2
+t = time.time()
+
+print(help_msg)
+
+while True:
+    ret1, image = capture.read()
+    if not ret1: break
+    
+    key = cv2.waitKey(1)
+    if key == 27 or count == 40: break # 'ESC' key: Exit
+    # elif key == 13: images.append(image) # 'Enter' key: Save image 
+    if time.time() - t > TIME_LIMIT:
+        images.append(image)
+        count += 1
+        ret2, pts = cv2.findChessboardCorners(image, board_pattern, None) # No flags
+        image = copy.deepcopy(image)
+        image = cv2.drawChessboardCorners(image, board_pattern, pts, ret2)
+
+    cv2.imshow("3DV Tutorial: Camera Calibration", image)
+
+capture.release()
+
+if(len(images)) == 0:
+    print("no images")
+    raise Exception("There is no captured images!")
+
+# Find 2D corner points from given images
+img_points = []
+h, w = 0,0
+for image in images:
+    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
+    h, w = gray.shape
+    ret3, corners = cv2.findChessboardCorners(gray, board_pattern) # No flags
+    if ret3 == True:
+        img_points.append(corners)
+
+if len(img_points) == 0:
+    raise Exception("No 2d Corner pts")
+
+# Prepare 3D points of the chess board
+objp = np.zeros((10*7, 3), np.float32)
+objp[:, :2] = np.mgrid[0:7, 0:10].T.reshape(-1, 2)
+obj_points = []
+for _ in images:
+    obj_points.append(objp)
+
+# Calibrate Camera
+K = np.eye(3,3, dtype=np.float32)
+dist_coeff = np.zeros((4,1))
+rms, K, dist_coeff, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, (h,w), None, None)
+
+# Report calibration results
+print("## Camera Calibration Results")
+print(f"* The number of applied images = {w}x{h}")
+print(f"* RMS error = {rms}")
+print(f"* Camera matrix (K) = \n{K}")
+print(f"* Distortion coefficient (k1, k2, p1, p2, k3, ...) = {dist_coeff}")
+
+# Save as cam_config.yaml
+file_name = "cfg/cam_config.yaml"
+cam_dict = {
+    "Intrinsic": K.flatten().tolist(),
+    "Distortion": dist_coeff.flatten().tolist(),
+    "RMS": rms,
+}
+with open(file_name, 'w') as f:
+    yaml.dump(cam_dict, f, sort_keys=False, default_flow_style=False)
+
+print("End!")

calibration/camera_calibration_v2.py

@@ -0,0 +1,29 @@
+import cv2
+import numpy as np
+
+def main():
+    img_name = "board.jpg"
+    img = cv2.imread(img_name)
+    img = cv2.resize(img, (640, 640))
+    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+    
+
+    gray = np.float32(gray)
+    dst = cv2.cornerHarris(gray, 2, 3, 0.04)
+
+    # img[dst>0.01*dst.max()]=[0,0,255]
+    x = dst[dst>0.01*dst.max()]
+    print(x.shape)
+    # for corner in img[dst>0.01*dst.max()]:
+    #     print(corner.shape)
+        # img = cv2.circle(img, tuple(corner), 5, (0, 0, 255), 1)
+
+    # print(dst.shape)
+    # print(test.shape)
+    cv2.imshow("img", img)
+    cv2.waitKey(0)
+
+
+
+if __name__ == "__main__":
+    main()

calibration/corner.py

@@ -0,0 +1,50 @@
+from re import S
+import cv2
+import numpy as np
+import yaml
+from yaml.loader import SafeLoader
+
+def main():
+    input = "data/chessboard.avi"
+    cam_params_file = "cam_config.yaml"
+    K = np.zeros((3,3))
+    dist_coeff = np.zeros((4,1))
+    with open(cam_params_file, 'r') as f:
+        cam_params = yaml.load(f, Loader=SafeLoader)
+        K = np.resize(cam_params["Intrinsic"], (3,3))
+        dist_coeff = np.resize(cam_params["Distortion"], (4,1))
+
+    # Open Video
+    capture = cv2.VideoCapture(input)
+    if capture.isOpened() == False: raise Exception("No video")
+
+    show_rectify = True
+    map1 = np.array([])
+    map2 = np.array([])
+
+    while True:
+        ret, image = capture.read()
+        if not ret: break
+        
+        info = "Original"
+        if(show_rectify):
+            if not (len(map1) & len(map2)):
+                map1, map2 = cv2.initUndistortRectifyMap(K, dist_coeff, None, None, (image.shape[1], image.shape[0]), cv2.CV_32FC1)
+            image = cv2.remap(image, map1, map2, interpolation=cv2.INTER_LINEAR)
+            info = "Rectified"
+        cv2.putText(image, info, (5,15), cv2.FONT_HERSHEY_PLAIN, 1, (0, 255,0))
+
+        cv2.imshow("3DV Tutorial: Distortion Correction", image)
+
+        key = cv2.waitKey(1)
+        if key == 27: break
+        elif key == 9: show_rectify = not show_rectify
+        elif key == 32:
+            key = cv2.waitKey()
+            if key == 27: break
+            elif key == 9: show_rectify = not show_rectify
+
+    capture.release()
+
+if __name__ == "__main__":
+    main()

calibration/distortion_correction.py

@@ -0,0 +1,72 @@
+import cv2
+import numpy as np
+import pyrealsense2 as rs
+from threading import Lock
+import time
+
+# This is cv2 and T265
+frame_mutex = Lock()
+frame_data = {"left"  : None,
+              "right" : None,
+              "timestamp_ms" : None
+              }
+
+def callback(frame):
+    global frame_data
+    if frame.is_frameset():
+        frameset = frame.as_frameset()
+        f1 = frameset.get_fisheye_frame(1).as_video_frame()
+        f2 = frameset.get_fisheye_frame(2).as_video_frame()
+        left_data = np.asanyarray(f1.get_data())
+        right_data = np.asanyarray(f2.get_data())
+        ts = frameset.get_timestamp()
+        frame_mutex.acquire()
+        frame_data["left"] = left_data
+        frame_data["right"] = right_data
+        frame_data["timestamp_ms"] = ts
+        frame_mutex.release()
+
+# How to see realsense with cv2
+pipeline = rs.pipeline()
+cfg = rs.config()
+
+# Get Device Product
+pipeline.start(cfg, callback)
+
+try:
+    WINDOW_TITLE = "Realsense"
+    cv2.namedWindow(WINDOW_TITLE, cv2.WINDOW_NORMAL)
+    profiles = pipeline.get_active_profile()
+    streams = {"left": profiles.get_stream(rs.stream.fisheye,1).as_video_stream_profile(),
+                "right": profiles.get_stream(rs.stream.fisheye,2).as_video_stream_profile()}
+    t = 0
+    while True:
+        t = time.time()
+        frame_mutex.acquire()
+        valid = frame_data["timestamp_ms"] is not None
+        frame_mutex.release()
+
+        if valid:
+            frame_mutex.acquire()
+            frame_copy = {"left": frame_data["left"].copy(),
+                          "right": frame_data["right"].copy()}
+            frame_shape = frame_copy["left"].shape
+            frame_mutex.release()
+            try:
+                t = time.time() - t
+                fps = 1/t
+
+                cv2.putText(frame_copy["left"], f"fps is {fps:.2}", (5, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255,255,255), 2)
+                cv2.putText(frame_copy["right"], f"shape is {frame_shape[1]} x {frame_shape[0]}", (5, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (255,255,255), 2)
+                cv2.imshow(WINDOW_TITLE, np.hstack((frame_copy["left"], frame_copy["right"])))
+            except Exception as e:
+                print(e)
+
+        key = cv2.waitKey(1)
+        if key == ord('q'): break
+
+except Exception as e:
+    print(e)
+
+finally:
+    pipeline.stop()

calibration/fisheye_calibration_v1.py

@@ -0,0 +1 @@
+pybackend2.cpython-38-x86_64-linux-gnu.so.2

calibration/pybackend2.cpython-38-x86_64-linux-gnu.so

@@ -0,0 +1 @@
+pybackend2.cpython-38-x86_64-linux-gnu.so.2.45.0

calibration/pybackend2.cpython-38-x86_64-linux-gnu.so.2

@@ -0,0 +1 @@
+pyrealsense2.cpython-38-x86_64-linux-gnu.so.2.45