We need to calibrate the real focal point, lense skew and lense distortion of any camera we use. Here is a quick tutorial of how to do that.
cd ~/wsr2/src
git clone git@gitlab.com:arcoslab/arcos-lab-cognitive-architecture/humanoid_utils/ros2-camera-utils.git
Remember to re-build you environment
Now we need to take a bunch of photos. The camera_utils package already has an example of a launchfile on how to do that. IN our case we used a HIK camera that publishes its images to ROS2 using the aravis ROS2 driver.
We have a little node (image_goblin) that reads the images, and tries to find a chessboard in it. It it is successful then stores the image in the current path.
Lets say that for now on the images will be stored in
IMAGE_DIR.
cd IMAGE_DIR
ros2 launch camera_utils goblin.launch.py
A window will appear, where you can see what images are being stored. In the particular case of our launch file we are storing 1000 images.
Now we can use the stored photos to do the actual calibration.
The following process is VERY computationally intensive. If you are using a laptop plug it first. Depending on the parameters you computer's CPU will be at full utilization for several minutes.
If you want to know how to use the script use --help
cd ~/wsr2/src/ros2-camera-utils/camera_utils/
python3 intrincsic_cal.py -n 20 -s 100 --path="IMAGE_DIR"
You will get 3 results:
An example result (100 batches of 80 images each):
The Best result:
Best Index: 24
Fitness: (lower is better) 0.014629057441649149
Calibration Matrix:
[[1.79855635e+03 0.00000000e+00 1.22391813e+03]
[0.00000000e+00 1.79838413e+03 1.03723658e+03]
[0.00000000e+00 0.00000000e+00 1.00000000e+00]]
Distortion:
[[-0.33122396 0.16313839 -0.00045727 -0.00059871 -0.05897175]]
Used Images:
['/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image01385_1734717438.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00262_1734714289.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00380_1734716758.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00180_1734716637.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00359_1734714341.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00761_1734717057.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00533_1734718570.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00965_1734717224.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00566_1734718584.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00988_1734717229.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00797_1734714646.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00532_1734714536.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00646_1734716969.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00480_1734714373.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00243_1734716661.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00826_1734714658.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00872_1734714684.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00583_1734718587.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00804_1734714650.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00116_1734716625.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00336_1734716740.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00833_1734718934.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00655_1734714601.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image01054_1734714822.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00500_1734718547.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00803_1734717087.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00366_1734714343.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00929_1734717205.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00835_1734718937.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00632_1734714593.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00857_1734718988.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00950_1734717213.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00224_1734714177.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00716_1734714621.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00903_1734717184.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00327_1734714335.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00432_1734716816.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image01141_1734717320.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00904_1734717184.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00624_1734718625.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image01277_1734717378.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00641_1734718636.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image01017_1734719213.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image01056_1734714822.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00956_1734714742.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00971_1734719132.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image01202_1734717361.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00372_1734718475.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00568_1734714550.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00859_1734714677.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00489_1734714395.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00079_1734716615.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00195_1734718379.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00894_1734717164.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00559_1734716908.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00798_1734717085.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00087_1734716617.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00628_1734718626.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image01079_1734714829.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00160_1734714013.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image01257_1734717374.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00695_1734717013.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00241_1734718398.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00822_1734714657.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00122_1734716626.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00779_1734714639.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00346_1734718444.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00748_1734714629.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00422_1734718501.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00351_1734716747.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image01064_1734717245.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00079_1734713881.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00815_1734714652.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00444_1734714364.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00295_1734718409.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00088_1734718316.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00250_1734718400.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image01097_1734717276.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image00118_1734718341.png', '/home/demo/wsr2/data/intrinsic_cal_hik_camera/full_res_friday/image01112_1734717289.png']
The calibration that was made, only works for a certain lens, a certain image resolution etc... if you change any of them, you have to recalibrate. But there are some tricks to make your like easier.
Some times it is better to define a Region of interest (ROI) of the image such that you can work with smaller images. This is something we do with the stream of some cameras to be able to send them over ROS2 topics at certain framerates. There are two basic operations we can do to the image: crop and resize.
When you crop you specify a width and height, when you move the image, you specify a new origin x,y (note that the origin is the top left corner in most cameras).
When you do this operation the new image can be defined as (Python OpenCV):
new = original[y:y+height, x:x+width, :]
The radial distortion is unchanged. The tangential distortion seems to be affected in some way, but it is yet unclear how.
The calibration matrix changes, you just have to add the correction to the terms of the matrix that indicate the center. The width and height have no effect on the calibration matrix.
original = [[fx, 0, cx],
[0, fy, cy],
[0, 0, 1]]
new = [[fx, 0, cx-x],
[0, fy, cy-y],
[0, 0, 1]]
Some times you want to keep the same aspect ratio, but have a smaller image. In this case you can re-scale or resize the image by a factor. When you do this the distortion parameters remain the same, and the calibration matrix is rescaled.
When you do the operation the new image can be defined as (Python OpenCV):
new = cv2.resize(original, (width*s, height*s))
The new calibration matrix becomes:
original = [[fx, 0, cx],
[0, fy, cy],
[0, 0, 1]]
new = [[fx*s, 0, cx*s],
[0, fy*s, cy*s],
[0, 0, 1]]