From 0435f84263984a8dc83ea478042248593882f281 Mon Sep 17 00:00:00 2001 From: Kwesi Rutledge Date: Thu, 7 Sep 2023 17:58:14 -0400 Subject: [PATCH] Added Unsafe calibration scripts --- .../camera_calibration/D435_calibration.py | 89 +++++++++++++ .../distortion_correction.py | 42 ++++++ .../point_cloud_calibration.py | 126 ++++++++++++++++++ scripts/camera_calibration/readme.md | 45 +++++++ 4 files changed, 302 insertions(+) create mode 100644 scripts/camera_calibration/D435_calibration.py create mode 100644 scripts/camera_calibration/distortion_correction.py create mode 100644 scripts/camera_calibration/point_cloud_calibration.py create mode 100644 scripts/camera_calibration/readme.md diff --git a/scripts/camera_calibration/D435_calibration.py b/scripts/camera_calibration/D435_calibration.py new file mode 100644 index 00000000..62002754 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/scripts/camera_calibration/distortion_correction.py b/scripts/camera_calibration/distortion_correction.py new file mode 100644 index 00000000..e7595254 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/scripts/camera_calibration/point_cloud_calibration.py b/scripts/camera_calibration/point_cloud_calibration.py new file mode 100644 index 00000000..06afa849 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/scripts/camera_calibration/readme.md b/scripts/camera_calibration/readme.md new file mode 100644 index 00000000..9c365b3a --- /dev/null +++ b/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 \ No newline at end of file