Note

This notebook can be downloaded here: aruco_calibration_rotation.ipynb

Camera calibration using CHARUCO

import numpy as np
import cv2, PIL, os
from cv2 import aruco
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import matplotlib as mpl
import pandas as pd
%matplotlib nbagg

2. Camera pose estimation using CHARUCO chessboard

First, let’s create the board.

workdir = "./workdir/"
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
board = aruco.CharucoBoard_create(7, 5, 1, .8, aruco_dict)
imboard = board.draw((2000, 2000))
cv2.imwrite(workdir + "chessboard.tiff", imboard)
fig = plt.figure()
ax = fig.add_subplot(1,1,1)
plt.imshow(imboard, cmap = mpl.cm.gray, interpolation = "nearest")
ax.axis("off")
plt.show()
<IPython.core.display.Javascript object>

And take photos of it from multiple angles, for example:

datadir = "../../data/calib_tel_ludo/"
images = np.array([datadir + f for f in os.listdir(datadir) if f.endswith(".png") ])
order = np.argsort([int(p.split(".")[-2].split("_")[-1]) for p in images])
images = images[order]
images
array(['../../data/calib_tel_ludo/VID_20180406_085421_0.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_5.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_10.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_15.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_20.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_25.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_30.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_35.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_40.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_45.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_50.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_55.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_60.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_65.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_70.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_75.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_80.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_85.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_90.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_95.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_100.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_105.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_110.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_115.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_120.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_125.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_130.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_135.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_140.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_145.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_150.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_155.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_160.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_165.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_170.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_175.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_180.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_185.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_190.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_195.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_200.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_205.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_210.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_215.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_220.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_225.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_230.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_235.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_240.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_245.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_250.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_255.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_260.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_265.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_270.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_275.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_280.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_285.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_290.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_295.png',
       '../../data/calib_tel_ludo/VID_20180406_085421_300.png'],
      dtype='<U53')
im = PIL.Image.open(images[0])
fig = plt.figure()
ax = fig.add_subplot(1,1,1)
plt.imshow(im)
#ax.axis('off')
plt.show()
<IPython.core.display.Javascript object>

Now, the camera calibration can be done using all the images of the chessboard. Two functions are necessary:

  • The first will detect markers on all the images and.
  • The second will proceed the detected markers to estimage the camera calibration data.
def read_chessboards(images):
    """
    Charuco base pose estimation.
    """
    print("POSE ESTIMATION STARTS:")
    allCorners = []
    allIds = []
    decimator = 0
    # SUB PIXEL CORNER DETECTION CRITERION
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001)

    for im in images:
        print("=> Processing image {0}".format(im))
        frame = cv2.imread(im)
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(gray, aruco_dict)

        if len(corners)>0:
            # SUB PIXEL DETECTION
            for corner in corners:
                cv2.cornerSubPix(gray, corner,
                                 winSize = (3,3),
                                 zeroZone = (-1,-1),
                                 criteria = criteria)
            res2 = cv2.aruco.interpolateCornersCharuco(corners,ids,gray,board)
            if res2[1] is not None and res2[2] is not None and len(res2[1])>3 and decimator%1==0:
                allCorners.append(res2[1])
                allIds.append(res2[2])

        decimator+=1

    imsize = gray.shape
    return allCorners,allIds,imsize
allCorners,allIds,imsize=read_chessboards(images)
POSE ESTIMATION STARTS:
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_0.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_5.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_10.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_15.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_20.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_25.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_30.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_35.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_40.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_45.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_50.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_55.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_60.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_65.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_70.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_75.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_80.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_85.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_90.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_95.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_100.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_105.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_110.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_115.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_120.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_125.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_130.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_135.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_140.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_145.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_150.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_155.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_160.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_165.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_170.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_175.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_180.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_185.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_190.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_195.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_200.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_205.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_210.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_215.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_220.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_225.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_230.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_235.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_240.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_245.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_250.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_255.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_260.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_265.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_270.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_275.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_280.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_285.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_290.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_295.png
=> Processing image ../../data/calib_tel_ludo/VID_20180406_085421_300.png
def calibrate_camera(allCorners,allIds,imsize):
    """
    Calibrates the camera using the dected corners.
    """
    print("CAMERA CALIBRATION")

    cameraMatrixInit = np.array([[ 1000.,    0., imsize[0]/2.],
                                 [    0., 1000., imsize[1]/2.],
                                 [    0.,    0.,           1.]])

    distCoeffsInit = np.zeros((5,1))
    flags = (cv2.CALIB_USE_INTRINSIC_GUESS + cv2.CALIB_RATIONAL_MODEL + cv2.CALIB_FIX_ASPECT_RATIO)
    #flags = (cv2.CALIB_RATIONAL_MODEL)
    (ret, camera_matrix, distortion_coefficients0,
     rotation_vectors, translation_vectors,
     stdDeviationsIntrinsics, stdDeviationsExtrinsics,
     perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
                      charucoCorners=allCorners,
                      charucoIds=allIds,
                      board=board,
                      imageSize=imsize,
                      cameraMatrix=cameraMatrixInit,
                      distCoeffs=distCoeffsInit,
                      flags=flags,
                      criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 10000, 1e-9))

    return ret, camera_matrix, distortion_coefficients0, rotation_vectors, translation_vectors
%time ret, mtx, dist, rvecs, tvecs = calibrate_camera(allCorners,allIds,imsize)
CAMERA CALIBRATION
CPU times: user 10.3 s, sys: 8.89 s, total: 19.2 s
Wall time: 5.26 s
ret
0.6363938527748627
mtx
array([[1.78952655e+03, 0.00000000e+00, 9.69572430e+02],
       [0.00000000e+00, 1.78952655e+03, 5.64872516e+02],
       [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
dist
array([[ 5.33659854e+00],
       [-1.67904382e+02],
       [ 3.32943561e-03],
       [-4.67385863e-03],
       [ 9.75622127e+02],
       [ 5.14691206e+00],
       [-1.66105367e+02],
       [ 9.69643912e+02],
       [ 0.00000000e+00],
       [ 0.00000000e+00],
       [ 0.00000000e+00],
       [ 0.00000000e+00],
       [ 0.00000000e+00],
       [ 0.00000000e+00]])

Check calibration results

i=20 # select image id
plt.figure()
frame = cv2.imread(images[i])
img_undist = cv2.undistort(frame,mtx,dist,None)
plt.subplot(1,2,1)
plt.imshow(frame)
plt.title("Raw image")
plt.axis("off")
plt.subplot(1,2,2)
plt.imshow(img_undist)
plt.title("Corrected image")
plt.axis("off")
plt.show()
<IPython.core.display.Javascript object>

3 . Use of camera calibration to estimate 3D translation and rotation of each marker on a scene

frame = cv2.imread("../../data/IMG_20180406_095219.jpg")
#frame = cv2.undistort(src = frame, cameraMatrix = mtx, distCoeffs = dist)
plt.figure()
plt.imshow(frame, interpolation = "nearest")
plt.show()
<IPython.core.display.Javascript object>

Post processing

gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters =  aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict,
                                                      parameters=parameters)
# SUB PIXEL DETECTION
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.0001)
for corner in corners:
    cv2.cornerSubPix(gray, corner, winSize = (3,3), zeroZone = (-1,-1), criteria = criteria)

frame_markers = aruco.drawDetectedMarkers(frame.copy(), corners, ids)

corners
[array([[[1211.    , 1744.    ],
         [1002.    , 1678.    ],
         [1095.    , 1553.    ],
         [1298.5002, 1611.7025]]], dtype=float32),
 array([[[1067.8948, 1503.2638],
         [ 880.    , 1447.    ],
         [ 971.8308, 1339.5516],
         [1155.9335, 1390.4458]]], dtype=float32),
 array([[[1589., 2408.],
         [1330., 2308.],
         [1423., 2120.],
         [1671., 2208.]]], dtype=float32),
 array([[[2033., 2261.],
         [1772., 2174.],
         [1835., 2005.],
         [2083., 2083.]]], dtype=float32),
 array([[[ 935., 2158.],
         [ 706., 2076.],
         [ 827., 1911.],
         [1046., 1986.]]], dtype=float32),
 array([[[1378., 2036.],
         [1153., 1957.],
         [1245., 1810.],
         [1460., 1882.]]], dtype=float32),
 array([[[ 348., 1942.],
         [ 144., 1867.],
         [ 291., 1725.],
         [ 484., 1792.]]], dtype=float32),
 array([[[1782., 1928.],
         [1556., 1853.],
         [1624., 1717.],
         [1839., 1783.]]], dtype=float32),
 array([[[ 798., 1837.],
         [ 597., 1770.],
         [ 713., 1637.],
         [ 906., 1700.]]], dtype=float32),
 array([[[2154., 1823.],
         [1930., 1755.],
         [1977., 1630.],
         [2188., 1691.]]], dtype=float32),
 array([[[1580., 1650.],
         [1381., 1590.],
         [1449., 1482.],
         [1641., 1535.]]], dtype=float32),
 array([[[ 273., 1658.],
         [  98., 1592.],
         [ 231., 1478.],
         [ 403., 1539.]]], dtype=float32),
 array([[[ 688., 1574.],
         [ 509., 1517.],
         [ 617., 1412.],
         [ 790., 1465.]]], dtype=float32),
 array([[[1415.4037, 1431.0923],
         [1225.0386, 1377.4968],
         [1300.2623, 1279.6166],
         [1483.125 , 1329.8298]]], dtype=float32),
 array([[[ 597.94867, 1363.2643 ],
         [ 421.2595 , 1307.9504 ],
         [ 535.3967 , 1211.2885 ],
         [ 704.76355, 1262.5137 ]]], dtype=float32),
 array([[[ 949.5966, 1301.637 ],
         [ 775.0423, 1250.5741],
         [ 867.6455, 1160.6498],
         [1035.8293, 1207.2653]]], dtype=float32),
 array([[[1929.4287, 1575.4489],
         [1717.393 , 1515.709 ],
         [1772.9988, 1407.4595],
         [1975.8889, 1461.9364]]], dtype=float32)]

Very fast processing !

Results

plt.figure()
plt.imshow(frame_markers, interpolation = "nearest")

plt.show()
<IPython.core.display.Javascript object>