OSOYOO PI CARキット ROS2化 #13 – ORB_SLAM3 カメラキャリブレーション

ROS2とPiCarを一式Raspberry PI5に搭載したのですが、カメラを変更しました。
また画像フォーマットもMotion Jpegに変更しましたので、キャリブレーションすることにしました。
私がORB_SLAM3で使用するモードは、単眼SLAMですので、Intrinsic parametersの設定が必要となります。方法としては、OpenCVを使う方法とKalibrを使う方法があるようです。
今回は、簡単そうだったのでOpenCVを使って調整を行いました。
方法/原理は、OpenCV:Camera Calibrationに記載されています。
ORB_SLAM3では起動時に指定するyamlファイル内にintrinsic paramtersを記載してあります。
簡単に言えばレンズのゆがみ補正パラメータで、キャリブレーション用のパターンをカメラで撮影して求めます。

■ キャリブレーション用パターン
■intrinsic paramters計測プログラム
■ .yamlの作成
■ 次回

■ キャリブレーション用パターン

私は、下記の画像を使用しました。
OpenCV:Camera Calibrationでは7行８列のパターンを使用することになっていますが、A4にパターンを作成したら７行１０列が収まりがよかったので変更しました。

色々と調べると、このパターンの周りには白い部分がないと認識率が落ちるとのことでしたので、下記のようにホワイトボードに張り付けてキャリブレーションパターンとしました。

これを下記のように色々な角度で撮影して保存しておきます。

■intrinsic paramters計測プログラム

OpenCV:Camera Calibrationのサンプルプログラムを変更して７行１０列のパターンでintrinsic parametersを算出するプログラムを作成しました。
このプログラムは、プログラムと同一ディレクトリ内に置かれている画像を解析してパラメータを算出します。先に撮影した画像を同一ディレクトリに入れて実行します。

#!/usr/bin/python3
import numpy as np
import cv2 as cv
import glob

# demention of checkerboard.
dim_checker = (6,9)

# 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[:,:2] objp[:,:2] = np.mgrid[0:dim_checker[0],0:dim_checker[1]].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')
num_det = 0

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, dim_checker, 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(corners2)
        # Draw and display the corners
        cv.drawChessboardCorners(img, dim_checker, corners2, ret)
        num_det += 1
        cv.imshow('img', img)
        cv.waitKey(500)
        print("o:", fname)
    else:
        print("x:", fname)

# this does calibration.
if num_det > 0:
    ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(
        objpoints, imgpoints, gray.shape[::-1], None, None)
    print('============')
    print('fx={} fy={}'.format(mtx[0][0], mtx[1][1]))
    print('cx={} cy={}'.format(mtx[0][2], mtx[1][2]))
    print('k1={} k2={} k3={}'.format(dist[0][0], dist[0][1], dist[0][4]))
    print('p1={} p2={}'.format(dist[0][2], dist[0][3]))
    print('============')

cv.destroyAllWindows()

実行時には下記のような検出結果を表示しつつ、全画像の検出が終わると、intrinsc parametersを最後に表示します。この値を、yamlファイルに転記すれば撮影したカメラ用のyamlが作成できます。

============
fx=733.1180416702534 fy=736.6720146569185
cx=532.0877752312018 cy=359.8242822174343
k1=-0.35579589551207635 k2=0.211972943439992 k3=-0.1179475105338516
p1=0.0010161469599404165 p2=0.00015364494266238775
============

■ .yamlの作成

ORB_SAM3ではカメラ解像度、ゆがみ補正、SLAM動作モード設定の為に.yamlファイルが用意されています。サンプルにあったTUM1.yamlをベースに修正します。
先ほど求めた補正値、画像解像度、フレームレートを下記緑字のようにセットします。
今回、ORB_SLAM3には、1024×768で撮影します。
青字部は、撮影解像度が少し高くなったのとRaspberry PI5に変更したので、特徴点の検出を1000ポイントから3000ポイントに変更しました。

YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
File.version: "1.0"

Camera.type: "PinHole"

# Camera calibration and distortion parameters (OpenCV) 
Camera1.fx: 733.1180416702534
Camera1.fy: 736.6720146569185
Camera1.cx: 532.0877752312018
Camera1.cy: 359.8242822174343

Camera1.k1: -0.35579589551207635
Camera1.k2: 0.211972943439992
Camera1.p1: 0.0010161469599404165
Camera1.p2: 0.00015364494266238775
Camera1.k3: -0.1179475105338516

# Camera frames per second 
Camera.fps: 30
# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

# Camera resolution
Camera.width: 1024
Camera.height: 768

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 3000

# ORB Extractor: Scale factor between levels in the scale pyramid   
ORBextractor.scaleFactor: 1.2

# ORB Extractor: Number of levels in the scale pyramid  
ORBextractor.nLevels: 8

# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast           
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

#---------------------------------------------------------------------------------------
# Viewer Parameters
#---------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1.0
Viewer.GraphLineWidth: 0.9
Viewer.PointSize: 2.0
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3.0
Viewer.ViewpointX: 0.0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500.0