[ VOXL ] VOA(Visual Obstacle Avoidance) 의 stereo camera calibration

 

Stereo depth mapping for VOA, on the other hand, is highly sensitive both to the lens parameters as well as the physical mounting of the cameras on the frame. Therefore we do not provide a default calibration file and stereo calibration MUST BE PERFORMED before use.

 

 

VOA 기능을 사용하기 위해서는 Stereo camera가 반드시 사전에 캘리브레이션 되어야 한다. 현재는 stereo vision camera는 사전에 factory에서 설정되어 있지 않으며 사용자가 해주어야 한다. 캘리브레이션은 기구적 위치뿐 아니라 카메라 파라메터 설정 모두 중요하다.

 

 

캘리브레이션 패턴

- asymmetric circles: github.com/opencv/opencv/blob/master/doc/acircles_pattern.png

opencv/opencv

- check board

 

We suggest printing as large as you can. Internally we use an acircles card roughly 52x73cm print of the above image with 60mm spacing between circles. We also use a 6x7 checkerboard with 50mm spacing. The remaining instructions will gives commands that you can copy/paste to use these particular two cards. Your card will likely be different so you can treat these as examples and modify the arguments from there.

 

크기는 52*73cm로 뽑고 패턴 사이 간격은 6cm 정도되어야 함

 

git에서 VOXL용 캘리브레이션 프로그램을 내려받는다.

 

swift@swift-HP-Pavilion-dv6-Notebook-PC:~/workspace$ mkdir git swift@swift-HP-Pavilion-dv6-Notebook-PC:~/workspace$ cd git swift@swift-HP-Pavilion-dv6-Notebook-PC:~/workspace/git$ git clone https://gitlab.com/voxl-public/voxl-camera-calibration.git Cloning into 'voxl-camera-calibration'... remote: Enumerating objects: 121, done. remote: Counting objects: 100% (121/121), done. remote: Compressing objects: 100% (66/66), done. remote: Total 121 (delta 62), reused 112 (delta 53), pack-reused 0 Receiving objects: 100% (121/121), 53.60 KiB | 169.00 KiB/s, done. Resolving deltas: 100% (62/62), done.

 

tracking camera에 사용하는 렌즈는 초광각 렌즈인데 원래 ROS에서는 지원하지 않는다. 하지만 이 패치를 하면서 초광각 렌즈를 ROS에서 사용할 수 있다.

swift@swift-HP-Pavilion-dv6-Notebook-PC:~/workspace/git$ cd voxl-camera-calibration/ swift@swift-HP-Pavilion-dv6-Notebook-PC:~/workspace/git/voxl-camera-calibration$ ll total 52 drwxr-xr-x 6 swift swift 4096 4월 4 18:34 ./ drwxr-xr-x 3 swift swift 4096 4월 4 18:34 ../ -rwxr-xr-x 1 swift swift 1483 4월 4 18:34 apply_fisheye_patch.sh* drwxr-xr-x 8 swift swift 4096 4월 4 18:34 .git/ -rw-r--r-- 1 swift swift 186 4월 4 18:34 .gitignore -rw-r--r-- 1 swift swift 1647 4월 4 18:34 LICENSE drwxr-xr-x 2 swift swift 4096 4월 4 18:34 patched_files/ -rwxr-xr-x 1 swift swift 3069 4월 4 18:34 push_stereo_cal_to_voxl.sh* -rwxr-xr-x 1 swift swift 2857 4월 4 18:34 push_tracking_cal_to_voxl.sh* drwxr-xr-x 2 swift swift 4096 4월 4 18:34 py_tools/ -rw-r--r-- 1 swift swift 273 4월 4 18:34 README.md drwxr-xr-x 2 swift swift 4096 4월 4 18:34 sample_cal_files/ -rwxr-xr-x 1 swift swift 1630 4월 4 18:34 undo_patch.sh* swift@swift-HP-Pavilion-dv6-Notebook-PC:~/workspace/git/voxl-camera-calibration$ ./apply_fisheye_patch.sh [sudo] password for swift: DONE

 

만약 초광각렌즈 패치를 적용하지 않고 복구하려고 하면 아래와 같은 명령어를 사용하면 된다.

./fisheye_patch/fisheye_unpatch.sh

 

 

VOXL에서 카메라 설정을 한다. VOXL Flight Deck의 경우 3번으로 설정되어야 한다.

yocto:~# voxl-configure-cameras

 

ROS에서 Stereo camera를 구동하기 위해 voxl_cam_ros 노드를 실행한다. voxl_cam_ros에 대한 상세한 내용은 다음을 참고한다. docs.modalai.com/voxl-cam-ros/

yocto:~# roslaunch voxl_cam_ros stereo.launch

 

만약 카메라 리소스를 camera server나 다른 프로그램에 의해 background에서 구동이 되고 있다면, 먼저 중단하고 실행해야 한다.아니면 아래와 같이 ROS에서 카메라를 slave 모드로 접근해서 읽을 수는 있다.

yocto:~# roslaunch voxl_cam_ros stereo.launch is_cam_master:=false

 

호스트에서는 카메라 영상을 확인하기 위해 ROS 이미지 뷰어를 실행한다.

swift@swift-HP-Pavilion-dv6-Notebook-PC:~/workspace/git/voxl-camera-calibration$ rqt_image_view

 

호스트에서 패턴을 이용하여 캘리브레이션을 수행하기 위한 노드를 실행한다.

아래의 경우는 asymmetric circles 패턴을 이용하여 캘리브레이션을 수행할 경우에 해당된느데 여기서 각 파라미터는 자신이 사용하는 패턴에 맞게 변경해주어야 한다. square 파라미터는 패턴사이의 평행 간격을 말한다.  대각거리가 아님을 주의해야 한다. 단위는 미터이다.

swift@swift-HP-Pavilion-dv6-Notebook-PC:~/workspace/git/voxl-camera-calibration$ rosrun camera_calibration cameracalibrator.py --camera_name=stereo --no-service-check --pattern 'acircles' --size 4x11 --square 0.060 right:=/stereo/right/image_raw left:=/stereo/left/image_raw right_camera:=/stereo/right left_camera:=/stereo/left

 

만약 체크패턴을 사용할 경우에는 아래와 같이 실행할 수 있다.

swift@swift-HP-Pavilion-dv6-Notebook-PC:~/workspace/git/voxl-camera-calibration$ rosrun camera_calibration cameracalibrator.py --camera_name=stereo --no-service-check --pattern 'chessboard' --size 6x7 --square 0.050 right:=/stereo/right/image_raw left:=/stereo/left/image_raw right_camera:=/stereo/right left_camera:=/stereo/left

 

 

카메라에 패턴을 이동하면서 X, Y, Skew값이 모두 녹색으로 변할 때까지 수행한다.

 

 

수행이 완료되면 Save result를 눌러서 결과를 파일로 저장한다.  

CUSTOM CALIBRATOR, with ModalAI Fisheye Patch CUSTOM CALIBRATOR, with ModalAI Fisheye Patch *** Added sample 1, p_x = 0.320, p_y = 0.252, p_size = 0.190, skew = 0.084 *** Added sample 2, p_x = 0.169, p_y = 0.175, p_size = 0.192, skew = 0.124 *** Added sample 3, p_x = 0.472, p_y = 0.301, p_size = 0.211, skew = 0.015 *** Added sample 4, p_x = 0.820, p_y = 0.472, p_size = 0.556, skew = 0.036 *** Added sample 5, p_x = 0.505, p_y = 0.552, p_size = 0.640, skew = 0.154 *** Added sample 6, p_x = 0.513, p_y = 0.510, p_size = 0.348, skew = 0.029 *** Added sample 7, p_x = 0.609, p_y = 0.878, p_size = 0.369, skew = 0.026 *** Added sample 8, p_x = 0.545, p_y = 0.616, p_size = 0.308, skew = 0.203 *** Added sample 9, p_x = 0.422, p_y = 0.578, p_size = 0.318, skew = 0.080 *** Added sample 10, p_x = 0.470, p_y = 0.695, p_size = 0.351, skew = 0.036 *** Added sample 11, p_x = 0.459, p_y = 0.222, p_size = 0.412, skew = 0.037 *** Added sample 12, p_x = 0.638, p_y = 0.576, p_size = 0.359, skew = 0.096 *** Added sample 13, p_x = 0.525, p_y = 0.400, p_size = 0.349, skew = 0.158 *** Added sample 14, p_x = 0.492, p_y = 0.276, p_size = 0.363, skew = 0.207 *** Added sample 15, p_x = 0.452, p_y = 0.380, p_size = 0.378, skew = 0.029 *** Added sample 16, p_x = 0.778, p_y = 0.349, p_size = 0.224, skew = 0.381 *** Added sample 17, p_x = 0.888, p_y = 0.292, p_size = 0.211, skew = 0.502 *** Added sample 18, p_x = 0.777, p_y = 0.321, p_size = 0.352, skew = 0.177 *** Added sample 19, p_x = 0.633, p_y = 0.236, p_size = 0.305, skew = 0.194 *** Added sample 20, p_x = 0.540, p_y = 0.233, p_size = 0.273, skew = 0.281 *** Added sample 21, p_x = 0.815, p_y = 0.854, p_size = 0.384, skew = 0.595 Stero Calibration, calculating left intrinsics 00:23:46.392537 Using instrinsic guess Stero Calibration, calculating right intrinsics 00:23:47.925835 Using instrinsic guess Stero Calibration, calculating extrinsics 00:23:49.918648 00:23:50.043990 Left: ('D = ', [-0.18831103077255, 0.09979392881997494, -0.001874558763111393, -0.003364828044223498, 0.0]) ('K = ', [496.4483869872976, 0.0, 318.13194222244135, 0.0, 496.00455632506436, 254.98228876117963, 0.0, 0.0, 1.0]) ('R = ', [0.7135620220051729, -0.006748466471476072, -0.7005595613166465, 0.012939598040397899, 0.9999099864063884, 0.0035476594155846837, 0.7004725601723752, -0.011596434152765206, 0.7135851141668375]) ('P = ', [7599.160763622445, 0.0, 1265.467307806015, 0.0, 0.0, 7599.160763622445, 232.83489227294922, 0.0, 0.0, 0.0, 1.0, 0.0]) Right: ('D = ', [-0.18074431297879, 0.09867497473634164, -0.0009884954467132986, 0.0036353564073705403, 0.0]) ('K = ', [498.7409883267622, 0.0, 291.5762187032763, 0.0, 498.30987684184066, 227.68080990553125, 0.0, 0.0, 1.0]) ('R = ', [0.7615991377315566, 0.058995280645396275, -0.645357505781191, -0.06461826976319257, 0.9977979698625183, 0.01495628801033251, 0.6448187595125938, 0.030311189349936044, 0.7637342464370913]) ('P = ', [7599.160763622445, 0.0, 1265.467307806015, -1711.1482708710546, 0.0, 7599.160763622445, 232.83489227294922, 0.0, 0.0, 0.0, 1.0, 0.0]) ('self.T ', [-0.17149381203576658, -0.01328431857227886, 0.14531899172307178]) ('self.R ', [0.9942899335632859, -0.07722967777136173, -0.07364173331778835, 0.07624005280745842, 0.9969585250967534, -0.016160247053250816, 0.07466580450669887, 0.010453521331962081, 0.997153820395394]) None # oST version 5.0 parameters [image] width 640 height 480 [stereo/left] camera matrix 496.448387 0.000000 318.131942 0.000000 496.004556 254.982289 0.000000 0.000000 1.000000 distortion -0.188311 0.099794 -0.001875 -0.003365 0.000000 rectification 0.713562 -0.006748 -0.700560 0.012940 0.999910 0.003548 0.700473 -0.011596 0.713585 projection 7599.160764 0.000000 1265.467308 0.000000 0.000000 7599.160764 232.834892 0.000000 0.000000 0.000000 1.000000 0.000000 # oST version 5.0 parameters [image] width 640 height 480 [stereo/right] camera matrix 498.740988 0.000000 291.576219 0.000000 498.309877 227.680810 0.000000 0.000000 1.000000 distortion -0.180744 0.098675 -0.000988 0.003635 0.000000 rectification 0.761599 0.058995 -0.645358 -0.064618 0.997798 0.014956 0.644819 0.030311 0.763734 projection 7599.160764 0.000000 1265.467308 -1711.148271 0.000000 7599.160764 232.834892 0.000000 0.000000 0.000000 1.000000 0.000000

 

 

저장된 파일은 호스트의 임시 폴더에 /tmp/calibrationdata.tar.gz 로 저장되며 이 파일을 voxl로 옮기기 위해서는 아래 명령어를 수행해주어야 한다.

wift@swift-HP-Pavilion-dv6-Notebook-PC:~/workspace/git/voxl-camera-calibration$ ./push_stereo_cal_to_voxl.sh

 

 

 

 

 

캘리브레이션이 정상적으로 완료되었으면, voxl에는 아래 파일들이 모두 존재해야 한다.

yocto:~# ls /home/root/.ros/camera_info/ yocto:~# tracking.yaml hires.yaml left.yaml right.yaml yocto:~# cat /home/root/.ros/camera_info/right.yaml yocto:~# cat /etc/snav/calibration.stereo.xml