Camera Calibration

The Jetbot camera is supposed to be used to detect both the Aruco markers for navigating and the strawberries to pick with the developed robot arm. It is therefore crucial to calibrate the camera before starting to develop the image processing functionality. The calibration is carried out in 3 major steps.

Capture images

Capture N images of a checkerboard from random poses (position + orientation) around the “typical” working distance and store the images for calibration. It is recommended to use about 20 images for the calibration process. The MAS507 ROS Package features a ImageSaving node which typically is used to save the images from the Jetbot while the WebViz application is used to see what the camera is “looking” at while capturing the images. Save the images using the following command after that ROS core and the MAS507 ROS package are started:

  • rostopic pub saveImage std_msgs/Bool 1

  • When the image is saved to ~/catkin_ws/src/mas507/calibration/images/ execute CTRL + C to terminate the saving command after that the image is saved.

  • Repeat N times.

Calibration using OpenCV

The calibration is done by executing the calibration Python script inside the ~/catkin_ws/src/mas507/calibration folder. The script looks like this:

# Imports
import cv2
import pathlib
import numpy as np

# Set a folder for saving images
imageLocation = pathlib.Path("./images")

# Shape of chessboard
sh1 = 6
sh2 = 8

# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# Make object point arrays, used by camera calibration function
objp = np.zeros((sh1 * sh2, 3), np.float32)
objp[:, :2] = np.mgrid[0:sh1, 0:sh2].T.reshape(-1, 2)

# Initialize list
objpoints = []  # 3d point in real world space
imgpoints = []  # 2d points in image plane.

# Find all images in path
filenames = imageLocation.glob("*.png")
for filename in filenames:
    # Read image
    img = cv2.imread(str(filename))
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    # Find chess board corners
    ret, corners = cv2.findChessboardCorners(gray, patternSize=(sh1, sh2))

    if ret == True:
        objpoints.append(objp)
        corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        imgpoints.append(corners2)
    else:
        print("Image %s trashed" % (str(filename)))

# Calibrate camera
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
    objpoints, imgpoints, gray.shape[::-1], None, None
)

# Save calibration data
calibrationPath = imageLocation.joinpath("intrinsicCalibration.npz")
np.savez(str(calibrationPath), mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs)

This script will save the calibration data to a .npz file which will be loaded by the ImageProcessing node later on to rectify the raw image captured by the camera.

Rectification

The next step is to rectify the image, which is carried out in the JetbotCamera node. Before restarting the MAS507 package, please copy or move the intrinsicCalibration.npz file from ~/catkin_ws/src/mas507/calibration/ to ~/catkin_ws/src/mas507/data/ and launch the package using roslaunch mas507 start.launch.

The raw image and the calibrated image can now be inspected using the WebViz application.

Color Calibration

The image is also color calibrated, but this step is not supposed to be changed by the students since the lecturer have already ensured that all the Jetbot cameras has been calibrated for possible color distortions. The color calibration procedure is seen also in the JetbotCamera node, where the color calibration matrix is loaded from colorCalibration.npz also found in the ~/catkin_ws/src/mas507/data/ folder. For more information about the color calibration procedure, please check out this lecture on color calibration.

If another image resolution is required, the color calibration has to be executed once again. The steps are the following:

  • Capture at least 20 images of a white wall or a piece of paper in decent light conditions.

  • Save these images to a suitable location e.g. a folder named ./whiteimages.

  • Create a file with the below content in the parent folder of ./whiteimages and execute the Python script. Remember to change the parameters for width and height according to the correct resolution.

import cv2
import pathlib
import numpy as np

# Set location of all image
path = pathlib.Path(r'./whiteimages')

# Set size of images in pixels
width = 720
height = 540

# Full path of all images within folder
filenames = [x for x in path.glob('*.png') if x.is_file()]

# Number of samples
N = len(filenames)

# Init placeholders
images = np.zeros((height, width, 3, N), dtype=np.uint8)
gains = np.zeros((height, width, 3), float)
means = np.zeros(N, float)
v = np.zeros((N), float)

# Iterate thorugh all images and save to arrays
i = 0
for filename in filenames:
    img = cv2.imread(str(filename))
    means[i] = img.mean()
    images[:, :, :, i] = img
    i += 1

# Calculate the calibration matrix
for i in range(0, images.shape[0]):  # height-direction
    for j in range(0, images.shape[1]):  # width-direction
        for k in range(0, 3): # color
            v[:] = images[i, j, k, :]

            # Ordinary Least Squares
            temp1 = np.sum(v**2)
            temp2 = np.sum(v*means)
            gains[i, j, k] = temp1**-1 * temp2

# Save color calibration gains
np.savez(str(path.joinpath('colorCalibration.npz')), arr_0=gains)