マーカーを用いたカメラ位置姿勢推定(OpenCV+ArUco)
ArUcoのマーカー種類
- SingleMarker
- Board
- ChessBoard
- Diamond
Single
マーカの一辺の長さを指定。 マーカとカメラ間の位置関係を求める。
Board
マーカの一辺の長さとマーカ間の長さを指定。 ボードとカメラ間の位置関係を求める。 単一マーカより精度は良い。
Chess
四角形の一辺の長さとマーカの一辺の長さを指定。 チェッカの交点を求める。
Diamond
四角形の一辺の長さとマーカの一辺の長さを指定。 ダイアモンドとカメラ間の位置関係を求める
参考: すごくわかりやすい↓
OpenCVarucoマーカ - プログラム関連の個人的メモ
ArUcoを使った位置姿勢推定
手順
- cameraのキャリブレーション
- マーカー作成・検出
- 位置姿勢推定
OpenCV: Detection of ArUco Markers
OpenCV: ArUco Marker Detection
キャリブレーション
ArUcoで姿勢推定を行うにはカメラの内部パラメータと歪パラメータ(cameraMatrix,distCoeffs)が必要
OpenCVでのカメラキャリブの説明
cameraMatrix and distCoeffs are the camera calibration parameters that need to be known a priori.
To perform camera pose estimation you need to know the calibration parameters of your camera. This is the camera matrix and distortion coefficients. If you do not know how to calibrate your camera, you can take a look to the calibrateCamera() function and the Calibration tutorial of OpenCV. You can also calibrate your camera using the aruco module as it is explained in the Calibration with aruco tutorial. Note that this only need to be done once unless the camera optics are modified (for instance changing its focus).
やり方
キャリブしたいカメラでチェスボードを用いて20枚くらいの写真を取る
その写真を用いて以下のようなスクリプトを実行
opencv/samples/cpp at master · kipr/opencv · GitHub ←ここのsample画像を用いて試してみる
import numpy as np import cv2 import glob # termination criteria criteria = (cv2.TERM_CRITERIA_EPS + cv2.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('left*.jpg') for fname in images: img = cv2.imread(fname) gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) # Find the chess board corners ret, corners = cv2.findChessboardCorners(gray, (7,6),None) # If found, add object points, image points (after refining them) if ret == True: objpoints.append(objp) corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria) imgpoints.append(corners2) # Draw and display the corners img = cv2.drawChessboardCorners(img, (7,6), corners2,ret) cv2.imshow('img',img) cv2.waitKey(500) ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None) #cameraMatrix -> mtx , distCoeffs -> dist print mtx, dist cv2.destroyAllWindows()
結果:
mtx = [[ 534.07088318 0. 341.53407683] [ 0. 534.11914504 232.94565267] [ 0. 0. 1. ]] dist = [[ -2.92971622e-01 1.07706883e-01 1.31038412e-03 -3.11032204e-05 4.34799808e-02]]
Calibrationの結果を今後も再利用したければ,Numpyの関数(np.savez, np.savetxt 等)を使ってカメラ行列とレンズ歪みパラメータを保存できる。
参考:
OpenCVのundistort(レンズ歪み補正)で端っこが欠けてしまうのをなんとかする - Qiita
カメラキャリブレーション — OpenCV-Python Tutorials 1 documentation
OpenCV: Camera Calibration and 3D Reconstruction
マーカー作成・検出
#encoding:utf-8 import cv2 aruco = cv2.aruco # DICT_4X4_50は4x4の格子でマーカ作成、ID50個 # drawMarker(dictionary, marker ID, marker_size[pix]) dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) marker = aruco.drawMarker(dictionary, 4, 100) cv2.imwrite('ar_marker.png', marker) img = cv2.imread('ar_marker.png') img = cv2.resize(img, None, fx=0.05, fy=0.05) corners, ids, rejectedImgPoints = aruco.detectMarkers(img, dictionary) aruco.drawDetectedMarkers(img, corners, ids, (0,255,0)) cv2.imwrite('DetectedMarkers.png', img)
位置姿勢推定(TODO:pytho化)
参考になるコード
// if at least one marker is detected if (ids.size() > 0){ // single marker if (markerType == ARUCO_SingleMarker){ // draw id cv::aruco::drawDetectedMarkers(undistImage, corners, ids); // pose estimation rvecsSingle.clear(); tvecsSingle.clear(); cv::aruco::estimatePoseSingleMarkers(corners, markerSingleSize, intrinsic, distortion, rvecsSingle, tvecsSinge); // draw axis for (int ii = 0; ii < ids.size(); ii++) cv::aruco::drawAxis(undistImage, intrinsic, distortion, rvecsSingle[ii], tvecsSingle[ii], 0.1); } // board else if (markerType == ARUCO_Board){ cv::aruco::drawDetectedMarkers(undistImage, corners, ids); int valid = cv::aruco::estimatePoseBoard(corners, ids, markerBoard, intrinsic, distortion, rvecBoard, tvecBoad); // if at least one board marker detected if (valid > 0) cv::aruco::drawAxis(undistImage, intrinsic, distortion, rvecBoard, tvecBoard, 0.1); }
Single
cv::aruco::estimatePoseSingleMarkers
マーカの位置推定を行う。 rvecs, tvecsにそれぞれのマーカとの位置関係が入っている。
rが回転成分、tが直進成分。回転成分に関してはrodrigues。 http://opencv.jp/opencv-2svn/cpp/camera_calibratio…
Board
cv::aruco::estimatePoseBoard
見つけたマーカ全部の情報を使ってボードの位置推定を行う。 マーカがある程度隠れていてもOKで精度も良い。
参考:
OpenCVarucoマーカ - プログラム関連の個人的メモ
全体参考
OpenCV: ArUco Marker Detection
arucoモジュールでマーカーを検出する · atinfinity/lab Wiki · GitHub
開発メモ その59 OpenCV 3.2 with ContribモジュールでArUcoを使用する - A certain engineer "COMPLEX"