Note

This notebook can be downloaded here: Projet+calibration-Paul.ipynb

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
imagesFolder = "E:\Desktop\S8\Projet 851\data"

aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)

fig = plt.figure()
nx = 8
ny = 6
for i in range(1, nx*ny+1):
    ax = fig.add_subplot(ny,nx, i)
    img = aruco.drawMarker(aruco_dict,i-1, 700)
    plt.imshow(img, cmap = mpl.cm.gray, interpolation = "nearest")
    ax.axis("off")

plt.savefig(imagesFolder + "/markers.pdf")
plt.show()
#plt.close()
<IPython.core.display.Javascript object>
board = aruco.CharucoBoard_create(3, 3, 1, 0.8, aruco_dict)
imboard = board.draw((4000, 4000))
fig = plt.figure()
ax = fig.add_subplot(1,1,1)
plt.imshow(imboard, cmap = mpl.cm.gray, interpolation = "nearest")
ax.axis("off")
cv2.imwrite(imagesFolder + "/chessboard.tiff",imboard)
#plt.savefig(imagesFolder + "/chessboard.pdf")
plt.grid()
plt.show()
print("Imprimer le damier de calibration!")
<IPython.core.display.Javascript object>
Imprimer le damier de calibration!
import cv2
import math

videoFile = "E:/Desktop/S8/Projet 851/outpy.avi"
imagesFolder = "E:/Desktop/S8/Projet 851/data/"
cap = cv2.VideoCapture(videoFile)
frameRate = cap.get(5) #frame rate
while(cap.isOpened()):
    frameId = cap.get(1) #current frame number
    ret, frame = cap.read()
    if (ret != True):
        break
    if (frameId <150):
        filename = imagesFolder + "\image_" +  str(int(frameId)) + ".jpg"
        cv2.imwrite(filename, frame)
cap.release()
print ("Done!")
Done!
im = PIL.Image.open(imagesFolder + "\image_0.jpg")
fig = plt.figure()
ax = fig.add_subplot(1,1,1)
plt.imshow(im)
ax.axis('off')
plt.show()
<IPython.core.display.Javascript object>
def read_chessboards(images):
    """
    Charuco base pose estimation.
    """
    print("POSE ESTIMATION STARTS:")
    allCorners = []
    allIds = []
    decimator = 0

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

        if len(res[0])>0:
            res2 = cv2.aruco.interpolateCornersCharuco(res[0],res[1],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
    print("finished")
#%%time
images = [imagesFolder + f for f in os.listdir(imagesFolder) if f.startswith("image_")]
allCorners,allIds,imsize=read_chessboards(images)
POSE ESTIMATION STARTS:
=> Processing image E:/Desktop/S8/Projet 851/data/image_69.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_139.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_15.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_145.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_12.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_142.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_60.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_130.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_67.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_137.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_52.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_2.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_83.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_102.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_29.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_5.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_55.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_84.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_105.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_27.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_20.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_31.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_36.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_38.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_114.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_95.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_44.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_113.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_92.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_43.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_126.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_76.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_121.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_71.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_128.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_78.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_21.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_26.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_28.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_54.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_4.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_85.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_104.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_3.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_53.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_82.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_103.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_66.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_136.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_61.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_131.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_13.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_143.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_68.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_138.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_14.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_144.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_129.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_79.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_120.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_70.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_127.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_77.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_112.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_93.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_42.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_39.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_115.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_94.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_45.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_37.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_30.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_124.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_74.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_123.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_73.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_116.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_97.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_46.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_111.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_90.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_41.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_33.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_34.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_118.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_99.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_48.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_25.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_59.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_9.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_88.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_109.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_22.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_0.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_50.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_81.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_100.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_57.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_7.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_86.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_107.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_62.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_132.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_65.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_135.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_19.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_149.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_17.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_147.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_10.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_140.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_35.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_119.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_98.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_49.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_32.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_110.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_91.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_40.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_117.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_96.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_47.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_122.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_72.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_125.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_75.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_11.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_141.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_16.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_146.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_64.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_134.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_18.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_148.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_63.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_133.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_6.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_56.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_87.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_106.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_51.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_1.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_80.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_101.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_23.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_24.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_8.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_58.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_89.jpg
=> Processing image E:/Desktop/S8/Projet 851/data/image_108.jpg
def calibrate_camera(allCorners,allIds,imsize):
    """
    Calibrates the camera using the dected corners.
    """
    print("CAMERA CALIBRATION")

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

    distCoeffsInit = np.zeros((5,1))
    flags = (cv2.CALIB_USE_INTRINSIC_GUESS + 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
    print("finished")
%%time
ret, mtx, dist, rvecs, tvecs = calibrate_camera(allCorners,allIds,imsize)
ret
CAMERA CALIBRATION
Wall time: 12min 46s
mtx
array([[  1.46963466e+03,   0.00000000e+00,   2.63094117e+02],
       [  0.00000000e+00,   1.47297770e+03,   3.19127464e+02],
       [  0.00000000e+00,   0.00000000e+00,   1.00000000e+00]])
dist
array([[ -4.15557150e+00],
       [  8.04563425e+02],
       [  1.72644822e-01],
       [ -4.62914356e-02],
       [ -1.41439828e+04],
       [  4.99936408e+00],
       [ -2.89968864e+02],
       [  1.96691829e+04],
       [  0.00000000e+00],
       [  0.00000000e+00],
       [  0.00000000e+00],
       [  0.00000000e+00],
       [  0.00000000e+00],
       [  0.00000000e+00]])
np.savetxt(imagesFolder+"calib_mtx_webcam.csv", mtx)
np.savetxt(imagesFolder+"calib_dist_webcam.csv", dist)

Check calibration

i=24 # select image id
plt.figure()
frame = cv2.imread(imagesFolder + "image_100.jpg".format(i))
img_undist = cv2.undistort(frame,mtx,dist,None)
plt.subplot(211)
plt.imshow(frame)
plt.title("Raw image")
plt.axis("off")
plt.subplot(212)
plt.imshow(img_undist)
plt.title("Corrected image")
plt.axis("off")
plt.show()
<IPython.core.display.Javascript object>

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

frame = cv2.imread(imagesFolder + "image_10.jpg")
plt.figure()
plt.imshow(frame)
plt.show()
<IPython.core.display.Javascript object>