Extrinsic calibration inital scripts

VarunBurde · VarunBurde · commit ea3810e38a76 · 2023-03-07T21:02:47.000+01:00
diff --git a/cv_pick_place/extrinsic_calibration.py b/cv_pick_place/extrinsic_calibration.py
@@ -0,0 +1,331 @@
+import os
+import argparse
+import sys
+# file_path_root = os.path.split(os.path.split(__file__)[0])[0]
+# sys.path.append('file_path_root')
+
+from robot_cell.detection.realsense_depth import DepthCamera
+from robot_cell.detection.packet_detector import PacketDetector
+ROB_CONFIG_FILE = os.path.join("config", "robot_config.json")
+import json
+from robot_cell.detection.threshold_detector import ThresholdDetector
+from robot_cell.detection.apriltag_detection import ProcessingApriltag
+import numpy as np
+import cv2
+
+def rotation_angles(matrix, order):
+    """
+    input
+        matrix = 3x3 rotation matrix (numpy array)
+        oreder(str) = rotation order of x, y, z : e.g, rotation XZY -- 'xzy'
+    output
+        theta1, theta2, theta3 = rotation angles in rotation order
+    """
+    r11, r12, r13 = matrix[0]
+    r21, r22, r23 = matrix[1]
+    r31, r32, r33 = matrix[2]
+
+    if order == 'xzx':
+        theta1 = np.arctan(r31 / r21)
+        theta2 = np.arctan(r21 / (r11 * np.cos(theta1)))
+        theta3 = np.arctan(-r13 / r12)
+
+    elif order == 'xyx':
+        theta1 = np.arctan(-r21 / r31)
+        theta2 = np.arctan(-r31 / (r11 *np.cos(theta1)))
+        theta3 = np.arctan(r12 / r13)
+
+    elif order == 'yxy':
+        theta1 = np.arctan(r12 / r32)
+        theta2 = np.arctan(r32 / (r22 *np.cos(theta1)))
+        theta3 = np.arctan(-r21 / r23)
+
+    elif order == 'yzy':
+        theta1 = np.arctan(-r32 / r12)
+        theta2 = np.arctan(-r12 / (r22 *np.cos(theta1)))
+        theta3 = np.arctan(r23 / r21)
+
+    elif order == 'zyz':
+        theta1 = np.arctan(r23 / r13)
+        theta2 = np.arctan(r13 / (r33 *np.cos(theta1)))
+        theta3 = np.arctan(-r32 / r31)
+
+    elif order == 'zxz':
+        theta1 = np.arctan(-r13 / r23)
+        theta2 = np.arctan(-r23 / (r33 *np.cos(theta1)))
+        theta3 = np.arctan(r31 / r32)
+
+    elif order == 'xzy':
+        theta1 = np.arctan(r32 / r22)
+        theta2 = np.arctan(-r12 * np.cos(theta1) / r22)
+        theta3 = np.arctan(r13 / r11)
+
+    elif order == 'xyz':
+        theta1 = np.arctan(-r23 / r33)
+        theta2 = np.arctan(r13 * np.cos(theta1) / r33)
+        theta3 = np.arctan(-r12 / r11)
+
+    elif order == 'yxz':
+        theta1 = np.arctan(r13 / r33)
+        theta2 = np.arctan(-r23 * np.cos(theta1) / r33)
+        theta3 = np.arctan(r21 / r22)
+
+    elif order == 'yzx':
+        theta1 = np.arctan(-r31 / r11)
+        theta2 = np.arctan(r21 * np.cos(theta1) / r11)
+        theta3 = np.arctan(-r23 / r22)
+
+    elif order == 'zyx':
+        theta1 = np.arctan(r21 / r11)
+        theta2 = np.arctan(-r31 * np.cos(theta1) / r11)
+        theta3 = np.arctan(r32 / r33)
+
+    elif order == 'zxy':
+        theta1 = np.arctan(-r12 / r22)
+        theta2 = np.arctan(r32 * np.cos(theta1) / r22)
+        theta3 = np.arctan(-r31 / r33)
+
+    theta1 = theta1 * 180 / np.pi
+    theta2 = theta2 * 180 / np.pi
+    theta3 = theta3 * 180 / np.pi
+
+    return (theta1, theta2, theta3)
+
+def drawText(
+    frame: np.ndarray, text: str, position: tuple[int, int], size: float = 1
+) -> None:
+    """
+    Draws white text with black border to the frame.
+
+    Args:
+        frame (np.ndarray): Frame into which the text will be draw.
+        text (str): Text to draw.
+        position (tuple[int, int]): Position on the frame in pixels.
+        size (float): Size modifier of the text.
+    """
+
+    cv2.putText(frame, text, position, cv2.FONT_HERSHEY_SIMPLEX, size, (0, 0, 0), 4)
+    cv2.putText(
+        frame, text, position, cv2.FONT_HERSHEY_SIMPLEX, size, (255, 255, 255), 2
+    )
+
+def bool_str(string: str) -> bool:
+    """
+    Used in argument parser to detect boolean flags written as a string.
+
+    Args:
+        string (str): String to be evaluated.
+
+    Returns:
+        bool: True, False, depending on contents of the string.
+
+    Raises:
+        argparse.ArgumentTypeError: Error in case the string does not contain any of the expected values.
+    """
+
+    if string in ["True", "true"]:
+        return True
+    elif string in ["False", "false"]:
+        return False
+    else:
+        raise argparse.ArgumentTypeError
+
+if __name__ == '__main__':
+
+    parser = argparse.ArgumentParser(description="Robot cell input arguments.")
+    parser.add_argument(
+        "--config-file",
+        default=ROB_CONFIG_FILE,
+        type=str,
+        dest="CONFIG_FILE",
+        help="Path to configuration file",
+    )
+    config, _ = parser.parse_known_args()
+
+    # Read config file using provided path
+    with open(config.CONFIG_FILE, "r") as file:
+        rob_config = json.load(file)
+
+    # Read all other input arguments as specified inside the config file
+    for param in rob_config.items():
+        if isinstance(param[1]["default"], bool):
+            parser.add_argument(
+                param[1]["arg"],
+                default=param[1]["default"],
+                dest=param[0],
+                help=param[1]["help"],
+                type=bool_str,
+            )
+        elif isinstance(param[1]["default"], list):
+            parser.add_argument(
+                param[1]["arg"],
+                default=param[1]["default"],
+                dest=param[0],
+                help=param[1]["help"],
+                nargs=len(param[1]["default"]),
+                type=type(param[1]["default"][0]),
+            )
+        elif isinstance(param[1]["default"], int):
+            parser.add_argument(
+                param[1]["arg"],
+                default=param[1]["default"],
+                dest=param[0],
+                help=param[1]["help"],
+                type=int,
+            )
+        elif isinstance(param[1]["default"], float):
+            parser.add_argument(
+                param[1]["arg"],
+                default=param[1]["default"],
+                dest=param[0],
+                help=param[1]["help"],
+                type=float,
+            )
+        elif isinstance(param[1]["default"], str):
+            parser.add_argument(
+                param[1]["arg"],
+                default=param[1]["default"],
+                dest=param[0],
+                help=param[1]["help"],
+                type=str,
+            )
+        else:
+            print(f"[WARNING] Default value of {param[0]} config parameter not handled")
+    rob_config = parser.parse_args()
+
+    # Read robot positions dictionaries from json file
+    with open(rob_config.path_robot_positions) as file:
+        robot_poses = json.load(file)
+
+
+    # Program variables
+    frame_count = 1  # Counter of frames for homography update
+    text_size = 1
+    homography = None  # Homography matrix
+
+    # Inititalize Apriltag Detector
+    apriltag = ProcessingApriltag()
+    apriltag.load_world_points(rob_config.path_homography_points)
+
+    detector = ThresholdDetector(
+        rob_config.hsv_ignore_vertical,
+        rob_config.hsv_ignore_horizontal,
+        rob_config.hsv_max_ratio_error,
+        rob_config.hsv_white_lower,
+        rob_config.hsv_white_upper,
+        rob_config.hsv_brown_lower,
+        rob_config.hsv_brown_upper,
+    )
+
+    camera = DepthCamera(config_path=rob_config.path_camera_config)
+    intrinsic = camera.intr
+    camera_parameter = [intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy]
+    fx, fy, cx, cy = camera_parameter
+    K = np.array([fx, 0, cx, 0, fy, cy, 0, 0, 1]).reshape(3, 3)
+    dist = np.array(camera.intr.coeffs)
+    print(K)
+
+    while True:
+
+        # Get frames from camera
+        success, depth_frame, rgb_frame, colorized_depth = camera.get_frames()
+        if not success:
+            continue
+
+
+        frame_height, frame_width, frame_channel_count = rgb_frame.shape
+        text_size = frame_height / 1000
+
+        # rgb_frame is used for detection, image_frame is used for graphics and displayed
+        image_frame = rgb_frame.copy()
+
+        # HOMOGRAPHY UPDATE
+        ##################
+
+        apriltag.detect_tags(rgb_frame)
+        homography = apriltag.compute_homog()
+
+        gray_frame = cv2.cvtColor(image_frame, cv2.COLOR_BGR2GRAY)
+        (corners, ids, rejected) = cv2.aruco.detectMarkers(
+            gray_frame,
+            cv2.aruco.Dictionary_get(cv2.aruco.DICT_APRILTAG_36h11),
+            parameters=cv2.aruco.DetectorParameters_create(),
+        )
+
+
+        transformation_marker = np.eye(4)
+
+        for (tag_corners, tag_id) in zip(corners, ids):
+
+
+            if tag_id == 1:
+                # Get (x, y) corners of the tag
+                corners = tag_corners.reshape((4, 2))
+                (top_left, top_right, bottom_right, bottom_left) = corners
+
+                top_left = (int(top_left[0]), int(top_left[1]))
+                top_right = (int(top_right[0]), int(top_right[1]))
+                bottom_right = (int(bottom_right[0]), int(bottom_right[1]))
+                bottom_left = (int(bottom_left[0]), int(bottom_left[1]))
+
+                # Compute centroid
+                cX = int((top_left[0] + bottom_right[0]) / 2.0)
+                cY = int((top_left[1] + bottom_right[1]) / 2.0)
+
+                cv2.circle(image_frame, (cX,cY), 10, (255, 0, 0), -1)
+
+                # TODO : check why 3d points from depth camera and tvec doesnt corresponds
+                marker_centroid = [cX, cY]
+                rvec, tvec, markerPoints =cv2.aruco.estimatePoseSingleMarkers(tag_corners, 0.05, K, dist)
+                tdp_marker_center = camera.pixel_to_3d_point(marker_centroid, camera.get_raw_depth_frame())
+
+                rotation_matrix, idk = cv2.Rodrigues(rvec)
+                angle_marker_w2c = rotation_angles(rotation_matrix, 'zyx')  # zxy
+                # print(angle_marker_w2c)
+
+                transformation_marker[:3, :3] = rotation_matrix
+                transformation_marker[:3,3:] = tvec.reshape(3,1)
+                transformation_marker = np.linalg.inv(transformation_marker)
+
+                # marker_point = camera.pixel_to_3d_point(marker_centroid, camera.get_raw_depth_frame())
+                # print(marker_point, tvec)
+
+                # TODO: save this transformation matrix
+
+        image_frame = apriltag.draw_tags(image_frame)
+        detector.set_homography(homography)
+        image_frame, detected_packets, mask = detector.detect_packet_hsv(rgb_frame,0,True)
+
+
+
+        for packet in detected_packets:
+            # Draw packet centroid value in milimeters
+            text_centroid = "X: {:.2f}, Y: {:.2f} (mm)".format(
+                packet.centroid_mm.x, packet.centroid_mm.y
+            )
+            drawText(
+                image_frame,
+                text_centroid,
+                (packet.centroid_px.x + 10, packet.centroid_px.y + int(80 * text_size)),
+                text_size,
+            )
+
+            pixel = [packet.centroid_px.x, packet.centroid_px.y]
+            threed_point = np.array(camera.pixel_to_3d_point(pixel, camera.get_raw_depth_frame())).reshape(3, 1)
+            threed_point = np.append(threed_point, 1)
+
+
+            transformed_3d_point = np.matmul(transformation_marker, threed_point)
+            print(transformed_3d_point * 1000)
+
+        image_frame = cv2.resize(image_frame, (960, 540))
+        cv2.imshow('window_name', image_frame)
+
+        # Press Q on keyboard to  exit
+        if cv2.waitKey(25) & 0xFF == ord('q'):
+            break
+
+        # closing all open windows
+    cv2.destroyAllWindows()
+
+
diff --git a/cv_pick_place/robot_cell/detection/realsense_depth.py b/cv_pick_place/robot_cell/detection/realsense_depth.py
@@ -120,7 +120,7 @@ def __init__(
         # Maximal supported Depth stream resolution of D435 camera is 1280 x 720
         # Maximal supported RGB stream resolution of D435 camera is 1920 x 1080
         self.config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
-        self.config.enable_stream(rs.stream.color, 1920, 1080, rs.format.bgr8, 30)
+        self.config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
 
         # Create object for aligning depth frame to RGB frame, so that they have equal resolution
         self.align = rs.align(rs.stream.color)
@@ -135,6 +135,12 @@ def __init__(
         # Start video stream
         self.profile = self.pipeline.start(self.config)
 
+        # Get intrinsic parameter
+        profile = self.profile.get_stream(rs.stream.depth)  # Fetch stream profile for depth stream
+        self.intr = profile.as_video_stream_profile().get_intrinsics()  # Downcast to video_stream_profile and fetch intrinsics
+        # self.depth_scale = profile.get_device().first_depth_sensor().get_depth_scale()
+
+
     def get_frames(self) -> tuple[bool, np.ndarray, np.ndarray, np.ndarray]:
         """
         Reads and processes frames from connected camera.
@@ -152,6 +158,7 @@ def get_frames(self) -> tuple[bool, np.ndarray, np.ndarray, np.ndarray]:
 
         # Extract RGB and depth frames from frameset
         depth_frame = frameset.get_depth_frame()
+        depth_frame_raw = depth_frame
         color_frame = frameset.get_color_frame()
 
         if not depth_frame or not color_frame:
@@ -160,6 +167,7 @@ def get_frames(self) -> tuple[bool, np.ndarray, np.ndarray, np.ndarray]:
         # Apply hole filling filter
         depth_frame = self.hole_filling.process(depth_frame)
 
+
         depth_frame = np.asanyarray(depth_frame.get_data())
         color_frame = np.asanyarray(color_frame.get_data())
 
@@ -171,6 +179,19 @@ def get_frames(self) -> tuple[bool, np.ndarray, np.ndarray, np.ndarray]:
 
         return True, depth_frame, color_frame, colorized_depth_frame
 
+    def get_raw_depth_frame(self):
+        frameset = self.pipeline.wait_for_frames()
+        frameset = self.align.process(frameset)
+        return frameset.get_depth_frame()
+
+    def pixel_to_3d_point(self, pixel, depth_frame):
+
+        dist = depth_frame.get_distance(pixel[0], pixel[1])
+        coordinates_3d = rs.rs2_deproject_pixel_to_point(self.intr, [pixel[0], pixel[1]], dist)
+        coordinates_3d = [coordinates_3d[0],coordinates_3d[1],coordinates_3d[2]]
+
+        return coordinates_3d
+
     def release(self):
         """
         Disconnects the camera.
diff --git a/requirements.txt b/requirements.txt
