Added Unsafe calibration scripts

scripts/camera_calibration/D435_calibration.py

@@ -0,0 +1,89 @@
+"""_summary_
+    camera_calibration.
+"""
+
+## Import
+from multiprocessing import Condition
+import pyrealsense2 as rs
+import numpy as np
+import cv2
+
+# Configure depth and color streams
+pipeline = rs.pipeline()
+config = rs.config()
+
+# Get device product line for setting a supporting resolution
+pipeline_wrapper = rs.pipeline_wrapper(pipeline)
+pipeline_profile = config.resolve(pipeline_wrapper)
+device = pipeline_profile.get_device()
+print(device)
+device_product_line = str(device.get_info(rs.camera_info.product_line))
+
+# Whether the camera supports RGB videos
+found_rgb = False
+for s in device.sensors:
+    if s.get_info(rs.camera_info.name) == 'RGB Camera':
+        found_rgb = True
+        break
+if not found_rgb:
+    print("The demo requires Depth camera with Color sensor")
+    exit(0)
+
+# set the configuration for streaming
+config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
+
+if device_product_line == 'L500':
+    config.enable_stream(rs.stream.color, 960, 540, rs.format.bgr8, 30)
+else:
+    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
+
+
+## --- Start streaming --- ##
+pipeline.start(config)
+
+# TODO: Launch video saving
+video_saving_option = False
+if video_saving_option: result = cv2.VideoWriter('ball bouncing.avi', cv2.VideoWriter_fourcc(*'MJPG'), 10, (640, 480))
+
+try:
+    while True:
+
+        # Wait for a coherent pair of frames: depth and color
+        frames = pipeline.wait_for_frames()
+        depth_frame = frames.get_depth_frame()
+        color_frame = frames.get_color_frame()
+        if not depth_frame or not color_frame:
+            continue
+
+        # Convert images to numpy arrays
+        depth_image = np.asanyarray(depth_frame.get_data())
+        rgb_image = np.asanyarray(color_frame.get_data())
+
+        # Apply colormap on depth image (image must be converted to 8-bit per pixel first)
+        depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
+
+        depth_colormap_dim = depth_colormap.shape
+        color_colormap_dim = rgb_image.shape
+
+        # If depth and color resolutions are different, resize color image to match depth image for display
+        if depth_colormap_dim != color_colormap_dim:
+            resized_color_image = cv2.resize(rgb_image, dsize=(depth_colormap_dim[1], depth_colormap_dim[0]), interpolation=cv2.INTER_AREA)
+            images = np.hstack((resized_color_image, depth_colormap))
+        else:
+            images = np.hstack((rgb_image, depth_colormap))
+
+        # Show images
+        cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
+        cv2.imshow('RealSense', images)
+        cv2.waitKey(1)
+
+        # Output video
+        if video_saving_option: result.write(filtered_rgb_image)
+
+finally:
+
+    # Stop streaming
+    pipeline.stop()
+
+    # Save video
+    if video_saving_option: result.release()

scripts/camera_calibration/distortion_correction.py

@@ -0,0 +1,42 @@
+import numpy as np
+import cv2 as cv
+import glob
+
+# termination criteria
+criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
+# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
+objp = np.zeros((6*7,3), np.float32)
+objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)
+# Arrays to store object points and image points from all the images.
+objpoints = [] # 3d point in real world space
+imgpoints = [] # 2d points in image plane.
+images = glob.glob('*.jpg')
+for fname in images:
+    img = cv.imread(fname)
+    gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
+    # Find the chess board corners
+    ret, corners = cv.findChessboardCorners(gray, (7,6), None)
+    # If found, add object points, image points (after refining them)
+    if ret == True:
+        objpoints.append(objp)
+        corners2 = cv.cornerSubPix(gray,corners, (11,11), (-1,-1), criteria)
+        imgpoints.append(corners)
+        # Draw and display the corners
+        cv.drawChessboardCorners(img, (7,6), corners2, ret)
+        cv.imshow('img', img)
+        cv.waitKey(500)
+cv.destroyAllWindows()
+
+def distortion_correction():
+    """_summary_
+        https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html
+        For Intel RealSense D435 camera, its two infrared streams have no distortion.
+        Therefore, this specific function doesn't not necessarily need to be used.
+    Returns:
+        _type_: _description_
+    """
+
+
+    return 0
+
+distortion_correction()

scripts/camera_calibration/point_cloud_calibration.py

@@ -0,0 +1,126 @@
+"""
+opencv_pose_estimator_example.py
+Description:
+    This script should open up a live feed and then perform pose estimation on the objects in the frame
+    for the two tags that are currently pasted on the block:
+        #3
+        #5
+    from the tagStandard41h12 directory of apriltags3.
+"""
+## License: Apache 2.0. See LICENSE file in root directory.
+## Copyright(c) 2015-2017 Intel Corporation. All Rights Reserved.
+
+###############################################
+##      Open CV and Numpy integration        ##
+###############################################
+
+import pyrealsense2 as rs
+import numpy as np
+import cv2
+from dt_apriltags import Detector
+
+# Configure depth and color streams
+pipeline = rs.pipeline()
+config = rs.config()
+
+# Get device product line for setting a supporting resolution
+pipeline_wrapper = rs.pipeline_wrapper(pipeline)
+pipeline_profile = config.resolve(pipeline_wrapper)
+device = pipeline_profile.get_device()
+device_product_line = str(device.get_info(rs.camera_info.product_line))
+
+found_rgb = False
+for s in device.sensors:
+    if s.get_info(rs.camera_info.name) == 'RGB Camera':
+        found_rgb = True
+        break
+if not found_rgb:
+    print("The demo requires Depth camera with Color sensor")
+    exit(0)
+
+config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
+
+if device_product_line == 'L500':
+    config.enable_stream(rs.stream.color, 960, 540, rs.format.bgr8, 30)
+else:
+    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
+
+# Configure april tag detector
+# detector = apriltag.Detector("tagStandard41h12")
+# AprilTag detector options
+# options = apriltag.DetectorOptions(families='tag41h12',
+#                                 border=1,
+#                                 nthreads=4,
+#                                 quad_decimate=1.0,
+#                                 quad_blur=0.0,
+#                                 refine_edges=True,
+#                                 refine_decode=False,
+#                                 refine_pose=True,
+#                                 debug=False,
+#                                 quad_contours=True)
+# detector = apriltag.Detector(options)
+at_detector = Detector(families='tagStandard41h12',
+                        nthreads=1,
+                        quad_decimate=1.0,
+                        quad_sigma=0.0,
+                        refine_edges=1,
+                        decode_sharpening=0.25,
+                        debug=0)
+
+# camera parameters
+cam_params0 = [ 386.738, 386.738, 321.281, 238.221 ]
+tag_size0 = 0.040084375
+
+# Start streaming
+pipeline.start(config)
+
+try:
+    while True:
+
+        # Wait for a coherent pair of frames: depth and color
+        frames = pipeline.wait_for_frames()
+        depth_frame = frames.get_depth_frame()
+        color_frame = frames.get_color_frame()
+        if not depth_frame or not color_frame:
+            continue
+
+        # Convert images to numpy arrays
+        depth_image = np.asanyarray(depth_frame.get_data())
+        color_image = np.asanyarray(color_frame.get_data())
+
+        # Apply colormap on depth image (image must be converted to 8-bit per pixel first)
+        depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
+
+        depth_colormap_dim = depth_colormap.shape
+        color_colormap_dim = color_image.shape
+
+        # If depth and color resolutions are different, resize color image to match depth image for display
+        if depth_colormap_dim != color_colormap_dim:
+            resized_color_image = cv2.resize(color_image, dsize=(depth_colormap_dim[1], depth_colormap_dim[0]), interpolation=cv2.INTER_AREA)
+            images = np.hstack((resized_color_image, depth_colormap))
+        else:
+            images = np.hstack((color_image, depth_colormap))
+
+        # Show images
+        cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
+        cv2.imshow('RealSense', images)
+        cv2.waitKey(1)
+
+        # Print whether or not detector detects anything.
+        gray_image = cv2.cvtColor(color_image,cv2.COLOR_BGR2GRAY)
+        print(
+            at_detector.detect(
+                gray_image,
+                estimate_tag_pose=True,
+                camera_params=cam_params0,
+                tag_size= tag_size0
+                )
+            )
+        print('intrinsics')
+        print(pipeline_profile)
+        print(depth_frame.profile.as_video_stream_profile().intrinsics)
+
+finally:
+
+    # Stop streaming
+    pipeline.stop()

scripts/camera_calibration/readme.md

@@ -0,0 +1,45 @@
+# Calibration
+This package contains a series of calibration for the Intel RealSense D435i camera.
+
+# Purpose
+Find the pose of an object in the camera frame and convert it into the base frame.
+
+# Procedure
+1. Correct distortion transformation [Camera Calibration](https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html)
+2. Find transformation from pixel to 3D point
+3. Find transformation from 3D point to base frame
+
+# Spec of D435i
+Ideal range:
+.3 m to 3 m
+
+Depth Field of View (FOV):
+87° × 58° 
+
+Depth output resolution:
+Up to 1280 × 720 
+
+Depth frame rate:
+Up to 90 fps
+
+Depth technology:
+Stereoscopic 
+
+Minimum depth distance
+(Min‑Z) at max resolution:
+~28 cm 
+
+Depth Accuracy:
+<2% at 2 m
+
+RGB sensor FOV (H × V):
+69° × 42° 
+
+RGB sensor resolution:
+2 MP
+
+RGB frame resolution:
+1920 × 1080 
+
+RGB frame rate:
+30 fps