Camera Posture Estimation Using An ArUco Diamond Marker

Posted on Edited on In ,

Preparation

Very similar to our previous post Camera Posture Estimation Using A Single aruco Marker, you need to make sure your camera has already been calibrated. In the coding section, it’s assumed that you can successfully load the camera calibration parameters.

Coding

The code can be found at OpenCV Examples.

First of all

We need to ensure cv2.so is under our system path. cv2.so is specifically for OpenCV Python.

1
2
import sys
sys.path.append('/usr/local/python/3.5')

Then, we import some packages to be used.

1
2
3
4
import os
import cv2
from cv2 import aruco
import numpy as np

Secondly

Again, we need to load all camera calibration parameters, including: cameraMatrix, distCoeffs, etc. :

1
2
3
4
calibrationFile = "calibrationFileName.xml"
calibrationParams = cv2.FileStorage(calibrationFile, cv2.FILE_STORAGE_READ)
camera_matrix = calibrationParams.getNode("cameraMatrix").mat()
dist_coeffs = calibrationParams.getNode("distCoeffs").mat()

If you are using a calibrated fisheye camera like us, two extra parameters are to be loaded from the calibration file.

1
2
r = calibrationParams.getNode("R").mat()
new_camera_matrix = calibrationParams.getNode("newCameraMatrix").mat()

Afterwards, two mapping matrices are pre-calculated by calling function cv2.fisheye.initUndistortRectifyMap() as (supposing the images to be processed are of 1080P):

1
2
image_size = (1920, 1080)
map1, map2 = cv2.fisheye.initUndistortRectifyMap(camera_matrix, dist_coeffs, r, new_camera_matrix, image_size, cv2.CV_16SC2)

Thirdly

The dictionary aruco.DICT_6X6_250 is to be loaded. Although current OpenCV provides four groups of aruco patterns, 4X4, 5X5, 6X6, 7X7, etc., it seems OpenCV Python does NOT provide a function named drawCharucoDiamond(). Therefore, we have to refer to the C++ tutorial Detection of Diamond Markers. And, we directly use this particular diamond marker in the tutorial:

aruco.DICT_6X6_250.diamond 
1
aruco_dict = aruco.Dictionary_get( aruco.DICT_6X6_250 )

After having this aruco diamond marker printed, the edge lengths of this particular diamond marker are to be measured and stored in two variables squareLength and markerLength.

1
2
squareLength = 40   # Here, our measurement unit is centimetre.
markerLength = 25   # Here, our measurement unit is centimetre.

Meanwhile, create aruco detector with default parameters.

1
arucoParams = aruco.DetectorParameters_create()

Finally

This time, let’s test on a video stream, a .mp4 file. We first load the video file and initialize a video capture handle.

1
2
videoFile = "aruco_diamond.mp4"
cap = cv2.VideoCapture(videoFile)

Then, we calculate the camera posture frame by frame:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
while(True):
    ret, frame = cap.read() # Capture frame-by-frame
    if ret == True:
        frame_remapped = cv2.remap(frame, map1, map2, cv2.INTER_LINEAR, cv2.BORDER_CONSTANT)     # for fisheye remapping
        frame_remapped_gray = cv2.cvtColor(frame_remapped, cv2.COLOR_BGR2GRAY)

        corners, ids, rejectedImgPoints = aruco.detectMarkers(frame_remapped_gray, aruco_dict, parameters=arucoParams)  # First, detect markers

        if ids != None: # if there is at least one marker detected
            diamondCorners, diamondIds = aruco.detectCharucoDiamond(frame_remapped_gray, corners, ids, squareLength/markerLength)   # Second, detect diamond markers
            if len(diamondCorners) >= 1:    # if there is at least one diamond detected
                im_with_diamond = aruco.drawDetectedDiamonds(frame_remapped, diamondCorners, diamondIds, (0,255,0))
                rvec, tvec = aruco.estimatePoseSingleMarkers(diamondCorners, squareLength, camera_matrix, dist_coeffs)  # posture estimation from a diamond
                im_with_diamond = aruco.drawAxis(im_with_diamond, camera_matrix, dist_coeffs, rvec, tvec, 100)  # axis length 100 can be changed according to your requirement
        else:
            im_with_diamond = frame_remapped

        cv2.imshow("diamondLeft", im_with_diamond)   # display

        if cv2.waitKey(2) & 0xFF == ord('q'):   # press 'q' to quit
            break
    else:
        break

The drawn axis is just the world coordinators and orientations estimated from the images taken by the testing camera. At the end of the code, we release the video capture handle and destroy all opening windows.

1
2
cap.release()   # When everything done, release the capture
cv2.destroyAllWindows()