使用intelrealsense實時計算aruco marker位姿（Python）

目錄

• • 效果圖
  • 1.前期準備
  • • 1.1.硬體裝置
    • 1.2.線上Aruco Marker生成位址
    • 1.3.Aruco Marker資訊
  • 2.實作
  • • 2.1.Code
  • 問題彙總

效果圖

1.前期準備

1.1.硬體裝置

• IntelRealsense D435i

1.2.線上Aruco Marker生成位址

線上生成位址: https://chev.me/arucogen/

根據類型(4x4、5x5、6x6、7x7等)，尺寸，ID去生成Aruco碼，點選open另存為PDF或者直接列印

1.3.Aruco Marker資訊

4x4指去掉最外一層黑色，内部圖像由4x4個正方形組成

2.實作

2.1.Code

intelrealsense内參和畸變系數通過pyrealsense2讀取内部資訊擷取

import pyrealsense2 as rs
import numpy as np
import cv2
# 提示沒有aruco的看問題彙總
import cv2.aruco as aruco

# 配置攝像頭與開啟pipeline
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
profile = pipeline.start(config)
align_to = rs.stream.color
align = rs.align(align_to)

# 擷取對齊的rgb和深度圖
def get_aligned_images():
    frames = pipeline.wait_for_frames()
    aligned_frames = align.process(frames)
    aligned_depth_frame = aligned_frames.get_depth_frame()
    color_frame = aligned_frames.get_color_frame()
    # 擷取intelrealsense參數
    intr = color_frame.profile.as_video_stream_profile().intrinsics
    # 内參矩陣，轉ndarray友善後續opencv直接使用
    intr_matrix = np.array([
        [intr.fx, 0, intr.ppx], [0, intr.fy, intr.ppy], [0, 0, 1]
    ])
    # 深度圖-16位
    depth_image = np.asanyarray(aligned_depth_frame.get_data())
    # 深度圖-8位
    depth_image_8bit = cv2.convertScaleAbs(depth_image, alpha=0.03)
    pos = np.where(depth_image_8bit == 0)
    depth_image_8bit[pos] = 255
    # rgb圖
    color_image = np.asanyarray(color_frame.get_data())
    # return: rgb圖，深度圖，相機内參，相機畸變系數(intr.coeffs)
    return color_image, depth_image, intr_matrix, np.array(intr.coeffs)


if __name__ == "__main__":
    n = 0
    while 1:
        rgb, depth, intr_matrix, intr_coeffs = get_aligned_images()
        # 擷取dictionary, 4x4的碼，訓示位50個
        aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
        # 建立detector parameters
        parameters = aruco.DetectorParameters_create()
        # 輸入rgb圖, aruco的dictionary, 相機内參, 相機的畸變參數
        corners, ids, rejected_img_points = aruco.detectMarkers(rgb, aruco_dict, parameters=parameters,cameraMatrix=intr_matrix, distCoeff=intr_coeffs)
        # 估計出aruco碼的位姿，0.045對應markerLength參數，機關是meter
        # rvec是旋轉向量， tvec是平移向量
        rvec, tvec, markerPoints = aruco.estimatePoseSingleMarkers(corners, 0.045, intr_matrix, intr_coeffs)
        try:
        	# 在圖檔上标出aruco碼的位置
            aruco.drawDetectedMarkers(rgb, corners)
            # 根據aruco碼的位姿标注出對應的xyz軸, 0.05對應length參數，代表xyz軸畫出來的長度 
            aruco.drawAxis(rgb, intr_matrix, intr_coeffs, rvec, tvec, 0.05)
            cv2.imshow('RGB image', rgb)
        except:
            cv2.imshow('RGB image', rgb)
        key = cv2.waitKey(1)
        # 按鍵盤q退出程式
        if key & 0xFF == ord('q') or key == 27:
            pipeline.stop()
            break
        # 按鍵盤s儲存圖檔
        elif key == ord('s'):
            n = n + 1
            # 儲存rgb圖
            cv2.imwrite('./img/rgb' + str(n) + '.jpg', rgb)

    cv2.destroyAllWindows()

           

問題彙總

1. 提示cv2沒有aruco這個參數

   cv2.aruco在opencv-contrib-python這個版本的opencv，但可能會跟opencv-python的包名沖突。

pip uninstall opencv-python
pip uninstall opencv-contrib-python
pip install opencv-contrib-python