空飛ぶロボットのつくりかた

ロボットをつくるために必要な技術をまとめます。ロボットの未来についても考えたりします。

マーカーを用いたカメラ位置姿勢推定(OpenCV+ArUco)

ArUcoのマーカー種類

  • SingleMarker
  • Board
  • ChessBoard
  • Diamond

Single

マーカの一辺の長さを指定。 マーカとカメラ間の位置関係を求める。

Board

マーカの一辺の長さとマーカ間の長さを指定。 ボードとカメラ間の位置関係を求める。 単一マーカより精度は良い。

Chess

四角形の一辺の長さとマーカの一辺の長さを指定。 チェッカの交点を求める。

Diamond

四角形の一辺の長さとマーカの一辺の長さを指定。 ダイアモンドとカメラ間の位置関係を求める

参考: すごくわかりやすい↓

OpenCVarucoマーカ - プログラム関連の個人的メモ

ArUcoを使った位置姿勢推定

手順

  1. cameraのキャリブレーション
  2. マーカー作成・検出
  3. 位置姿勢推定

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).

やり方

  1. キャリブしたいカメラでチェスボードを用いて20枚くらいの写真を取る

  2. その写真を用いて以下のようなスクリプトを実行

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

カメラ校正 - 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マーカ - プログラム関連の個人的メモ

全体参考

ArUco マーカーの検出 - Qiita

arucoモジュール を試す - 機械学習備忘録

OpenCV: ArUco Marker Detection

arucoモジュールでマーカーを検出する · atinfinity/lab Wiki · GitHub

開発メモ その59 OpenCV 3.2 with ContribモジュールでArUcoを使用する - A certain engineer "COMPLEX"

OpenCVarucoマーカ - プログラム関連の個人的メモ