From b8ba91ebad3d224643f80cbd3f0abff836b0d0fb Mon Sep 17 00:00:00 2001 From: DavidTorresOcana Date: Wed, 31 Jul 2019 09:27:44 +0200 Subject: [PATCH 01/10] Add Fisheye calibration tool --- camera_calibration_fisheye/CHANGELOG.rst | 170 +++ camera_calibration_fisheye/CMakeLists.txt | 31 + camera_calibration_fisheye/README.md | 55 + camera_calibration_fisheye/button.jpg | Bin 0 -> 21110 bytes camera_calibration_fisheye/doc/conf.py | 201 +++ camera_calibration_fisheye/doc/index.rst | 18 + .../fiseye_cal_reference/mono/calibrate.py | 95 ++ .../mono/test_undistort.py | 42 + .../mono/test_undistorted_full.py | 54 + .../stereo/CMakeLists.txt | 12 + .../fiseye_cal_reference/stereo/README.md | 38 + .../fiseye_cal_reference/stereo/calibrate.cpp | 142 ++ .../fiseye_cal_reference/stereo/popt_pp.h | 39 + camera_calibration_fisheye/mainpage.dox | 59 + .../nodes/cameracalibrator.py | 154 +++ .../nodes/cameracheck.py | 57 + camera_calibration_fisheye/package.xml | 31 + camera_calibration_fisheye/rosdoc.yaml | 4 + .../scripts/tarfile_calibration.py | 235 ++++ camera_calibration_fisheye/setup.py | 9 + .../src/camera_calibration/__init__.py | 0 .../src/camera_calibration/calibrator.py | 1182 +++++++++++++++++ .../camera_calibration/camera_calibrator.py | 352 +++++ .../src/camera_calibration/camera_checker.py | 200 +++ camera_calibration_fisheye/test/directed.py | 274 ++++ .../test/multiple_boards.py | 87 ++ stereo_image_proc/cfg/Disparity.cfg | 39 - 27 files changed, 3541 insertions(+), 39 deletions(-) create mode 100644 camera_calibration_fisheye/CHANGELOG.rst create mode 100644 camera_calibration_fisheye/CMakeLists.txt create mode 100644 camera_calibration_fisheye/README.md create mode 100644 camera_calibration_fisheye/button.jpg create mode 100644 camera_calibration_fisheye/doc/conf.py create mode 100644 camera_calibration_fisheye/doc/index.rst create mode 100644 camera_calibration_fisheye/fiseye_cal_reference/mono/calibrate.py create mode 100644 camera_calibration_fisheye/fiseye_cal_reference/mono/test_undistort.py create mode 100644 camera_calibration_fisheye/fiseye_cal_reference/mono/test_undistorted_full.py create mode 100644 camera_calibration_fisheye/fiseye_cal_reference/stereo/CMakeLists.txt create mode 100644 camera_calibration_fisheye/fiseye_cal_reference/stereo/README.md create mode 100644 camera_calibration_fisheye/fiseye_cal_reference/stereo/calibrate.cpp create mode 100644 camera_calibration_fisheye/fiseye_cal_reference/stereo/popt_pp.h create mode 100644 camera_calibration_fisheye/mainpage.dox create mode 100644 camera_calibration_fisheye/nodes/cameracalibrator.py create mode 100644 camera_calibration_fisheye/nodes/cameracheck.py create mode 100644 camera_calibration_fisheye/package.xml create mode 100644 camera_calibration_fisheye/rosdoc.yaml create mode 100644 camera_calibration_fisheye/scripts/tarfile_calibration.py create mode 100644 camera_calibration_fisheye/setup.py create mode 100644 camera_calibration_fisheye/src/camera_calibration/__init__.py create mode 100644 camera_calibration_fisheye/src/camera_calibration/calibrator.py create mode 100644 camera_calibration_fisheye/src/camera_calibration/camera_calibrator.py create mode 100644 camera_calibration_fisheye/src/camera_calibration/camera_checker.py create mode 100644 camera_calibration_fisheye/test/directed.py create mode 100644 camera_calibration_fisheye/test/multiple_boards.py delete mode 100755 stereo_image_proc/cfg/Disparity.cfg diff --git a/camera_calibration_fisheye/CHANGELOG.rst b/camera_calibration_fisheye/CHANGELOG.rst new file mode 100644 index 000000000..aa32b37e8 --- /dev/null +++ b/camera_calibration_fisheye/CHANGELOG.rst @@ -0,0 +1,170 @@ +1.12.23 (2018-05-10) +-------------------- +* camera_checker: Ensure cols + rows are in correct order (`#319 `_) + Without this commit, specifying a smaller column than row size lead to + huge reported errors: + ``` + $ rosrun camera_calibration cameracheck.py --size 6x7 --square 0.0495 + Linearity RMS Error: 13.545 Pixels Reprojection RMS Error: 22.766 Pixels + $ rosrun camera_calibration cameracheck.py --size 7x6 --square 0.0495 + Linearity RMS Error: 0.092 Pixels Reprojection RMS Error: 0.083 Pixels + ``` + This commit switches columns and rows around if necessary. +* Contributors: Martin Günther + +1.12.22 (2017-12-08) +-------------------- +* Changed flags CV_LOAD_IMAGE_COLOR by IMREAD_COLOR to adapt to Opencv3. (`#252 `_) +* Fixed stereo calibration problem with chessboard with the same number of rows and cols by rotating the corners to same direction. +* Contributors: jbosch + +1.12.21 (2017-11-05) +-------------------- +* re-add the calibration nodes but now using the Python modules. + Fixes `#298 `_ +* Move nodes to Python module. +* Contributors: Vincent Rabaud + +1.12.20 (2017-04-30) +-------------------- +* properly save bytes buffer as such + This is useful for Python 3 and fixes `#256 `_. +* Get tests slightly looser. + OpenCV 3.2 gives slightly different results apparently. +* Use floor division where necessary. (`#247 `_) +* Fix and Improve Camera Calibration Checker Node (`#254 `_) + * Fix according to calibrator.py API + * Add approximate to cameracheck +* Force first corner off chessboard to be uppler left. + Fixes `#140 `_ +* fix doc jobs + This is a proper fix for `#233 `_ +* During stereo calibration check that the number of corners detected in the left and right images are the same. This fixes `ros-perception/image_pipeline#225 `_ +* Contributors: Leonard Gerard, Martin Peris, Vincent Rabaud, hgaiser + +1.12.19 (2016-07-24) +-------------------- +* Fix array check in camerachecky.py + This closes `#205 `_ +* Contributors: Vincent Rabaud + +1.12.18 (2016-07-12) +-------------------- + +1.12.17 (2016-07-11) +-------------------- +* fix typo np -> numpy +* fix failing tests +* Contributors: Shingo Kitagawa, Vincent Rabaud + +1.12.16 (2016-03-19) +-------------------- +* clean OpenCV dependency in package.xml +* Contributors: Vincent Rabaud + +1.12.15 (2016-01-17) +-------------------- +* better 16 handling in mkgray + This re-uses `#150 `_ and therefore closes `#150 `_ +* fix OpenCV2 compatibility +* fix tests with OpenCV3 +* [Calibrator]: add yaml file with calibration data in output +* Contributors: Vincent Rabaud, sambrose + +1.12.14 (2015-07-22) +-------------------- +* remove camera_hammer and install Python nodes properly + camera_hammer was just a test for camera info, nothing to do with + calibration. Plus the test was basic. +* Correct three errors that prevented the node to work properly. +* Contributors: Filippo Basso, Vincent Rabaud + +1.12.13 (2015-04-06) +-------------------- +* replace Queue by deque of fixed size for simplicity + That is a potential fix for `#112 `_ +* Contributors: Vincent Rabaud + +1.12.12 (2014-12-31) +-------------------- +* try to improve `#112 `_ +* Contributors: Vincent Rabaud + +1.12.11 (2014-10-26) +-------------------- + +1.12.10 (2014-09-28) +-------------------- +* Update calibrator.py + bugfix: stereo calibrator crashed after the signature of the method for the computation of the epipolar error changed but the function call was not updated +* Contributors: Volker Grabe + +1.12.9 (2014-09-21) +------------------- +* fix bad Python +* only analyze the latest image + fixes `#97 `_ +* flips width and height during resize to give correct aspect ratio +* Contributors: Russell Toris, Vincent Rabaud + +1.12.8 (2014-08-19) +------------------- +* install scripts in the local bin (they are now rosrun-able again) + fixes `#93 `_ +* fix default Constructor for OpenCV flags + this does not change anything in practice as the flag is set by the node. + It just fixes the test. +* Contributors: Vincent Rabaud + +1.12.6 (2014-07-27) +------------------- +* make sure the GUI is started in its processing thread and fix a typo + This fully fixes `#85 `_ +* fix bad call to save an image +* have display be in its own thread + that could be a fix for `#85 `_ +* fix bad usage of Numpy + fixes `#89 `_ +* fix asymmetric circle calibration + fixes `#35 `_ +* add more tests +* improve unittests to include all patterns +* install Python scripts properly + and fixes `#86 `_ +* fix typo that leads to segfault + fixes `#84 `_ +* also print self.report() on calibrate ... allows to use the params without having to commit them (e.g. for extrensic calibration between to cameras not used as stereo pair) +* fixes `#76 `_ + Move Python approximate time synchronizer to ros_comm +* remove all trace of cv in Python (use cv2) +* remove deprecated file (as mentioned in its help) +* fixes `#25 `_ + This is just removing deprecated options that were around since diamondback +* fixes `#74 `_ + calibrator.py is now using the cv2 only API when using cv_bridge. + The API got changed too but it seems to only be used internally. +* Contributors: Vincent Rabaud, ahb + +1.12.5 (2014-05-11) +------------------- +* Fix `#68 `_: StringIO issues in calibrator.py +* fix architecture independent +* Contributors: Miquel Massot, Vincent Rabaud + +1.12.4 (2014-04-28) +------------------- + +1.12.3 (2014-04-12) +------------------- +* camera_calibration: Fix Python import order +* Contributors: Scott K Logan + +1.12.2 (2014-04-08) +------------------- +* Fixes a typo on stereo camera info service calls + Script works after correcting the call names. +* Contributors: JoonasMelin + +1.11.4 (2013-11-23 13:10:55 +0100) +---------------------------------- +- add visualization during calibration and several calibration flags (#48) diff --git a/camera_calibration_fisheye/CMakeLists.txt b/camera_calibration_fisheye/CMakeLists.txt new file mode 100644 index 000000000..552452725 --- /dev/null +++ b/camera_calibration_fisheye/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 2.8) +project(camera_calibration_fisheye) + +find_package(catkin REQUIRED) +catkin_package() + +catkin_python_setup() + +# if(CATKIN_ENABLE_TESTING) + # # Unit test of calibrator.py + # catkin_add_nosetests(test/directed.py) + + # # Tests simple calibration dataset + # catkin_download_test_data(camera_calibration.tar.gz http://download.ros.org/data/camera_calibration/camera_calibration.tar.gz + # DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests + # MD5 6da43ea314640a4c15dd7a90cbc3aee0 + # ) + + # # Tests multiple checkerboards + # catkin_download_test_data(multi_board_calibration.tar.gz http://download.ros.org/data/camera_calibration/multi_board_calibration.tar.gz + # DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests + # MD5 ddc0f69582d140e33f9d3bfb681956bb + # ) + # catkin_add_nosetests(test/multiple_boards.py) +# endif() + +catkin_install_python(PROGRAMS nodes/cameracalibrator.py + nodes/cameracheck.py + scripts/tarfile_calibration.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/camera_calibration_fisheye/README.md b/camera_calibration_fisheye/README.md new file mode 100644 index 000000000..f761bdfc9 --- /dev/null +++ b/camera_calibration_fisheye/README.md @@ -0,0 +1,55 @@ +# camera_calibration_fisheye + +Same as [camera_calibration](http://wiki.ros.org/camera_calibration) but implementing OpenCV3 fisheye calibration functionality for monoCameras + + +# Usage +Camera Calibrator +To run the cameracalibrator.py node for a monocular camera using an 8x6 chessboard with 108mm squares: + + +``` +rosrun camera_calibration_fisheye cameracalibrator.py --size 8x6 --square 0.108 image:=/my_camera/image camera:=/my_camera +``` +When you click on the "Save" button after a succesfull calibration, the data (calibration data and images used for calibration) will be written to /tmp/calibrationdata.tar.gz. + +To run the cameracalibrator.py node for a stereo camera: + +``` +rosrun camera_calibration_fisheye cameracalibrator.py --size 8x6 --square 0.108 right:=/my_stereo/right/image_raw left:=/my_stereo/left/image_raw left_camera:=/my_stereo/left right_camera:=/my_stereo/right +``` + +cameracalibrator.py supports the following options: + +``` + Chessboard Options: + You must specify one or more chessboards as pairs of --size and + --square options. + + -p PATTERN, --pattern=PATTERN + calibration pattern to detect - 'chessboard', + 'circles', 'acircles' + -s SIZE, --size=SIZE + chessboard size as NxM, counting interior corners + (e.g. a standard chessboard is 7x7) + -q SQUARE, --square=SQUARE + chessboard square size in meters + + ROS Communication Options: + --approximate=APPROXIMATE + allow specified slop (in seconds) when pairing images + from unsynchronized stereo cameras + --no-service-check disable check for set_camera_info services at startup + + Calibration Optimizer Options: + --fix-principal-point + fix the principal point at the image center + --fix-aspect-ratio enforce focal lengths (fx, fy) are equal + --zero-tangent-dist + set tangential distortion coefficients (p1, p2) to + zero + -k NUM_COEFFS, --k-coefficients=NUM_COEFFS + number of radial distortion coefficients to use (up to + 6, default 2) + +``` diff --git a/camera_calibration_fisheye/button.jpg b/camera_calibration_fisheye/button.jpg new file mode 100644 index 0000000000000000000000000000000000000000..494a6f6fc5247fdb882536643408f1c5abf7c8a1 GIT binary patch literal 21110 zcmeIaby!qg*FS!S?gph>I%Z(#kZ$P)0bzil8M;JJQCg5t1eETO1{IVJ1(hyAK@kw7 zQ{j6CSer*64Ltgp7gSgn^5_Tt^c2+j=toPU%Y*rob^>XV+#h)rgDpb(ldcA7vfLlC zXqAxsA2dh;7(Zz+I{@=+oF1u85B6O|GX zM)%Y|cz`YLPYL{=?;pA>e*XfA|NjB56)FO^u)&7#CW#Yo4@#7M`$z``NO#lptV&cMJe&ciPZ6%`d_=8}|^ zfXN7oh{DiHAo%$BM1(}=iHXm{SQ%Jh|Lf=UIY5pF&R0832pfPw4#6acoOS}tU^%hT zD;)$~ld}hcfr*8UgNuhxKnMcr$p8!pCME_JCN?$}*a(m?Fde`m$EIL~D&SD++u^c# zQ^6vVa`4y{AHSeBc(cJFas?HMPe60-JS`pP1ukwLUQxK1xP+vXlCp}bn!1Ljp^>qP zshPQjy#vD0$rr0}Bfi3m0842u1)HFv+p7S)n)-3i`Nq-jr;x2s|pqq@2eu@YzKSHmI+l-Vo4m zh`#6CL|5&sX8&2mBL7b{`(3fW>NN!rVM4&p!z2gffqh=?+-n5f#g&4O>TdVRl+lrV zamKLVd(gb;7SeBSDYLc`NNrqSarKhHE3)vqx-YoaDk#26*Ec=_F;b^MO;efwJArG} zS_45Wo>mU}wo+2O+=8qvx3#ch*_du#a!FEY{P$fd-ThzG6qEeGWPWRtdP;j%8r3=dQ_i7Llojgzw%&8T7$- z2Xv*=lLj^5Sr$(-1cumQFURQjy~b>k3gmCE&+#}$yPmVX{^{GwU8~EQ0kWOt;qTy% z?*=4#%T^sQb}CJIWGA=kz5U9h2X*f2TfOU?&TC55J?^f{*~FU1YP%_DzspV}PiN9# zyk=Y&;jNgNS4&{lzhnLWLi`gX9mCw`+T^_Ztez|K6@F8yk4Y*#%vJmDMzhNrodP$5 z!k)dH3hT|F=^&ZCy#s%mo~_kUK0##r!R*!wUuo8*Fs7w3M00YNpvE%iLG~>bC08HD zkL3a64EZtV=`!kM^>SmlflAA+036p?SvcvOC-mfcfgikmWq5tluOocu{59rl7cwaQ zO(kbht7{g~(WL9POcs!}u~=!h`6PX0n&C?Ni8cY9DQER1zC0=F-4tT>b^@+<+%F#Q zc6mwnZ8Kw75>nd z-t?HMp53w1F4eYl?luw-~ikJ5Vhye+@{`6#s^k2>y+d$wnjxK}uxd)qBP=_j$p zY|-LV4U?#OY`mQ^2)e%`bP(a)oA3 zkX>YkjGd6hRzqS%%EWhB?0)GXB~YuxLIfEkmUstieiOBKFZfOIX!$8nu+Nk_zgkn~ zQ`CG47)=jgacn9pEjRC@e2dT55kp0nm2R7e1Q}h5eaKBeDI;+yzrCD*Voc(+u~Wb- z@M;LbcI;S)fJ*+&NgCZO^Gi~%_lF1bUfrLYyqEFe)8d;QIjE_b+Lw>#UeQdAUz*B# za@dj{X`85vqrrCXexA~{Js0y!!=gFFBcxdRQ|tQI2*s-it8rrLFMF?A@Uz8sO}dl> zW`?ys_3=G91%e)2NSDm$?jHZRU!M2TA^5=%Ny!H!KNqV9|0jy)JuUkdB}K&htpfUx z%)=W&7Z<|ceR%q+>H=lZJDxQ3pjby%EE(@ zVdGMgCa${FNCUwn&ik;!UM+`SCI4?-wt+E7&P|;K4chprt9rZCstbC}$tp=pNm-I6 z{2Dd|3T|80D;mwQ&2Gt;G)gXSKNdA@%qXIMS~a{QORB(GAM3w^^NjlH=PLP)mzKNw z2}xD*7CVsV)H6emY0#Zu9(-~yqcRm<>t~lL7bitGlF!ag1^H03o*`b-L@)U03Qkz~ z^D(n~c~l;F^W!-OLmi~3@H+k|L8{P>+C-(xo?a&*)L!;YmuM}k4)@Df-Y$)L1W6{C zJ(hoZtLrc=^tGCWy}GO`PnFEwXjJP`0nQ=b@#^6?d#vR{5gATRkGCd5GOfFquf7^_ zw#KV!$H>#=PetGV_QZM0NjAm%@OAEF1p~*YH7aK-Ykp@fy2;lSuj+`yQNnBc=MD+_ zhj$)w$Icokq<*w##68GZ4;9JNt+?FdBZCmImy9P3_pvrw-dfq0djN?r^Lr%}yCuzx zi~Ne6K6QclS<1TH=%|sGVs&S69_Uxr%aWnzHy9w55#vNKzSvFih_(8QjyFS_Z>e79 zYsjkrLibAp15Agl)0|}vdP057gph7#hhsIWA)7Wjx(Mv{56ZYJd>0nlgWZ+{FqW3J z8MBv$9#1U0C~Pqf6~1-7JoxUD0sHXIR`baP>MxrI(pqtg7u!;i)6RSTZ7c8fcc>yb z8e|qOZZLLr_W6`NMoAI`6YLD|oiEQZThId-OZ$uUuR*mgVpHk=kAeAg^7K9FXJ=nu zFDW4*4Ykp+Gw?A*i6R5FjHT=w)Z`ituH*f^b55$Z~ys(#FMtbdcpT zh3g3Gcqt*Ak?O%HgkkU{Bl}=idr1c_c{u`^K&e1?FL#8m9ZR6Qn}?57pe)y!aw(8T zvxT@o5X!+(%0OB5Cj|T@%k{G?0RaJm0V0B)C?_GPq@<*fFiZ#r695qcK0zM7c7XyO zKJ1_d0+ur!$_O8O6w=EV>FM!PmoL)sFJ*tL7&_&wW}r9=iwydwl!m*V6T%ef;Oi_0 zlYj}!2>mPf??`QghZ7nq35ER${mK4~Rq}NALLq#7z*5OUe**vH{RZked-{5Uje&7xr(sj-4w)?rgvOv+ie%zmh#2z;-!H|GAU?1v^Xljk^T4 zg+D?W>4fkB^Ms4>!{FjzkN|@uKTHh94-*#!1IUvQ<%f$1g8>TCAYVd+A0{crFCq-( z7lBHEL6RR11A_<*iNOZfQSxP8`ov$a#$kWqJP653@vYbsvA7@W579~Rw z7E`2$gJ*!x5BWdAe^Y??PjWw#{(<}@tOKqE4tBnF|B3kDu;AKw){FmHVTK~Q^ z(J6l{nCPEU=x<;q2-MlqhW>IRBlHh8y8LHMRW~~yXD?*nZ#=jJ{KWrj(r+0hPd87L zA!v6Hast1U|H1o>LpO>M(j5UVbf9grbNgHG{``FMt9Q=|^ACZ4r~;=m(ia(waQKgP z{Uz~RnM$CSr;i`%tZZ^nA*k4|8viZfH(d3SpZgUAN)84S`c>OstlyYll6ojlCzPE# z+M@hy^B-CL;{O&fboTQFjftTb0^wj{=jMly6O|AVfk_C7!Qmp{3_p|pC+N4lf)@hi z3)bO>EL=iT0w(%5*}nsR%l#^-BGS(1hph1LTKzNax0tG@y`9@n6Ds+a#GmZHW6`Du z@uvw96%msFjf)Ue6bk<}z|czn0sJ?5{M$Nw%oPWyy z3*fg3B|jg~1u#H(I3Q5q(EBx9{)PP;{;M6qbq(>4k#NTR4K;N}`XUtVz{Ub6)2~7E zH|B4sp*_L_p=9TU?scOeFU0R+{3GGF$e9aarVLI&kF&+#hx>HaX#bk@cNz0PWX}Ak zzhwSM`YrP}g9i4qm?*!fu%wvqSqvIcaVYFZ3_Iieh($m?9Qp$z%8!;3KZ`+w3mQ|9 z1I8dnR01x17K6qXq(M^)#-MQ)7XeK-ItKAzEQyXKKqMHW^AZ6KIXXt?A_5wCbPURZ z@tG`W-bJC{j5~|L(t`94j`$fZfsQ4?a)R+0MiLIv;xIH{9CQW17<3p!p^~6Zbc~jj zlt9Z$N}w^45@-x)_RnJIS&Y^ud8SQL5}l*unNCS`NnpZIF@Dgg;0GNMeldP=ehGd_ zez0kvP<|+kA1cBR73Bw=3=jkr=LbC!7>EV^6wplpJs3FX%z(ZK=(d2~h_r}=lB%kb zu%fsER8mXIvPb-I3x3XQv~dDU@xL*Z4)(tdrI#Pd z?QE0cU@wGlLx8iu*9WwfQ1tEww0{ovQjVS|cROD>BzlJ-Jk`elg`@hTA3%yN1 zdmX>C|C`tIJNXY|{hKFsW@L|Kk1cY7Kf|Iy(Qp#=rbLclV#|eTJ3N@bK|PuXY;Ba(;eD z2Pp|K!mHDAe+0)(+-K`qR zXTu5ga}3FH{ZOs+->c?g5jqFNBk%$109sy|%H$QhINFRVSl{>=D2RvCM zLi2qQ_Rb(J4AMj>BLgLnmH_}fBBwv-D}T_wh(NHM;7P2fR}kosI{UJ4+w-uXCk=}_ zBESvd>norKeoC=JIk2F=1K4>4fv2Wtbw*186zH~P0UKEiTv){fV1i)t|9<}4#NVj@ zo{_^Ulw$u+JpNEKNO1( zdPl(`@UMgT|19`NvHmCrzX8G#^y0u3h{XgPWuTn{yW7J7+*N>|r;r~1RS*9k#r`M* zn*1@YL4q>>1fa4J1jvS{0L-M})fS!4#%u7GUJxGJ;XYt=6 zj70D!h7Zz-122zyNVT8n_6k0Gfa< zU<8;0mjQdg8E^+sKmZU5L;z7hEN}-%1u}q#Kt50cR01_XBk&Y>0dxZcz-wR>mDD7*rUH7#A>vFeES(Ftjj?Fsw10FuXB> zF>YWaVBEvV#VE&Uz<7Z%fH8_OkFkoeiwR&7V$xu;VhUhNU@BqiVp?E2VxlmwV#Z*m zVdi31VzyxRU=Cx>V}8Lr#KOU%#A3k`z>>yN$1=vU$MVLyiWP^IfmMvvfYpgLjP)LC z1M38v7@Gl`4_gXb1KSMS2|EBg3OfzE5W60`3wsp%BlaE+4h{_t7mhfNI*vJx3r+}5 z98MNa70z>X?Uf0t$1(omhg`8$?!SxrSNs}9q@zj6Y=x$TkwbQKjI$~kP~nb$PyS4xDZ4T z+#{$Y=pvXQ*d`<-WFwR$)F(s|UL(9mSWVbV_@3~Ph=PchNQuab$d4$2sEFt}(In9} zF){H4;)}!<#JqnAiYc)Oqxbo zOFBgQg$$qU0+|w-9obc~2V~7;<7B(!l;luyU2=EwIPy~RKJw2LxD*#CR45!MZcyY= zbWki%Vp6hGDpA@~UZ>2b?4tZgg+s+fr9tIF6-!k?HAuBVO+hV6Z9*ML{eb!z^*jwG z%>^0_8aJ9mni`rhnxk`!=M>H%&PAWAJoon8{(1WI7tbTk$DFS^KYIR%mW5V@)|EDi zwvl$04vUVLPM{kLY_seH>=Nvb>?!Q+>>C`+96B7K9Az9+ocNp)oX(u-oIRZT7q~8%UbuCk z@xo`W^IRHS!CYlrGu*`7^4#9sx!fZ>I6M+Ot~^;huX!#lnkA3Oouf3grsxio%L0#d^hkB`KvarRU0+ z%4*6n%7ZEtD#j`qD(_U;RGn1IR5#S%Y9VUR)v?vJ)RWZ5H5fJQHA*x#G{rT;G&{A3 zv<$T}v_5F_YNNDUbTD)@b?)lST;jasaj8)k(AChrt2?X5rRS~pL?1_APyd1bCj+QK zs6me*rJ;>sso|cHqEVvJv@y4_pK-ehsfm?IiOIgHvT3sEyqS<$nAw0iojKCH$pY8H z#G=q**HXnY)$*ei+$zdy%$mzO(7Nw3-DS7S&umC->}+anv29IlOKrc|>DuMm?OxHi zl6hsrUdcY)e%0ZkLyE&PLKcyXSaOtgOm_V2B{~;)%I}q@EhSDuc=)tjv$C|jd&F)9GM)sbKUfM%MHdG*KRD_RJ&OgMG}RI8owod z>)~yz+sNC4(IV0JqfcTSV)|o+W7A^4#o5R8$3x@q#h)fPCJZKuC1&5jx#MwXEJ;48 zIGH>-IC*52`X5 zGNUv1vmCPCW?#&%e8~7P_TkYZu`@fHVX(Y&1GI&NrDiy=m5LZf}upsedB)r06N@(+AI}pCz^uwBBev zZ3}MOeeU)A%L}I$3+*=T(;a3VBc1x4gI!u(z1^za9X$#?ZN0L+Py3|$n)@aC8wbP& z8eYO**1v+ksvi^^Y#0(BYI-gCy5)_`o7T7TZ(j^64|k7fj0}uk8htZnJT@_IH9kM# zF!6cPZE|zUf9iPp+6?wg%q-b#+B=4KxpO>oRr6x=&)zG)fBC`i!}Nmv!s?>W;>`)<#D?|47vfcBvHP~x!X$n z=KuOWyV>-A75*jn<7Sf+;D8AMFZKMo*~A3;G+5|6Kxj|p$AiAqgoRCj3Bdqw2uYEF z=|65Z;b5Y#)1mMGfES+tTyi`LR!S&76}1AJz8yOU%sV29hEwsJh?2n-RL6i@~5))XEam3PErYIazXh+@tL z4abW&oA{vq>!*La;H#G7C8?FSHz55fIJpomfI`<+oNH%T5BwIxYRP^>YKO?o z(nII&rC_;dcrH)qb`RHx;$mL1Xs??(yB}$;-X7d1)%i%KTE&)gf8R}2X2~COrud7@ z_pYnnh4=0=dhbdmU)_%MU1T4ZJh+$Ss`0?8p^lly(8T&)3O;6{U(M88H3H1*rR~Wn zZljDkU*fL!RFkOLANsG0Cy*{3dLFw{C)Bj`R&cHkX1jjsQX}xDZo2Bd&6pV(VqMw# zY;7JzReQNQfgmNR$0I5^Wr2)>l z=}x$e|Gux-zF$deTVs%)pt)HpXS1MjKSAo{qhuqtX%m?=x8+a%k$EI<86*|hn`&^g z%w;_3eOppxyFs@rI1 zZ0-|eP#kB<&fp!J2-b=q@ZQg~^tM$|8g_iz>1Z-vLZ!=hL*ee?f&Thc8C!?si$IpV z_DWuWF6?Dv&Hc4EE9Ye5YoE~K*n}}bX+313R~5Xy?S_rgPFzL$tlS%M$BGov21p8a z-wumYHz!$CSlW0z9)7w@(H()R;CPUR5?SspC49no3Z#(SC68m=j8Q!7juz!#EwUDSEm+EgdR zR4cxrUE9~=QaE|$UAf1a3L5Fuw-~GlL|bxJLhl5(W4u+o84ADJ$56WV(xq>5p`pFH z=kBoV#a#~BlaCQ*3bagwM~h2MQipjjPl4|WRjGzXmWRe2BMrf1mn9CaM_;kL`Oa=a zR|gs8`95t6uh*hah|%?}IqMaDbH1xNM|TD;TgArD50P1MYB{ETQiY$3)R4v;oB4=;U?-qqmW*L*GV9XhzdbHD?M7u)OEml#hHnloV7{vdhT@|I}epkPgK zZk_dtn`n>pPK+gaqah$|+m-Ncay-j%avd`3Ooz&yL zeyY-70G=64e#>dL z=5%bvdFPReWysrsbst&?OJi+#)M($Wo$fI1Br##;IbPBvw)s#&A&K(=b!XMMVa+DI z_BJ~Y;mu-W91E#y6uv$FdZgOLB(vVkjccoTuj0c*7sp4$1rDcG=Qf`8eXiPh9bIX~ z6+~`m!^3_#iDN^#M&_I*HR{pW^vC;bq!gnC{9ENxHQA|0&z%A<2)PBwwbIZ(8`WQI zam~v)sH0t^n^#_q(`hcVD)twSq*>eJgpQZH=%sTiUu|!`@A;zl6(n)pDrLxLR^tna zbRw{p-H92uF9vwXd`?o`>>SC~Wy5gkt}=2wU4y;T&VCY`SdN5R@+o5r=n zr1*@9Jo-5+DzehJU$C+naw@Me%7|#D3O#+&mAOlqC7JTkK$h-vgqOFr0lon@luo&X zC*IPOnw9w6vkOy)(-?0ZF8S5**fr3_)5&L5iI)zHS{jW?65x|v%s(>PT;fein4CC= zlA4Xu=mrkx2jzUd!=(ZX^KLh#RV)<7e~js|i!>*$eBt4Q&BOh{wsc#n6z=Z&sJQaJ zax!&1YZU)CEsuAZmIfKDTnV~Jjzory(8H5QB(mRMblvFw0t>mZ6-soBc?x8@#=X+J zbEgTW8KU~+q%vTc85iqYz{ri_sWggy-ToXaNPk-Pdbm8-^KaisWP9db*)ik?eBLvr zCpzTtY?WEupTjjMwc-`G`AFPr%qW3-{P=s=cKmM2-aD1s>V^UwC$|A+7VrJVldGZO zwsvm>!#lrqllTwyV)n%CZI&7+SjAx)NqnuA-P`Z!`MTws+}!d0$VJ^9Umy1-AR+H1 z!E_27f4@4F@4%}mKASze6|&UkI5_deE7aa(F0P~q#rVt!mx$iKdp6O#;gUEmeVlt? z+=DpYE#ebtgkZ*pQUlI$bJN!1tq+>c$?5do3{0aWdJ8V=?qewpS&9!V-a2V;7A8w& zAUUD}@scI&uaxJLDe+;>5JlqAVwGVC!>cDf8$+-Mr#nNXeadU!3HXtZS$c?y)XN%k zDu<(zIl?9xdA))u5Q;MU66i4x2hJ>f;D|?gyZFlI82So!aw-NDk89yNvf2;(nhh@) z%vts4I2p+~1cQTNGQ18J7VcY~2L z-)I!|O--E6*kta#w50ZSbi+IC)b0w)gw{!dcHwQD!QC7F%!c3Ie)148_(1AvQ1)?B zRbS8IwI}x-(SkuJ-J*m0$TZQ3)=1bE4!F##9u9UaaLGa2;+S!}{c(-f#yo76r>`)| z>8q>v_~#aK#6^>jX@`V$Jd2t+jvl;`J^sd+!q9lMwaVvC@|Vxgx?~_dLD9i31EtgU zS8Q{DCc`_sC~p2sq%&;M$?)@>X8kg&3pV3(VxkI3WQdmD*WL5cDA9L(<*vcqjK6Te98F=RRO zHnGz;M$BNYhkPN;YMfci3Oxmii=o)+QI26rx`(QBK|_s*uXpEc{Y0U02BNCwa>)w# zObZi`p~Rzbq2|5)rP$AQ{HlkJ>z8JqwFw+w4OxeWcr+}*G}5kUiBP6p@xD79>O?!| zer?NCSDKEJ1C{igJrM}pKY30PBtwd8O1#`nudtTCPRbqD}fjKaw_)SUPCx#l8( zD{I7jobq+=75V0Vjx5LZx9`pDQd~<=hW8rf41Bnv0zb^WJ@TL0iP=<*yVz{z(p&Uo zdeI=5VMu*&=hMXjY&WBNUaaaIn)kzF-jt3~PkUPL3dnz14R7Zv!>~2#t z^7C!v<<*KO>N8Za(bBb6Go;0lim_-)&dm;-{?^A&!4XlTg#i!l)3Y$h`SJ+wM9oC1 zqG}Ai1u+;m7BB8Q`rht6K0|o@+S(Y?%6WZW_b1YXFN=g~^c5J}o|za9i*3l<((7<9 zOIk3e)8;pzT;gCpe;!vp?cm$41gl;x8I{9h*~o}yd3w*yheJl|6VGNV-u5nynoK#+ z;<(p78AnGY zmEydmbw&C4MH-IxdMIxsoKNcSI!};u4q2jB7ZP!OEcdE$<7A~cah0|t?3n6VVO%PA zMEk?X5w!WK+&A*6Nfw?1*>K}xS34Yw7^x;QTigu%yo=IA=@(`8`d&yJoYzVhYC391 zG;a!FddEjbiJ6U6?suFkv*vZ!$<4?(I?sxI?^>I`78%YlvMbMVjwS7tppcI1of)~C zG`ye73N0pNQb@ZS-akhy6Sy-(e&`5`UgXhKmntF95xKqmJ{d{xmq6u&f;KN-e04bQ z|FvwDx^lad`p%RDYRI>Bw$RjBpuvUUNtSQSJ(!GdioULNE(T$nt|DeSFSPL3pM5FB zx8eM&O`;+ae-|qwDu)ph-zXOgps6^jo8PyOct5|%!k{Cp?<;0fDLiGguDrxhM_=3W zaW*X*KMsscsxstj+{sbHhZBm0LCWH&^q2#!=s*L_ddDILB}CJdJXtZszYxB&f!~gE zxk|ZB&ER@myz1HuSFgvtaGFU~_H=uyL-q?MvaWaR$EoTku{;%J9^T7;U{4|%Vj^c@ zq`_2o&c+SOE8TVZ!K)Jg;TLc=_E)+xuG}jz8dp*&?DiV;XN|4B`ZW|gh{QVt%LOyp zJ3~d(+?#EhJia3fo_4HGd7O)t?Vi=}i+vm81FsD3545d_eOnS~V{reIjb@<3kaBSs zp}1!w@=H`1wX{>yE#9`0qcsu}P#1Q07Y*MM2-m+!`#J2f1Gntd zK!NBhfzJLrS1HPMc!M85=lMjT!L&4@-Xir*hL7nHtyfW4f{-r8=EAJAo7#1W>dWhO z@?8RHo(KNx9_f5e+Ajq~#ontO^tUj@1WgzZ_i)OwrbI(2cM5%;mAwmPDj4`O=Mo@J znnPMkG>ed6GE6jLGrFT3*k9977EKJ}2zDi^UA<-f#qO}HctWKH-zcrUP9+41XlUI;O+Yzre(%t*&bDzl*ZOd zSQzF~mcy!73zuaf)xLd8=hqRp6?1C(H?<$qMx+@x2E^+2a=Rn#9O|m-89%viE{l<~ z*ZNo-Q{+CKy@^`v^lm1SNH@Bb!I$RD6=66Q<(%xW%O1q$Yi=*m(LPvO!%BpQ;Bbfw z7;$LgJq7=qWQqKQmRSKKsG7C%vZw*44qqQ-55HTb z$wlqyuI-?-2eh@#PXp3-U$t8CY}ydW<)OqHC9g}xL&7_y*;v?bBok06Y!1mq&QIMn z?(qK<`bol}Nb>8jF8>{F`4jJ|wuaWN@BB6w+O{auc8w`$opS4p%&aFCZyy9NSvuxp zWxb&CHOj+UZ{oTFAAKinJDMefUtTUy0=4f&YOq8OuW^pGE?%F=>6HuGUL^BNX2*&T zXq|fJLIr<}4_(kWKYS9q=iS z!z{gjZHwdb^y2)#SlYeq>eM%I z>ny4N%JV4Y<0!FWjjl>}t-;GU;Q@Yf(=Og>>B!2sqD4mQwCS%TD~r~l9*#ru-hS!s z*VBV4{Iz0c(gioCWc&pf(%D+3DGj~_-!82u<9j=-a&MSuSHZg|?nb_cuirRNU_-;Z zw*&efNZ#b&pz!-ddUzTJ)Z5F6hB<&9bq5v8uKSm4yicTXk5j-RG-x0uwqPzs(#G89 zLkm4P8m%YS_Xt%I=mnI9MfFVhH>T#rp=wIYO3Ml+e8e0{y%!*QMghY9ZH01;wPA6| z^!?spOV2uNZ48CR8}p}IzFcR#MPVIU7o5!A8LRIa27N%2LU=jy%k>w%#I0S=U=`Tu z(JKPw%9~c7iW1EB(|D;eI4?!f+U77_;h?`pxCD)aS5g%eejH-ES{`^e%=RYbj*dBgC+|ar$WBf+gxgQ?f&zT$JY&P341j|}E2->w5 z4-phV^}FGOArfW~=58foX zlTfwN-x!iIsDX2ALzCWy;_SRN9e@()UoyLM0WnBVH`auCTj*Q&k;2jwAF(Cx57OcI zV|rs=?x#RBuT!%XPt^c}$W@YrHLI=BUV};t07??)H-suwO5Yi_5gLGd=H6or0L=?N5Dl zJ#LRB;b7>BU%07;zRK?1UOLU<47vplJf~Uchu*M zxi+!ig|$8Nl39eA!a{6o&aglG}Hop`yK_=k*nq zFldjpZoVb{SYD@J-PY9$+4Rq*O6@FJl9Q|05?JX6M(kxBuJB;whmckubU(edOW*ZP zHb_S8O6_*YgQu3LTM6sBWddf?;D_{N@HVscD)HNN_AyRyE)MDwG2H^*wWk@r#l-Cm9qRYgge zprsp-Ma<{S%M0e?CwbF5YgMY^K8FwIE_LUl_jAP$K13&IwKpEyiq{bGS-DO<(5b5n zXng!QJZ1{sbdQL-2_JXm#h3TFR@{&#J3N`PuJ7y^3Ab8xTt+;S58906^-(#9kc zefK>MGg3jGKiL3}?mdHwu54_yi&GJs*zQzo(xh=p`FCxvN-6aOB@Q!bjE9x2wBHpb z18*gT>+e@`z0Xad_6)k4a9~mCVpfX0Vfv}Kq%w1>vYX{f;(dx5sR!x3<=~`o~Je0ut#$F3V;u2rYL_1%K)rP9ry|c)D0h zc&m#)tnR$&qc`Sv6;!&1oR7{VkN>DlfuV{rj_95yMp~)t1`Qp5$8H}Lr&V$-XyH$q z$*(r!H<3E(OwtriURLl(SfYf{m;?tmaqxC&rE<_XIR@v=4nFj7TW{#$h1*!g+@_@# zJCzHprAh2FQAEF* zSEH#4T&57pUq-dUUV4-3K5MpUwsGj}_Nsko<1}qVG03%-7CI%^QCFxxDzKxu@hBsT$%(Bb`k zdz+@~ccUDE%xm~#121igNxjdj z{rr&>QYD{?VjM+OFf_XrrsUgh|o?1`w+4joUuDhB> z#n34liYv0jx2SnhHh%Bg(+6%_ymiQ4^C}LeEl9z4f78WC-+eoZ^V)UtVO`OUWVHR5vdtC@ydWB z`BE|mr+I|oqC8n;fk!|a)e2LGaDISqb@?R^%ETEivL3pKbIjSE#=IhR70-1X8q-kH z9+SGNC?&fc|MFk~ENhLd5d(?soIh+tahtBkv?Ej3 z^h!zN3@^`$OB;6k9v{N;m5xEGw?tKLZd8d6jIUq$`WRN(A{2-1>$uUAyk`@ihCe)s z4YG04czlmv_Q`bjmA9j#l7`V^&r7S~NgQGhUdp<23NO39Zf+pFYB2J4tSBVQtAx`S z(dCR>vJ-mGswripK1yrPNS>RVqm}eJk6(*IBs1%>$tKtNZJLLX5?9yw132EhT9e{$ z6mY_+Wkt(WOVpIq)L-e)X_)d6xR~7-PEVcG67hq+Tw4BAs4QFC@m^}`4cnNGfUm#l zbA$X#nbD|gm@~Af*TdQ2^W51I!4QU7&TV_QHF$RpWMXxkG@|P?w+g*}}g|1d)6;&9UkR~eX&G(d4IV1$l z=kkRnkS4w0xN4R7c+tiGB=6|z)C7I=x53OTtzyuJRl3yT&uD$o_xQfuZkdaqIrhCt zM|{T*fz-_n{73HGOoq`KLFAlIr$_yB?yfx3x3^=IITtkQY?FI2lrgb)dwW`bciNDr zgEXst;MHQ6$Gf?h{YOvCo29)4Kf!nqtOAo_w*B5u$8l1wY2Hkbcj=6!D~yaeXSJF% zpQEjm&MLJ}yK(qbuRsO1YpNf)ar3JvKP34fli5Ngi_f>N&*yigZl%eU2c4gJH-B&4 zpI2XqZXweJ&TF*$iQ;Up#ynKE%z6>>yZ)iQ&8mIRotZD>TCZFnW{bvoRqqJ7%gt%UpHMDoSH^^idNtmD1?=UVRd z%4@T;yiZ|SoCnl3_7fis4U!w!)R4$v8C)yO+D{zCVmncXj7cGM7C{*kjJVg!qY6v- z91TBToYmhY&U(&X*07TQ-e-d_*)Z-te10fx+Fyi-78NFW6C}XeG8f86bzm-)q#|!&v+$AIyDW8uVXbS*oUY&+3~Y z&+=U|CKrCxlW+%6LIN1Nn2DHoUu?*bsL24N4aH8<$6w3b-F(4gc~>Sw{N(mYGtJ0| zd2SR&Yv=X(!-by53)g#x9}hh|MpnAWU0`HKJh9H6*cGWYHY`x8_q(+(LS3LaeBs4s z;)14TKSDb`r69PKLrxK$0o8;4LdzQ)VH0az_rtMr(sO#;Z}qIs&4HgJsbBBf23gt9 zL@avFkTTvrnJNfv_w#V2emYL!&s%<>{QgExVtTAQl-Iz4mm$FT#4?ycvo)MCrEoF* z^;TIbl&Ce$H=ApIVqJjKy)BondtiXs*(Cu&T+MpHlV44{C;$@evk+d)oWH6DD=ho) zj^G<(dfca#j0Ghz);e#FrgunMs@Slpi*hUVO1kD3h+S0pG_2@?zqjl9JO=-)fmFCn zQ;F|m<3bBQcH@K%iSWG4)-8%u@1=O}qX9o-h6-XM3MIBh-USg%a}s;*kP&q`!U1GRr!Y6I+Jt!`0)w;-6Ctv-+Qiq z5uKV{(#xsYi8eI1i_)uqdi<3&jZRUG*I?FLLsJjnNKMK2?%N!!vM=;U7#nD?U@IhR z4C3IRH1EhHRnyH0$UyZCIlPA74v215tLMgWrKzF3N|SPHMJTI{i8Q9Mmh*CZMdEj! zo-Z7F5v&cDyFE^SolQErd-=W%kI%A)Kt0>L&PPHYzjH{E@vy2>s`Ib<)okSNdL$d= zB#=kyEz+#jW+K#@`tGdD4PURivUPnqYBjW^;E8t}%##02jixg_kEQ8brC>iSGNipN z;H=+9^E}l~=Y+M+y|8_CPpFsWw;Av-nQ9cwArleip<4QTHzwVz`Q$~2sWv1pQQYLcjau$ZA?xh0CXDcQK88J zTzzw`M>Q8E7)b5rU92^J-içZyBLlB)R_IlzzjXjC*HYqEgQn7_P%S{Xj!&YiY z!A0`kkG!`z7~+*FNK`vr?=wBQN^O1#9u(dZCMA<`Xu{bSaL}GxXiU5mz}Y!9!dHLm zvcW|+9oxq|8d~nntlbZ^6t^Vzx~bL@?l$yHe7)YhVY)TwXYD{*Yb!S4eobOJ{7U$o zyU(i@Vdm0W|MY@_=J#4QB8EWwdwpLn;G8!2P~DBp|a_;=+En zVq`<0)Zj84y6fbJ#K;V>yMIx(LBB1cwMlk**)ruEPeyPEr%J(0nw^rh3ERrd_d6QW z3Ut@6QL`DJACz&9e;jfcT74@(GmXlxnB$3Y{PVk9bqBoa_m0c4Nk{EiPSoNpg)m>M zKCh)&%Y2e`>-ny3Uqt28r>EXswX{KO@mo}wgvb4W!-J|PoPljJ> zUKt8*RJ@GfmFie7de&(9IOYXbl<-WLhla%=1FKFddsD*R^ZfzC$qFM}R(q^K!YHoR z@0|Ib*UU~@R%K)doK71zN&#MsbNWH&p!v7@bnS{qh2RLOTY%J1YyM=bFSw%UyG+Kdyp3e`rq9 zmkJfQA$1c2&p=_3*(-=i?Gq_Wf&v4h=m_ha$8|Bw;12^jdpt%v(fDSDeYe|9j2?y4 zVqo3EbA0|NhSqaT<}WMu+e_=9E2aE-zvH( zcHJno#D%!i>h?`0BQ>VPdEA7cHT})!T#0sMS?xQSclXkCxlVxuneedMOd{>eFkyR4 zuS;S>tsRWV0dsBFL|-X_16ttgB@c6tMPXeZzWVGf0>v1`Fj#J~F4Ih)2E#@&oR34N zSuo^_`R(oXcVP_^dn2UzJ;gceBvKFZzh;fga)tG>pP$Ax*mjb`v6Q{l7cI>a^T3qO z!lbU{TCO@vveB05kT;XME_-BH6BZUSvRLG6e9wx{@$y(%i|XwFtee36xA^A=2h4Yx z1ag+`UB#{v1vU+eTjEtu>7~=fP55j?EH+V4_bpuSabDY;Ui@tKP~w0oTvlB}c4&FQ zKqt+@uCA`1Vj2lx+ejP8CMSKzJq1LEZ7Ti- ItU8_if8pd9>Hq)$ literal 0 HcmV?d00001 diff --git a/camera_calibration_fisheye/doc/conf.py b/camera_calibration_fisheye/doc/conf.py new file mode 100644 index 000000000..82ad7fe0b --- /dev/null +++ b/camera_calibration_fisheye/doc/conf.py @@ -0,0 +1,201 @@ +# -*- coding: utf-8 -*- +# +# camera_calibration documentation build configuration file, created by +# sphinx-quickstart on Mon Jun 1 14:21:53 2009. +# +# This file is execfile()d with the current directory set to its containing dir. +# +# Note that not all possible configuration values are present in this +# autogenerated file. +# +# All configuration values have a default; values that are commented out +# serve to show the default. + +import sys, os + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +#sys.path.append(os.path.abspath('.')) + +# -- General configuration ----------------------------------------------------- + +# Add any Sphinx extension module names here, as strings. They can be extensions +# coming with Sphinx (named 'sphinx.ext.*') or your custom ones. +extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.pngmath'] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# The suffix of source filenames. +source_suffix = '.rst' + +# The encoding of source files. +#source_encoding = 'utf-8' + +# The master toctree document. +master_doc = 'index' + +# General information about the project. +project = 'camera_calibration' +copyright = '2009, Willow Garage, Inc.' + +# The version info for the project you're documenting, acts as replacement for +# |version| and |release|, also used in various other places throughout the +# built documents. +# +# The short X.Y version. +version = '0.1' +# The full version, including alpha/beta/rc tags. +release = '0.1.0' + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +#language = None + +# There are two options for replacing |today|: either, you set today to some +# non-false value, then it is used: +#today = '' +# Else, today_fmt is used as the format for a strftime call. +#today_fmt = '%B %d, %Y' + +# List of documents that shouldn't be included in the build. +#unused_docs = [] + +# List of directories, relative to source directory, that shouldn't be searched +# for source files. +exclude_trees = ['_build'] + +# The reST default role (used for this markup: `text`) to use for all documents. +#default_role = None + +# If true, '()' will be appended to :func: etc. cross-reference text. +#add_function_parentheses = True + +# If true, the current module name will be prepended to all description +# unit titles (such as .. function::). +#add_module_names = True + +# If true, sectionauthor and moduleauthor directives will be shown in the +# output. They are ignored by default. +#show_authors = False + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = 'sphinx' + +# A list of ignored prefixes for module index sorting. +#modindex_common_prefix = [] + + +# -- Options for HTML output --------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. Major themes that come with +# Sphinx are currently 'default' and 'sphinxdoc'. +html_theme = 'default' + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +#html_theme_options = {} + +# Add any paths that contain custom themes here, relative to this directory. +#html_theme_path = [] + +# The name for this set of Sphinx documents. If None, it defaults to +# " v documentation". +#html_title = None + +# A shorter title for the navigation bar. Default is the same as html_title. +#html_short_title = None + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +#html_logo = None + +# The name of an image file (within the static path) to use as favicon of the +# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +#html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +#html_static_path = ['_static'] + +# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, +# using the given strftime format. +#html_last_updated_fmt = '%b %d, %Y' + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +#html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +#html_sidebars = {} + +# Additional templates that should be rendered to pages, maps page names to +# template names. +#html_additional_pages = {} + +# If false, no module index is generated. +#html_use_modindex = True + +# If false, no index is generated. +#html_use_index = True + +# If true, the index is split into individual pages for each letter. +#html_split_index = False + +# If true, links to the reST sources are added to the pages. +#html_show_sourcelink = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +#html_use_opensearch = '' + +# If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). +#html_file_suffix = '' + +# Output file base name for HTML help builder. +htmlhelp_basename = 'camera_calibrationdoc' + + +# -- Options for LaTeX output -------------------------------------------------- + +# The paper size ('letter' or 'a4'). +#latex_paper_size = 'letter' + +# The font size ('10pt', '11pt' or '12pt'). +#latex_font_size = '10pt' + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, author, documentclass [howto/manual]). +latex_documents = [ + ('index', 'camera_calibration.tex', 'stereo\\_utils Documentation', + 'James Bowman', 'manual'), +] + +# The name of an image file (relative to this directory) to place at the top of +# the title page. +#latex_logo = None + +# For "manual" documents, if this is true, then toplevel headings are parts, +# not chapters. +#latex_use_parts = False + +# Additional stuff for the LaTeX preamble. +#latex_preamble = '' + +# Documents to append as an appendix to all manuals. +#latex_appendices = [] + +# If false, no module index is generated. +#latex_use_modindex = True + +# Example configuration for intersphinx: refer to the Python standard library. +intersphinx_mapping = { + 'http://docs.python.org/': None, + 'http://docs.scipy.org/doc/numpy' : None, + 'http://www.ros.org/doc/api/tf/html/python/' : None + } diff --git a/camera_calibration_fisheye/doc/index.rst b/camera_calibration_fisheye/doc/index.rst new file mode 100644 index 000000000..741e4e5b7 --- /dev/null +++ b/camera_calibration_fisheye/doc/index.rst @@ -0,0 +1,18 @@ +camera_calibration +================== + +The camera_calibration package contains a user-friendly calibration tool, +cameracalibrator. This tool uses the following Python classes, which +conveniently hide some of the complexities of using OpenCV's calibration +process and chessboard detection, and the details of constructing a ROS +CameraInfo message. These classes are documented here for people who +need to extend or make a new calibration tool. + +For details on the camera model and camera calibration process, see +http://docs.opencv.org/master/d9/d0c/group__calib3d.html + +.. autoclass:: camera_calibration.calibrator.MonoCalibrator + :members: + +.. autoclass:: camera_calibration.calibrator.StereoCalibrator + :members: diff --git a/camera_calibration_fisheye/fiseye_cal_reference/mono/calibrate.py b/camera_calibration_fisheye/fiseye_cal_reference/mono/calibrate.py new file mode 100644 index 000000000..c2d641b0c --- /dev/null +++ b/camera_calibration_fisheye/fiseye_cal_reference/mono/calibrate.py @@ -0,0 +1,95 @@ +import sys +import cv2 +import numpy as np +import os +from os import listdir + +def lryaml( name, d, k, r, p): # camera_name, distortion, intrinsics, R, P ... name, d, k, r, p + calmessage = ("" + + "image_width: " + str( size[0]) + "\n" + + "image_height: " + str( size[1]) + "\n" + + "camera_name: " + name + "\n" + + "camera_matrix:\n" + + " rows: 3\n" + + " cols: 3\n" + + " data: [" + ", ".join(["%8f" % i for i in k.reshape(1,9)[0]]) + "]\n" + + "distortion_model: " + ("rational_polynomial" if d.size > 5 else "plumb_bob") + "\n" + + "distortion_coefficients:\n" + + " rows: 1\n" + + " cols: 5\n" + + " data: [" + ", ".join(["%8f" % d[i,0] for i in range(d.shape[0])]) + "]\n" + + "rectification_matrix:\n" + + " rows: 3\n" + + " cols: 3\n" + + " data: [" + ", ".join(["%8f" % i for i in r.reshape(1,9)[0]]) + "]\n" + + "projection_matrix:\n" + + " rows: 3\n" + + " cols: 4\n" + + " data: [" + ", ".join(["%8f" % i for i in p.reshape(1,12)[0]]) + "]\n" + + "") + return calmessage + + +CHECKERBOARD = (6,8) +subpix_criteria = (cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1) +calibration_flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC+cv2.fisheye.CALIB_CHECK_COND+cv2.fisheye.CALIB_FIX_SKEW +objp = np.zeros((1, CHECKERBOARD[0]*CHECKERBOARD[1], 3), np.float32) +objp[0,:,:2] = np.mgrid[0:CHECKERBOARD[0], 0:CHECKERBOARD[1]].T.reshape(-1, 2) + +objpoints = [] # 3d point in real world space +imgpoints = [] # 2d points in image plane. + +origin_dir = 'calibrationdata/' + +images = os.listdir(origin_dir) + +_img_shape = None +for fname in images: + if fname.endswith(".png"): + img = cv2.imread(origin_dir+fname) + if _img_shape == None: + _img_shape = img.shape[:2] + else: + assert _img_shape == img.shape[:2], "All images must share the same size." + + gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) + # Find the chess board corners + ret, corners = cv2.findChessboardCorners(gray, CHECKERBOARD, cv2.CALIB_CB_ADAPTIVE_THRESH+cv2.CALIB_CB_FAST_CHECK+cv2.CALIB_CB_NORMALIZE_IMAGE) + + # If found, add object points, image points (after refining them) + if ret == True: + objpoints.append(objp) + cv2.cornerSubPix(gray,corners,(3,3),(-1,-1),subpix_criteria) + imgpoints.append(corners) + +N_OK = len(objpoints) +K = np.zeros((3, 3)) +D = np.zeros((4, 1)) +rvecs = [np.zeros((1, 1, 3), dtype=np.float64) for i in range(N_OK)] +tvecs = [np.zeros((1, 1, 3), dtype=np.float64) for i in range(N_OK)] + +rms, _, _, _, _ = cv2.fisheye.calibrate( + objpoints, + imgpoints, + gray.shape[::-1], + K, + D, + rvecs, + tvecs, + calibration_flags, + (cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6) + ) + +# R is identity matrix for monocular calibration +R = np.eye(3, dtype=np.float64) +P = np.zeros((3, 4), dtype=np.float64) + +print("Found " + str(N_OK) + " valid images for calibration") +print("DIM=" + str(_img_shape[::-1])) +print("K=np.array(" + str(K.tolist()) + ")") +print("D=np.array(" + str(D.tolist()) + ")") +print("R=np.array(" + str(R) + ")") +print("T=np.array(" + str(P) + ")") + + +# Write to Yaml diff --git a/camera_calibration_fisheye/fiseye_cal_reference/mono/test_undistort.py b/camera_calibration_fisheye/fiseye_cal_reference/mono/test_undistort.py new file mode 100644 index 000000000..7d4c4c54f --- /dev/null +++ b/camera_calibration_fisheye/fiseye_cal_reference/mono/test_undistort.py @@ -0,0 +1,42 @@ +import sys +#sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages') +import cv2 +assert cv2.__version__[0] == '3', 'The fisheye module requires opencv version >= 3.0.0' +import numpy as np +import os +from os import listdir +import glob + +# You should replace these 3 lines with the output in calibration step +DIM=(800, 503) +K=np.array([[290.0609844653627, 0.0, 399.7119405014219], [0.0, 289.1594924628207, 274.1984860354366], [0.0, 0.0, 1.0]]) +D=np.array([[-0.03122721497580657], [-0.020698242172871275], [0.0035143644102214144], [0.0006258984511625903]]) + +# DIM=(1920, 1208) +# K=np.array([[696.0621148872598, 0.0, 947.6971496853826], [0.0, 696.9356605571118, 651.8101521495024], [0.0, 0.0, 1.0]]) +# D=np.array([[-0.02814215088649154], [-0.03164955633645717], [0.016420714751802366], [-0.003441866971663222]]) + +methods = [("area", cv2.INTER_AREA), + ("nearest", cv2.INTER_NEAREST), + ("linear", cv2.INTER_LINEAR), + ("cubic", cv2.INTER_CUBIC), + ("lanczos4", cv2.INTER_LANCZOS4)] + +def undistort(img_path): + img = cv2.imread(img_path) + h,w = img.shape[:2] + + map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM, cv2.CV_16SC2) + undistorted_img = [] + for i,m in enumerate(methods): + print "interpolation ", m[0] + undistorted_img = cv2.remap(img, map1, map2, interpolation=m[1], borderMode=cv2.BORDER_CONSTANT) + img = np.hstack((img, undistorted_img)) + print img.shape + + cv2.imshow("undistorted", img) + cv2.waitKey(0) + cv2.destroyAllWindows() +if __name__ == '__main__': + for p in sys.argv[1:]: + undistort(p) diff --git a/camera_calibration_fisheye/fiseye_cal_reference/mono/test_undistorted_full.py b/camera_calibration_fisheye/fiseye_cal_reference/mono/test_undistorted_full.py new file mode 100644 index 000000000..8287e98d1 --- /dev/null +++ b/camera_calibration_fisheye/fiseye_cal_reference/mono/test_undistorted_full.py @@ -0,0 +1,54 @@ +import sys +#sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages') +import cv2 +assert cv2.__version__[0] == '3', 'The fisheye module requires opencv version >= 3.0.0' +import numpy as np +import os +from os import listdir +import glob + +# You should replace these 3 lines with the output in calibration step +# DIM=(800, 503) +# K=np.array([[290.0609844653627, 0.0, 399.7119405014219], [0.0, 289.1594924628207, 274.1984860354366], [0.0, 0.0, 1.0]]) +# D=np.array([[-0.03122721497580657], [-0.020698242172871275], [0.0035143644102214144], [0.0006258984511625903]]) + +DIM=(1920, 1208) +K=np.array([[696.0621148872598, 0.0, 947.6971496853826], [0.0, 696.9356605571118, 651.8101521495024], [0.0, 0.0, 1.0]]) +D=np.array([[-0.02814215088649154], [-0.03164955633645717], [0.016420714751802366], [-0.003441866971663222]]) + +def undistort(img_path, balance=0.0, dim2=None, dim3=None): + img = cv2.imread(img_path) + dim1 = img.shape[:2][::-1] #dim1 is the dimension of input image to un-distort + assert dim1[0]/dim1[1] == DIM[0]/DIM[1], "Image to undistort needs to have same aspect ratio as the ones used in calibration" + if not dim2: + dim2 = dim1 + + if not dim3: + dim3 = dim1 + + scaled_K = K * dim1[0] / DIM[0] # The values of K is to scale with image dimension. + scaled_K[2][2] = 1.0 # Except that K[2][2] is always 1.0 + # This is how scaled_K, dim2 and balance are used to determine the final K used to un-distort image. OpenCV document failed to make this clear! + print "dim1 ",dim1, "dim2 ",dim2, "dim3 ",dim3 + new_K = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(scaled_K, D, dim2, np.eye(3), balance=1.0) + new_K[0][2] = int(dim2[0]/2) - 0# Set manually the center of the img in New Camera matrix. X axis + new_K[1][2] = int(dim2[1]/2) + 0 # Y-axis + + ###### + map1, map2 = cv2.fisheye.initUndistortRectifyMap(scaled_K, D, np.eye(3), scaled_K, dim3, cv2.CV_16SC2) + #print new_K + undistorted_img = cv2.remap(img, map1, map2, interpolation=cv2.INTER_CUBIC, borderMode=cv2.BORDER_CONSTANT) + + ## + ##undistorted_img = cv2.resize(undistorted_img,(800, 503),interpolation=cv2.INTER_CUBIC) + + cv2.imshow("undistorted", undistorted_img) + cv2.waitKey(0) + cv2.destroyAllWindows() +if __name__ == '__main__': + if len(sys.argv)==2: + undistort( sys.argv[1]) + elif len(sys.argv)==3: + undistort( sys.argv[1],dim2=tuple( map(int, sys.argv[2].split(',') ) ) ) + elif len(sys.argv)==4: + undistort( sys.argv[1],dim2=tuple( map(int, sys.argv[2].split(',') ) ),dim3=tuple( map(int, sys.argv[3].split(',') ) ) ) \ No newline at end of file diff --git a/camera_calibration_fisheye/fiseye_cal_reference/stereo/CMakeLists.txt b/camera_calibration_fisheye/fiseye_cal_reference/stereo/CMakeLists.txt new file mode 100644 index 000000000..ffc67b7cb --- /dev/null +++ b/camera_calibration_fisheye/fiseye_cal_reference/stereo/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 2.8.11) +project(FISHEYE_STEREO) + +set(CMAKE_INCLUDE_CURRENT_DIR ON) +set(CMAKE_CXX_FLAGS "-O3 -Wall -Wextra") +set(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -Wextra") + +find_package(OpenCV 3.4 EXACT REQUIRED PATHS /usr/local/) +include_directories($(OpenCV_INCLUDE_DIRS)) + +add_executable(calibrate calibrate.cpp) +target_link_libraries(calibrate ${OpenCV_LIBS} "-lpopt") \ No newline at end of file diff --git a/camera_calibration_fisheye/fiseye_cal_reference/stereo/README.md b/camera_calibration_fisheye/fiseye_cal_reference/stereo/README.md new file mode 100644 index 000000000..026a1e588 --- /dev/null +++ b/camera_calibration_fisheye/fiseye_cal_reference/stereo/README.md @@ -0,0 +1,38 @@ +## OpenCV C++ Stereo Fisheye Calibration + +This contains a source file to calibrate a stereo system comprising of fisheye lenses. It calibrates the extrinsics and the intrinsics of the cameras without any initial guesses. If you are looking for stereo calibration with lenses which follow the pinhole model check [here](https://github.com/sourishg/stereo_calibration). + +### Dependencies + +- OpenCV +- popt + +### Compilation + +Compile all the files using the following commands. + +```bash +mkdir build && cd build +cmake .. +make +``` + +Make sure your are in the `build` folder to run the executables. + +### Data + +Some sample calibration images are stored in the `imgs` folder. + +### Running calibration + +Run the executable with the following command + +```bash +./calibrate -w [board_width] -h [board_height] -s [square_size] -n [num_imgs] -d [img_dir] -l [left_img_prefix] -r [right_img_prefix] -o [calib_file] +``` + +For example if you use the images in the `imgs` folder run the following command + +```bash +./calibrate -w 9 -h 6 -s 0.02423 -n 29 -d ../imgs/ -l left -r right -o cam_stereo.yml +``` \ No newline at end of file diff --git a/camera_calibration_fisheye/fiseye_cal_reference/stereo/calibrate.cpp b/camera_calibration_fisheye/fiseye_cal_reference/stereo/calibrate.cpp new file mode 100644 index 000000000..4a050aef3 --- /dev/null +++ b/camera_calibration_fisheye/fiseye_cal_reference/stereo/calibrate.cpp @@ -0,0 +1,142 @@ +#include +#include +#include +#include +#include +#include +#include "popt_pp.h" + +using namespace std; +using namespace cv; + +vector< vector< Point3d > > object_points; +vector< vector< Point2f > > imagePoints1, imagePoints2; +vector< Point2f > corners1, corners2; +vector< vector< Point2d > > left_img_points, right_img_points; + +Mat img1, img2, gray1, gray2, spl1, spl2; + +void load_image_points(int board_width, int board_height, float square_size, int num_imgs, + char* img_dir, char* leftimg_filename, char* rightimg_filename) { + Size board_size = Size(board_width, board_height); + int board_n = board_width * board_height; + + for (int i = 1; i <= num_imgs; i++) { + char left_img[100], right_img[100]; + sprintf(left_img, "%s%s%02d.png", img_dir, leftimg_filename, i); + sprintf(right_img, "%s%s%02d.png", img_dir, rightimg_filename, i); + img1 = imread(left_img, CV_LOAD_IMAGE_COLOR); + img2 = imread(right_img, CV_LOAD_IMAGE_COLOR); + cv::cvtColor(img1, gray1, CV_BGR2GRAY); + cv::cvtColor(img2, gray2, CV_BGR2GRAY); + + bool found1 = false, found2 = false; + + found1 = cv::findChessboardCorners(img1, board_size, corners1, + CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS); + found2 = cv::findChessboardCorners(img2, board_size, corners2, + CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS); + + if (found1) + { + cv::cornerSubPix(gray1, corners1, cv::Size(5, 5), cv::Size(-1, -1), + cv::TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1)); + cv::drawChessboardCorners(gray1, board_size, corners1, found1); + } + if (found2) + { + cv::cornerSubPix(gray2, corners2, cv::Size(5, 5), cv::Size(-1, -1), + cv::TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1)); + cv::drawChessboardCorners(gray2, board_size, corners2, found2); + } + + vector obj; + for( int i = 0; i < board_height; ++i ) + for( int j = 0; j < board_width; ++j ) + obj.push_back(Point3d(double( (float)j * square_size ), double( (float)i * square_size ), 0)); + + if (found1 && found2) { + cout << i << ". Found corners!" << endl; + imagePoints1.push_back(corners1); + imagePoints2.push_back(corners2); + object_points.push_back(obj); + } + } + for (int i = 0; i < imagePoints1.size(); i++) { + vector< Point2d > v1, v2; + for (int j = 0; j < imagePoints1[i].size(); j++) { + v1.push_back(Point2d((double)imagePoints1[i][j].x, (double)imagePoints1[i][j].y)); + v2.push_back(Point2d((double)imagePoints2[i][j].x, (double)imagePoints2[i][j].y)); + } + left_img_points.push_back(v1); + right_img_points.push_back(v2); + } +} + +int main(int argc, char const *argv[]) +{ + int board_width, board_height, num_imgs; + float square_size; + char* img_dir; + char* leftimg_filename; + char* rightimg_filename; + char* out_file; + + static struct poptOption options[] = { + { "board_width",'w',POPT_ARG_INT,&board_width,0,"Checkerboard width","NUM" }, + { "board_height",'h',POPT_ARG_INT,&board_height,0,"Checkerboard height","NUM" }, + { "square_size",'s',POPT_ARG_FLOAT,&square_size,0,"Checkerboard square size","NUM" }, + { "num_imgs",'n',POPT_ARG_INT,&num_imgs,0,"Number of checkerboard images","NUM" }, + { "img_dir",'d',POPT_ARG_STRING,&img_dir,0,"Directory containing images","STR" }, + { "leftimg_filename",'l',POPT_ARG_STRING,&leftimg_filename,0,"Left image prefix","STR" }, + { "rightimg_filename",'r',POPT_ARG_STRING,&rightimg_filename,0,"Right image prefix","STR" }, + { "out_file",'o',POPT_ARG_STRING,&out_file,0,"Output calibration filename (YML)","STR" }, + POPT_AUTOHELP + { NULL, 0, 0, NULL, 0, NULL, NULL } + }; + + POpt popt(NULL, argc, argv, options, 0); + int c; + while((c = popt.getNextOpt()) >= 0) {} + + load_image_points(board_width, board_height, square_size, num_imgs, img_dir, leftimg_filename, rightimg_filename); + + printf("Starting Calibration\n"); + cv::Matx33d K1, K2, R; + cv::Vec3d T; + cv::Vec4d D1, D2; + int flag = 0; + flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC; + flag |= cv::fisheye::CALIB_CHECK_COND; + flag |= cv::fisheye::CALIB_FIX_SKEW; + //flag |= cv::fisheye::CALIB_FIX_K2; + //flag |= cv::fisheye::CALIB_FIX_K3; + //flag |= cv::fisheye::CALIB_FIX_K4; + cv::fisheye::stereoCalibrate(object_points, left_img_points, right_img_points, + K1, D1, K2, D2, img1.size(), R, T, flag, + cv::TermCriteria(3, 12, 0)); + + cv::FileStorage fs1(out_file, cv::FileStorage::WRITE); + fs1 << "K1" << Mat(K1); + fs1 << "K2" << Mat(K2); + fs1 << "D1" << D1; + fs1 << "D2" << D2; + fs1 << "R" << Mat(R); + fs1 << "T" << T; + printf("Done Calibration\n"); + + printf("Starting Rectification\n"); + + cv::Mat R1, R2, P1, P2, Q; + cv::fisheye::stereoRectify(K1, D1, K2, D2, img1.size(), R, T, R1, R2, P1, P2, +Q, CV_CALIB_ZERO_DISPARITY, img1.size(), 0.0, 1.1); + + fs1 << "R1" << R1; + fs1 << "R2" << R2; + fs1 << "P1" << P1; + fs1 << "P2" << P2; + fs1 << "Q" << Q; + + printf("Done Rectification\n"); + return 0; +} diff --git a/camera_calibration_fisheye/fiseye_cal_reference/stereo/popt_pp.h b/camera_calibration_fisheye/fiseye_cal_reference/stereo/popt_pp.h new file mode 100644 index 000000000..61c5bdb82 --- /dev/null +++ b/camera_calibration_fisheye/fiseye_cal_reference/stereo/popt_pp.h @@ -0,0 +1,39 @@ +#ifndef _INCLUDED_POPT_PP_H_ +#define _INCLUDED_POPT_PP_H_ + +#include + +class POpt{ +protected: + poptContext con; +public: + // creation and deletion + POpt(const char *name, int argc, const char **argv, + const poptOption *options, int flags) + {con = poptGetContext(name,argc,argv,options,flags);} + POpt(const char *name, int argc, char **argv, + const poptOption *options, int flags) + {con = poptGetContext(name,argc,(const char **)argv,options,flags);} + ~POpt() + {poptFreeContext(con);} + + // functions for processing options + int getNextOpt() + {return(poptGetNextOpt(con));} + void ignoreOptions() + {while(getNextOpt() >= 0);} + const char *getOptArg() + {return(poptGetOptArg(con));} + const char *strError(int error) + {return(poptStrerror(error));} + const char *badOption(int flags = POPT_BADOPTION_NOALIAS) + {return(poptBadOption(con,flags));} + + // processing other arguments + const char *getArg() + {return(poptGetArg(con));} + void ignoreArgs() + {while(getArg());} +}; + +#endif diff --git a/camera_calibration_fisheye/mainpage.dox b/camera_calibration_fisheye/mainpage.dox new file mode 100644 index 000000000..dbcfe690e --- /dev/null +++ b/camera_calibration_fisheye/mainpage.dox @@ -0,0 +1,59 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b The camera_calibration package contains tools for calibrating monocular and stereo cameras. + +\section codeapi Code API + +camera_calibration does not have a code API. + +\section rosapi ROS API + +List of nodes: +- \b calibrationnode + + + +
+ +\subsection node_name calibrationnode + +calibrationnode subscribes to ROS raw image topics, and presents a +calibration window. It can run in both monocular and stereo modes. +The calibration window shows the current images from the cameras, +highlighting the checkerboard. When the user presses the "CALIBRATE" +button, the node computes the camera calibration parameters. When the +user clicks "UPLOAD", the node uploads these new calibration parameters +to the camera driver using a service call. + +\subsubsection Usage +\verbatim +$ node_type1 [standard ROS args] +\endverbatim + +\par Example + +\verbatim +$ rosrun camera_calibration cal.py right:=/my_stereo/right/image_raw left:=/my_stereo/left/image_raw left_camera:=/my_stereo/left right_camera:=/my_stereo/right +\endverbatim + + +\subsubsection topics ROS topics + +Subscribes to: +- \b "left": [sensor_msgs/Image] left raw image topic, for stereo cameras +- \b "right": [sensor_msgs/Image] left raw image topic, for stereo cameras +- \b "image": [sensor_msgs/Image] raw image topic, for monocular cameras + +Makes service calls to: + +\subsubsection services ROS services +- \b "foo_service": [std_srvs/FooType] description of foo_service +- \b "camera/set_camera_info": [sensor_msgs/SetCameraInfo] node of the camera for monocular +- \b "left_camera/set_camera_info": [sensor_msgs/SetCameraInfo] node of the left stereo camera +- \b "right_camera/set_camera_info": [sensor_msgs/SetCameraInfo] node of the left stereo camera + + + +*/ diff --git a/camera_calibration_fisheye/nodes/cameracalibrator.py b/camera_calibration_fisheye/nodes/cameracalibrator.py new file mode 100644 index 000000000..206962e1d --- /dev/null +++ b/camera_calibration_fisheye/nodes/cameracalibrator.py @@ -0,0 +1,154 @@ +#!/usr/bin/python +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2009, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import cv2 +import functools +import message_filters +import os +import rospy +from camera_calibration.camera_calibrator import OpenCVCalibrationNode +from camera_calibration.calibrator import ChessboardInfo, Patterns +from message_filters import ApproximateTimeSynchronizer + + +def main(): + from optparse import OptionParser, OptionGroup + parser = OptionParser("%prog --size SIZE1 --square SQUARE1 [ --size SIZE2 --square SQUARE2 ]", + description=None) + parser.add_option("-c", "--camera_name", + type="string", default='narrow_stereo', + help="name of the camera to appear in the calibration file") + group = OptionGroup(parser, "Chessboard Options", + "You must specify one or more chessboards as pairs of --size and --square options.") + group.add_option("-p", "--pattern", + type="string", default="chessboard", + help="calibration pattern to detect - 'chessboard', 'circles', 'acircles'") + group.add_option("-s", "--size", + action="append", default=[], + help="chessboard size as NxM, counting interior corners (e.g. a standard chessboard is 7x7)") + group.add_option("-q", "--square", + action="append", default=[], + help="chessboard square size in meters") + parser.add_option_group(group) + group = OptionGroup(parser, "ROS Communication Options") + group.add_option("--approximate", + type="float", default=0.0, + help="allow specified slop (in seconds) when pairing images from unsynchronized stereo cameras") + group.add_option("--no-service-check", + action="store_false", dest="service_check", default=True, + help="disable check for set_camera_info services at startup") + parser.add_option_group(group) + group = OptionGroup(parser, "Calibration Optimizer Options") + group.add_option("--fix-principal-point", + action="store_true", default=False, + help="fix the principal point at the image center") + group.add_option("--fix-aspect-ratio", + action="store_true", default=False, + help="enforce focal lengths (fx, fy) are equal") + group.add_option("--zero-tangent-dist", + action="store_true", default=False, + help="set tangential distortion coefficients (p1, p2) to zero") + group.add_option("-k", "--k-coefficients", + type="int", default=2, metavar="NUM_COEFFS", + help="number of radial distortion coefficients to use (up to 6, default %default)") + group.add_option("--disable_calib_cb_fast_check", action='store_true', default=False, + help="uses the CALIB_CB_FAST_CHECK flag for findChessboardCorners") + parser.add_option_group(group) + options, args = parser.parse_args() + + if len(options.size) != len(options.square): + parser.error("Number of size and square inputs must be the same!") + + if not options.square: + options.square.append("0.108") + options.size.append("8x6") + + boards = [] + for (sz, sq) in zip(options.size, options.square): + size = tuple([int(c) for c in sz.split('x')]) + boards.append(ChessboardInfo(size[0], size[1], float(sq))) + + if options.approximate == 0.0: + sync = message_filters.TimeSynchronizer + else: + sync = functools.partial(ApproximateTimeSynchronizer, slop=options.approximate) + + num_ks = options.k_coefficients + + calib_flags = 0 + if options.fix_principal_point: + calib_flags |= cv2.CALIB_FIX_PRINCIPAL_POINT + if options.fix_aspect_ratio: + calib_flags |= cv2.CALIB_FIX_ASPECT_RATIO + if options.zero_tangent_dist: + calib_flags |= cv2.CALIB_ZERO_TANGENT_DIST + if (num_ks > 3): + calib_flags |= cv2.CALIB_RATIONAL_MODEL + if (num_ks < 6): + calib_flags |= cv2.CALIB_FIX_K6 + if (num_ks < 5): + calib_flags |= cv2.CALIB_FIX_K5 + if (num_ks < 4): + calib_flags |= cv2.CALIB_FIX_K4 + if (num_ks < 3): + calib_flags |= cv2.CALIB_FIX_K3 + if (num_ks < 2): + calib_flags |= cv2.CALIB_FIX_K2 + if (num_ks < 1): + calib_flags |= cv2.CALIB_FIX_K1 + + pattern = Patterns.Chessboard + if options.pattern == 'circles': + pattern = Patterns.Circles + elif options.pattern == 'acircles': + pattern = Patterns.ACircles + elif options.pattern != 'chessboard': + print('Unrecognized pattern %s, defaulting to chessboard' % options.pattern) + + if options.disable_calib_cb_fast_check: + checkerboard_flags = 0 + else: + checkerboard_flags = cv2.CALIB_CB_FAST_CHECK + + rospy.init_node('cameracalibrator') + node = OpenCVCalibrationNode(boards, options.service_check, sync, calib_flags, pattern, options.camera_name, + checkerboard_flags=checkerboard_flags) + rospy.spin() + +if __name__ == "__main__": + try: + main() + except Exception as e: + import traceback + traceback.print_exc() diff --git a/camera_calibration_fisheye/nodes/cameracheck.py b/camera_calibration_fisheye/nodes/cameracheck.py new file mode 100644 index 000000000..6af00d621 --- /dev/null +++ b/camera_calibration_fisheye/nodes/cameracheck.py @@ -0,0 +1,57 @@ +#!/usr/bin/python +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2009, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import rospy +from camera_calibration.camera_checker import CameraCheckerNode + + +def main(): + from optparse import OptionParser + rospy.init_node('cameracheck') + parser = OptionParser() + parser.add_option("-s", "--size", default="8x6", help="specify chessboard size as nxm [default: %default]") + parser.add_option("-q", "--square", default=".108", help="specify chessboard square size in meters [default: %default]") + parser.add_option("--approximate", + type="float", default=0.0, + help="allow specified slop (in seconds) when pairing images from unsynchronized stereo cameras") + + options, args = parser.parse_args() + size = tuple([int(c) for c in options.size.split('x')]) + dim = float(options.square) + approximate = float(options.approximate) + CameraCheckerNode(size, dim, approximate) + rospy.spin() + +if __name__ == "__main__": + main() diff --git a/camera_calibration_fisheye/package.xml b/camera_calibration_fisheye/package.xml new file mode 100644 index 000000000..971dbf2f6 --- /dev/null +++ b/camera_calibration_fisheye/package.xml @@ -0,0 +1,31 @@ + + camera_calibration_fisheye + 1.12.23 + + camera_calibration allows easy calibration of monocular or stereo + cameras using a checkerboard calibration target. + + James Bowman + Patrick Mihelich + David Torres + David Torres + + catkin + + rostest + + cv_bridge + image_geometry + message_filters + rospy + std_srvs + sensor_msgs + + BSD + http://www.ros.org/wiki/camera_calibration + + + + + + diff --git a/camera_calibration_fisheye/rosdoc.yaml b/camera_calibration_fisheye/rosdoc.yaml new file mode 100644 index 000000000..0136cd7a2 --- /dev/null +++ b/camera_calibration_fisheye/rosdoc.yaml @@ -0,0 +1,4 @@ + - builder: sphinx + name: Python API + output_dir: python + sphinx_root_dir: doc diff --git a/camera_calibration_fisheye/scripts/tarfile_calibration.py b/camera_calibration_fisheye/scripts/tarfile_calibration.py new file mode 100644 index 000000000..5d552ac5f --- /dev/null +++ b/camera_calibration_fisheye/scripts/tarfile_calibration.py @@ -0,0 +1,235 @@ +#!/usr/bin/env python +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2009, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import os +import sys +import numpy + +import cv2 +import cv_bridge +import tarfile + +from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, CalibrationException, ChessboardInfo + +import rospy +import sensor_msgs.srv + +def waitkey(): + k = cv2.waitKey(6) + return k + +def display(win_name, img): + cv2.namedWindow(win_name, cv2.WINDOW_NORMAL) + cv2.imshow( win_name, numpy.asarray( img[:,:] )) + k=-1 + while k ==-1: + k=waitkey() + cv2.destroyWindow(win_name) + if k in [27, ord('q')]: + rospy.signal_shutdown('Quit') + + +def cal_from_tarfile(boards, tarname, mono = False, upload = False, calib_flags = 0, visualize = False, alpha=1.0): + if mono: + calibrator = MonoCalibrator(boards, calib_flags) + else: + calibrator = StereoCalibrator(boards, calib_flags) + + calibrator.do_tarfile_calibration(tarname) + + print(calibrator.ost()) + + if upload: + info = calibrator.as_message() + if mono: + set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo) + + response = set_camera_info_service(info) + if not response.success: + raise RuntimeError("connected to set_camera_info service, but failed setting camera_info") + else: + set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo) + set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo) + + response1 = set_left_camera_info_service(info[0]) + response2 = set_right_camera_info_service(info[1]) + if not (response1.success and response2.success): + raise RuntimeError("connected to set_camera_info service, but failed setting camera_info") + + if visualize: + + #Show rectified images + calibrator.set_alpha(alpha) + + archive = tarfile.open(tarname, 'r') + if mono: + for f in archive.getnames(): + if f.startswith('left') and (f.endswith('.pgm') or f.endswith('png')): + filedata = archive.extractfile(f).read() + file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8) + im=cv2.imdecode(file_bytes,cv2.IMREAD_COLOR) + + bridge = cv_bridge.CvBridge() + try: + msg=bridge.cv2_to_imgmsg(im, "bgr8") + except cv_bridge.CvBridgeError as e: + print(e) + + #handle msg returns the recitifed image with corner detection once camera is calibrated. + drawable=calibrator.handle_msg(msg) + vis=numpy.asarray( drawable.scrib[:,:]) + #Display. Name of window:f + display(f, vis) + else: + limages = [ f for f in archive.getnames() if (f.startswith('left') and (f.endswith('pgm') or f.endswith('png'))) ] + limages.sort() + rimages = [ f for f in archive.getnames() if (f.startswith('right') and (f.endswith('pgm') or f.endswith('png'))) ] + rimages.sort() + + if not len(limages) == len(rimages): + raise RuntimeError("Left, right images don't match. %d left images, %d right" % (len(limages), len(rimages))) + + for i in range(len(limages)): + l=limages[i] + r=rimages[i] + + if l.startswith('left') and (l.endswith('.pgm') or l.endswith('png')) and r.startswith('right') and (r.endswith('.pgm') or r.endswith('png')): + # LEFT IMAGE + filedata = archive.extractfile(l).read() + file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8) + im_left=cv2.imdecode(file_bytes,cv2.IMREAD_COLOR) + + bridge = cv_bridge.CvBridge() + try: + msg_left=bridge.cv2_to_imgmsg(im_left, "bgr8") + except cv_bridge.CvBridgeError as e: + print(e) + + #RIGHT IMAGE + filedata = archive.extractfile(r).read() + file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8) + im_right=cv2.imdecode(file_bytes,cv2.IMREAD_COLOR) + try: + msg_right=bridge.cv2_to_imgmsg(im_right, "bgr8") + except cv_bridge.CvBridgeError as e: + print(e) + + drawable=calibrator.handle_msg([ msg_left,msg_right] ) + + h, w = numpy.asarray(drawable.lscrib[:,:]).shape[:2] + vis = numpy.zeros((h, w*2, 3), numpy.uint8) + vis[:h, :w ,:] = numpy.asarray(drawable.lscrib[:,:]) + vis[:h, w:w*2, :] = numpy.asarray(drawable.rscrib[:,:]) + + display(l+" "+r,vis) + + +if __name__ == '__main__': + from optparse import OptionParser + parser = OptionParser("%prog TARFILE [ opts ]") + parser.add_option("--mono", default=False, action="store_true", dest="mono", + help="Monocular calibration only. Calibrates left images.") + parser.add_option("-s", "--size", default=[], action="append", dest="size", + help="specify chessboard size as NxM [default: 8x6]") + parser.add_option("-q", "--square", default=[], action="append", dest="square", + help="specify chessboard square size in meters [default: 0.108]") + parser.add_option("--upload", default=False, action="store_true", dest="upload", + help="Upload results to camera(s).") + parser.add_option("--fix-principal-point", action="store_true", default=False, + help="fix the principal point at the image center") + parser.add_option("--fix-aspect-ratio", action="store_true", default=False, + help="enforce focal lengths (fx, fy) are equal") + parser.add_option("--zero-tangent-dist", action="store_true", default=False, + help="set tangential distortion coefficients (p1, p2) to zero") + parser.add_option("-k", "--k-coefficients", type="int", default=2, metavar="NUM_COEFFS", + help="number of radial distortion coefficients to use (up to 6, default %default)") + parser.add_option("--visualize", action="store_true", default=False, + help="visualize rectified images after calibration") + parser.add_option("-a", "--alpha", type="float", default=1.0, metavar="ALPHA", + help="zoom for visualization of rectifies images. Ranges from 0 (zoomed in, all pixels in calibrated image are valid) to 1 (zoomed out, all pixels in original image are in calibrated image). default %default)") + + options, args = parser.parse_args() + + if len(options.size) != len(options.square): + parser.error("Number of size and square inputs must be the same!") + + if not options.square: + options.square.append("0.108") + options.size.append("8x6") + + boards = [] + for (sz, sq) in zip(options.size, options.square): + info = ChessboardInfo() + info.dim = float(sq) + size = tuple([int(c) for c in sz.split('x')]) + info.n_cols = size[0] + info.n_rows = size[1] + + boards.append(info) + + if not boards: + parser.error("Must supply at least one chessboard") + + if not args: + parser.error("Must give tarfile name") + if not os.path.exists(args[0]): + parser.error("Tarfile %s does not exist. Please select valid tarfile" % args[0]) + + tarname = args[0] + + num_ks = options.k_coefficients + + calib_flags = 0 + if options.fix_principal_point: + calib_flags |= cv2.CALIB_FIX_PRINCIPAL_POINT + if options.fix_aspect_ratio: + calib_flags |= cv2.CALIB_FIX_ASPECT_RATIO + if options.zero_tangent_dist: + calib_flags |= cv2.CALIB_ZERO_TANGENT_DIST + if (num_ks > 3): + calib_flags |= cv2.CALIB_RATIONAL_MODEL + if (num_ks < 6): + calib_flags |= cv2.CALIB_FIX_K6 + if (num_ks < 5): + calib_flags |= cv2.CALIB_FIX_K5 + if (num_ks < 4): + calib_flags |= cv2.CALIB_FIX_K4 + if (num_ks < 3): + calib_flags |= cv2.CALIB_FIX_K3 + if (num_ks < 2): + calib_flags |= cv2.CALIB_FIX_K2 + if (num_ks < 1): + calib_flags |= cv2.CALIB_FIX_K1 + + cal_from_tarfile(boards, tarname, options.mono, options.upload, calib_flags, options.visualize, options.alpha) diff --git a/camera_calibration_fisheye/setup.py b/camera_calibration_fisheye/setup.py new file mode 100644 index 000000000..fd51af2ee --- /dev/null +++ b/camera_calibration_fisheye/setup.py @@ -0,0 +1,9 @@ +#!/usr/bin/env python +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup() +d['packages'] = ['camera_calibration'] +d['package_dir'] = {'':'src'} + +setup(**d) diff --git a/camera_calibration_fisheye/src/camera_calibration/__init__.py b/camera_calibration_fisheye/src/camera_calibration/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/camera_calibration_fisheye/src/camera_calibration/calibrator.py b/camera_calibration_fisheye/src/camera_calibration/calibrator.py new file mode 100644 index 000000000..eabe4b485 --- /dev/null +++ b/camera_calibration_fisheye/src/camera_calibration/calibrator.py @@ -0,0 +1,1182 @@ +#!/usr/bin/env python +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2009, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +try: + from cStringIO import StringIO +except ImportError: + from io import StringIO +from io import BytesIO +import cv2 +assert cv2.__version__[0] == '3', 'The fisheye module requires opencv version >= 3.0.0' + +import cv_bridge +import image_geometry +import math +import numpy.linalg +import pickle +import random +import sensor_msgs.msg +import tarfile +import time +from distutils.version import LooseVersion + + +# Supported calibration patterns +class Patterns: + Chessboard, Circles, ACircles = list(range(3)) + +class CalibrationException(Exception): + pass + +# TODO: Make pattern per-board? +class ChessboardInfo(object): + def __init__(self, n_cols = 0, n_rows = 0, dim = 0.0): + self.n_cols = n_cols + self.n_rows = n_rows + self.dim = dim + +# Make all private!!!!! +def lmin(seq1, seq2): + """ Pairwise minimum of two sequences """ + return [min(a, b) for (a, b) in zip(seq1, seq2)] + +def lmax(seq1, seq2): + """ Pairwise maximum of two sequences """ + return [max(a, b) for (a, b) in zip(seq1, seq2)] + +def _pdist(p1, p2): + """ + Distance bwt two points. p1 = (x, y), p2 = (x, y) + """ + return math.sqrt(math.pow(p1[0] - p2[0], 2) + math.pow(p1[1] - p2[1], 2)) + +def _get_outside_corners(corners, board): + """ + Return the four corners of the board as a whole, as (up_left, up_right, down_right, down_left). + """ + xdim = board.n_cols + ydim = board.n_rows + + if corners.shape[1] * corners.shape[0] != xdim * ydim: + raise Exception("Invalid number of corners! %d corners. X: %d, Y: %d" % (corners.shape[1] * corners.shape[0], + xdim, ydim)) + + up_left = corners[0,0] + up_right = corners[xdim - 1,0] + down_right = corners[-1,0] + down_left = corners[-xdim,0] + + return (up_left, up_right, down_right, down_left) + +def _get_skew(corners, board): + """ + Get skew for given checkerboard detection. + Scaled to [0,1], which 0 = no skew, 1 = high skew + Skew is proportional to the divergence of three outside corners from 90 degrees. + """ + # TODO Using three nearby interior corners might be more robust, outside corners occasionally + # get mis-detected + up_left, up_right, down_right, _ = _get_outside_corners(corners, board) + + def angle(a, b, c): + """ + Return angle between lines ab, bc + """ + ab = a - b + cb = c - b + return math.acos(numpy.dot(ab,cb) / (numpy.linalg.norm(ab) * numpy.linalg.norm(cb))) + + skew = min(1.0, 2. * abs((math.pi / 2.) - angle(up_left, up_right, down_right))) + return skew + +def _get_area(corners, board): + """ + Get 2d image area of the detected checkerboard. + The projected checkerboard is assumed to be a convex quadrilateral, and the area computed as + |p X q|/2; see http://mathworld.wolfram.com/Quadrilateral.html. + """ + (up_left, up_right, down_right, down_left) = _get_outside_corners(corners, board) + a = up_right - up_left + b = down_right - up_right + c = down_left - down_right + p = b + c + q = a + b + return abs(p[0]*q[1] - p[1]*q[0]) / 2. + +def _get_corners(img, board, refine = True, checkerboard_flags=0): + """ + Get corners for a particular chessboard for an image + """ + h = img.shape[0] + w = img.shape[1] + if len(img.shape) == 3 and img.shape[2] == 3: + mono = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + else: + mono = img + (ok, corners) = cv2.findChessboardCorners(mono, (board.n_cols, board.n_rows), flags = cv2.CALIB_CB_ADAPTIVE_THRESH | + cv2.CALIB_CB_NORMALIZE_IMAGE | checkerboard_flags) + if not ok: + return (ok, corners) + + # If any corners are within BORDER pixels of the screen edge, reject the detection by setting ok to false + # NOTE: This may cause problems with very low-resolution cameras, where 8 pixels is a non-negligible fraction + # of the image size. See http://answers.ros.org/question/3155/how-can-i-calibrate-low-resolution-cameras + BORDER = 8 + if not all([(BORDER < corners[i, 0, 0] < (w - BORDER)) and (BORDER < corners[i, 0, 1] < (h - BORDER)) for i in range(corners.shape[0])]): + ok = False + + # Ensure that all corner-arrays are going from top to bottom. + if board.n_rows!=board.n_cols: + if corners[0, 0, 1] > corners[-1, 0, 1]: + corners = numpy.copy(numpy.flipud(corners)) + else: + direction_corners=(corners[-1]-corners[0])>=numpy.array([[0.0,0.0]]) + + if not numpy.all(direction_corners): + if not numpy.any(direction_corners): + corners = numpy.copy(numpy.flipud(corners)) + elif direction_corners[0][0]: + corners=numpy.rot90(corners.reshape(board.n_rows,board.n_cols,2)).reshape(board.n_cols*board.n_rows,1,2) + else: + corners=numpy.rot90(corners.reshape(board.n_rows,board.n_cols,2),3).reshape(board.n_cols*board.n_rows,1,2) + + if refine and ok: + # Use a radius of half the minimum distance between corners. This should be large enough to snap to the + # correct corner, but not so large as to include a wrong corner in the search window. + min_distance = float("inf") + for row in range(board.n_rows): + for col in range(board.n_cols - 1): + index = row*board.n_rows + col + min_distance = min(min_distance, _pdist(corners[index, 0], corners[index + 1, 0])) + for row in range(board.n_rows - 1): + for col in range(board.n_cols): + index = row*board.n_rows + col + min_distance = min(min_distance, _pdist(corners[index, 0], corners[index + board.n_cols, 0])) + radius = int(math.ceil(min_distance * 0.5)) + cv2.cornerSubPix(mono, corners, (radius,radius), (-1,-1), + ( cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1 )) + + return (ok, corners) + +def _get_circles(img, board, pattern): + """ + Get circle centers for a symmetric or asymmetric grid + """ + h = img.shape[0] + w = img.shape[1] + if len(img.shape) == 3 and img.shape[2] == 3: + mono = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + else: + mono = img + + flag = cv2.CALIB_CB_SYMMETRIC_GRID + if pattern == Patterns.ACircles: + flag = cv2.CALIB_CB_ASYMMETRIC_GRID + mono_arr = numpy.array(mono) + (ok, corners) = cv2.findCirclesGrid(mono_arr, (board.n_cols, board.n_rows), flags=flag) + + # In symmetric case, findCirclesGrid does not detect the target if it's turned sideways. So we try + # again with dimensions swapped - not so efficient. + # TODO Better to add as second board? Corner ordering will change. + if not ok and pattern == Patterns.Circles: + (ok, corners) = cv2.findCirclesGrid(mono_arr, (board.n_rows, board.n_cols), flags=flag) + + return (ok, corners) + + +# TODO self.size needs to come from CameraInfo, full resolution +class Calibrator(object): + """ + Base class for calibration system + """ + def __init__(self, boards, flags=0, pattern=Patterns.Chessboard, name='', checkerboard_flags=cv2.CALIB_CB_FAST_CHECK): + # Ordering the dimensions for the different detectors is actually a minefield... + if pattern == Patterns.Chessboard: + # Make sure n_cols > n_rows to agree with OpenCV CB detector output + self._boards = [ChessboardInfo(max(i.n_cols, i.n_rows), min(i.n_cols, i.n_rows), i.dim) for i in boards] + elif pattern == Patterns.ACircles: + # 7x4 and 4x7 are actually different patterns. Assume square-ish pattern, so n_rows > n_cols. + self._boards = [ChessboardInfo(min(i.n_cols, i.n_rows), max(i.n_cols, i.n_rows), i.dim) for i in boards] + elif pattern == Patterns.Circles: + # We end up having to check both ways anyway + self._boards = boards + + # Set to true after we perform calibration + self.calibrated = False + self.calib_flags = flags + self.checkerboard_flags = checkerboard_flags + self.pattern = pattern + self.br = cv_bridge.CvBridge() + + # self.db is list of (parameters, image) samples for use in calibration. parameters has form + # (X, Y, size, skew) all normalized to [0,1], to keep track of what sort of samples we've taken + # and ensure enough variety. + self.db = [] + # For each db sample, we also record the detected corners. + self.good_corners = [] + # Set to true when we have sufficiently varied samples to calibrate + self.goodenough = False + self.param_ranges = [0.7, 0.7, 0.4, 0.5] + self.name = name + + def mkgray(self, msg): + """ + Convert a message into a 8-bit 1 channel monochrome OpenCV image + """ + # as cv_bridge automatically scales, we need to remove that behavior + # TODO: get a Python API in cv_bridge to check for the image depth. + if self.br.encoding_to_dtype_with_channels(msg.encoding)[0] in ['uint16', 'int16']: + mono16 = self.br.imgmsg_to_cv2(msg, '16UC1') + mono8 = numpy.array(numpy.clip(mono16, 0, 255), dtype=numpy.uint8) + return mono8 + elif 'FC1' in msg.encoding: + # floating point image handling + img = self.br.imgmsg_to_cv2(msg, "passthrough") + _, max_val, _, _ = cv2.minMaxLoc(img) + if max_val > 0: + scale = 255.0 / max_val + mono_img = (img * scale).astype(numpy.uint8) + else: + mono_img = img.astype(numpy.uint8) + return mono_img + else: + return self.br.imgmsg_to_cv2(msg, "mono8") + + def get_parameters(self, corners, board, size): + """ + Return list of parameters [X, Y, size, skew] describing the checkerboard view. + """ + (width, height) = size + Xs = corners[:,:,0] + Ys = corners[:,:,1] + area = _get_area(corners, board) + border = math.sqrt(area) + # For X and Y, we "shrink" the image all around by approx. half the board size. + # Otherwise large boards are penalized because you can't get much X/Y variation. + p_x = min(1.0, max(0.0, (numpy.mean(Xs) - border / 2) / (width - border))) + p_y = min(1.0, max(0.0, (numpy.mean(Ys) - border / 2) / (height - border))) + p_size = math.sqrt(area / (width * height)) + skew = _get_skew(corners, board) + params = [p_x, p_y, p_size, skew] + return params + + def is_good_sample(self, params): + """ + Returns true if the checkerboard detection described by params should be added to the database. + """ + if not self.db: + return True + + def param_distance(p1, p2): + return sum([abs(a-b) for (a,b) in zip(p1, p2)]) + + db_params = [sample[0] for sample in self.db] + d = min([param_distance(params, p) for p in db_params]) + #print "d = %.3f" % d #DEBUG + # TODO What's a good threshold here? Should it be configurable? + return d > 0.2 + + _param_names = ["X", "Y", "Size", "Skew"] + + def compute_goodenough(self): + if not self.db: + return None + + # Find range of checkerboard poses covered by samples in database + all_params = [sample[0] for sample in self.db] + min_params = all_params[0] + max_params = all_params[0] + for params in all_params[1:]: + min_params = lmin(min_params, params) + max_params = lmax(max_params, params) + # Don't reward small size or skew + min_params = [min_params[0], min_params[1], 0., 0.] + + # For each parameter, judge how much progress has been made toward adequate variation + progress = [min((hi - lo) / r, 1.0) for (lo, hi, r) in zip(min_params, max_params, self.param_ranges)] + # If we have lots of samples, allow calibration even if not all parameters are green + # TODO Awkward that we update self.goodenough instead of returning it + self.goodenough = (len(self.db) >= 40) or all([p == 1.0 for p in progress]) + + return list(zip(self._param_names, min_params, max_params, progress)) + + def mk_object_points(self, boards, use_board_size = False): + opts = [] + for i, b in enumerate(boards): + num_pts = b.n_cols * b.n_rows + opts_loc = numpy.zeros((num_pts, 1, 3), numpy.float32) + for j in range(num_pts): + opts_loc[j, 0, 0] = (j / b.n_cols) + if self.pattern == Patterns.ACircles: + opts_loc[j, 0, 1] = 2*(j % b.n_cols) + (opts_loc[j, 0, 0] % 2) + else: + opts_loc[j, 0, 1] = (j % b.n_cols) + opts_loc[j, 0, 2] = 0 + if use_board_size: + opts_loc[j, 0, :] = opts_loc[j, 0, :] * b.dim + opts.append(opts_loc) + return opts + + def get_corners(self, img, refine = True): + """ + Use cvFindChessboardCorners to find corners of chessboard in image. + + Check all boards. Return corners for first chessboard that it detects + if given multiple size chessboards. + + Returns (ok, corners, board) + """ + + for b in self._boards: + if self.pattern == Patterns.Chessboard: + (ok, corners) = _get_corners(img, b, refine, self.checkerboard_flags) + else: + (ok, corners) = _get_circles(img, b, self.pattern) + if ok: + return (ok, corners, b) + return (False, None, None) + + def downsample_and_detect(self, img): + """ + Downsample the input image to approximately VGA resolution and detect the + calibration target corners in the full-size image. + + Combines these apparently orthogonal duties as an optimization. Checkerboard + detection is too expensive on large images, so it's better to do detection on + the smaller display image and scale the corners back up to the correct size. + + Returns (scrib, corners, downsampled_corners, board, (x_scale, y_scale)). + """ + # Scale the input image down to ~VGA size + height = img.shape[0] + width = img.shape[1] + scale = math.sqrt( (width*height) / (640.*480.) ) + if scale > 1.0: + scrib = cv2.resize(img, (int(width / scale), int(height / scale))) + else: + scrib = img + # Due to rounding, actual horizontal/vertical scaling may differ slightly + x_scale = float(width) / scrib.shape[1] + y_scale = float(height) / scrib.shape[0] + + if self.pattern == Patterns.Chessboard: + # Detect checkerboard + (ok, downsampled_corners, board) = self.get_corners(scrib, refine = True) + + # Scale corners back to full size image + corners = None + if ok: + if scale > 1.0: + # Refine up-scaled corners in the original full-res image + # TODO Does this really make a difference in practice? + corners_unrefined = downsampled_corners.copy() + corners_unrefined[:, :, 0] *= x_scale + corners_unrefined[:, :, 1] *= y_scale + radius = int(math.ceil(scale)) + if len(img.shape) == 3 and img.shape[2] == 3: + mono = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + else: + mono = img + cv2.cornerSubPix(mono, corners_unrefined, (radius,radius), (-1,-1), + ( cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1 )) + corners = corners_unrefined + else: + corners = downsampled_corners + else: + # Circle grid detection is fast even on large images + (ok, corners, board) = self.get_corners(img) + # Scale corners to downsampled image for display + downsampled_corners = None + if ok: + if scale > 1.0: + downsampled_corners = corners.copy() + downsampled_corners[:,:,0] /= x_scale + downsampled_corners[:,:,1] /= y_scale + else: + downsampled_corners = corners + + return (scrib, corners, downsampled_corners, board, (x_scale, y_scale)) + + + def lrmsg(self, d, k, r, p): + """ Used by :meth:`as_message`. Return a CameraInfo message for the given calibration matrices """ + msg = sensor_msgs.msg.CameraInfo() + (msg.width, msg.height) = self.size + if d.size > 5: + msg.distortion_model = "rational_polynomial" + else: + msg.distortion_model = "plumb_bob" + msg.D = numpy.ravel(d).copy().tolist() + msg.K = numpy.ravel(k).copy().tolist() + msg.R = numpy.ravel(r).copy().tolist() + msg.P = numpy.ravel(p).copy().tolist() + return msg + + def lrreport(self, d, k, r, p): + print("D = ", numpy.ravel(d).tolist()) + print("K = ", numpy.ravel(k).tolist()) + print("R = ", numpy.ravel(r).tolist()) + print("P = ", numpy.ravel(p).tolist()) + + def lrost(self, name, d, k, r, p): + calmessage = ( + "# oST version 5.0 parameters\n" + + "\n" + + "\n" + + "[image]\n" + + "\n" + + "width\n" + + str(self.size[0]) + "\n" + + "\n" + + "height\n" + + str(self.size[1]) + "\n" + + "\n" + + "[%s]" % name + "\n" + + "\n" + + "camera matrix\n" + + " ".join(["%8f" % k[0,i] for i in range(3)]) + "\n" + + " ".join(["%8f" % k[1,i] for i in range(3)]) + "\n" + + " ".join(["%8f" % k[2,i] for i in range(3)]) + "\n" + + "\n" + + "distortion\n" + + " ".join(["%8f" % d[i,0] for i in range(d.shape[0])]) + "\n" + + "\n" + + "rectification\n" + + " ".join(["%8f" % r[0,i] for i in range(3)]) + "\n" + + " ".join(["%8f" % r[1,i] for i in range(3)]) + "\n" + + " ".join(["%8f" % r[2,i] for i in range(3)]) + "\n" + + "\n" + + "projection\n" + + " ".join(["%8f" % p[0,i] for i in range(4)]) + "\n" + + " ".join(["%8f" % p[1,i] for i in range(4)]) + "\n" + + " ".join(["%8f" % p[2,i] for i in range(4)]) + "\n" + + "\n") + assert len(calmessage) < 525, "Calibration info must be less than 525 bytes" + return calmessage + + def lryaml(self, name, d, k, r, p): + calmessage = ("" + + "image_width: " + str(self.size[0]) + "\n" + + "image_height: " + str(self.size[1]) + "\n" + + "camera_name: " + name + "\n" + + "camera_matrix:\n" + + " rows: 3\n" + + " cols: 3\n" + + " data: [" + ", ".join(["%8f" % i for i in k.reshape(1,9)[0]]) + "]\n" + + "distortion_model: " + ("rational_polynomial" if d.size > 5 else "plumb_bob") + "\n" + + "distortion_coefficients:\n" + + " rows: 1\n" + + " cols: 4\n" + + " data: [" + ", ".join(["%8f" % d[i,0] for i in range(d.shape[0])]) + "]\n" + + "rectification_matrix:\n" + + " rows: 3\n" + + " cols: 3\n" + + " data: [" + ", ".join(["%8f" % i for i in r.reshape(1,9)[0]]) + "]\n" + + "projection_matrix:\n" + + " rows: 3\n" + + " cols: 4\n" + + " data: [" + ", ".join(["%8f" % i for i in p.reshape(1,12)[0]]) + "]\n" + + "") + return calmessage + + def do_save(self): + filename = '/tmp/calibrationdata.tar.gz' + tf = tarfile.open(filename, 'w:gz') + self.do_tarfile_save(tf) # Must be overridden in subclasses + tf.close() + print(("Wrote calibration data to", filename)) + +def image_from_archive(archive, name): + """ + Load image PGM file from tar archive. + + Used for tarfile loading and unit test. + """ + member = archive.getmember(name) + imagefiledata = numpy.fromstring(archive.extractfile(member).read(), numpy.uint8) + imagefiledata.resize((1, imagefiledata.size)) + return cv2.imdecode(imagefiledata, cv2.IMREAD_COLOR) + +class ImageDrawable(object): + """ + Passed to CalibrationNode after image handled. Allows plotting of images + with detected corner points + """ + def __init__(self): + self.params = None + +class MonoDrawable(ImageDrawable): + def __init__(self): + ImageDrawable.__init__(self) + self.scrib = None + self.linear_error = -1.0 + + +class StereoDrawable(ImageDrawable): + def __init__(self): + ImageDrawable.__init__(self) + self.lscrib = None + self.rscrib = None + self.epierror = -1 + self.dim = -1 + + +class MonoCalibrator(Calibrator): + """ + Calibration class for monocular cameras:: + + images = [cv2.imread("mono%d.png") for i in range(8)] + mc = MonoCalibrator() + mc.cal(images) + print mc.as_message() + """ + + is_mono = True # TODO Could get rid of is_mono + + def __init__(self, *args, **kwargs): + if 'name' not in kwargs: + kwargs['name'] = 'fisheye' + super(MonoCalibrator, self).__init__(*args, **kwargs) + + def cal(self, images): + """ + Calibrate camera from given images + """ + goodcorners = self.collect_corners(images) + self.cal_fromcorners(goodcorners) + self.calibrated = True + + def collect_corners(self, images): + """ + :param images: source images containing chessboards + :type images: list of :class:`cvMat` + + Find chessboards in all images. + + Return [ (corners, ChessboardInfo) ] + """ + self.size = (images[0].shape[1], images[0].shape[0]) + corners = [self.get_corners(i) for i in images] + + goodcorners = [(co, b) for (ok, co, b) in corners if ok] + if not goodcorners: + raise CalibrationException("No corners found in images!") + return goodcorners + + def cal_fromcorners(self, good): + """ + :param good: Good corner positions and boards + :type good: [(corners, ChessboardInfo)] + + + """ + boards = [ b for (_, b) in good ] + + ipts = [ points for (points, _) in good ] + opts = self.mk_object_points(boards) + + self.intrinsics = numpy.zeros((3, 3), numpy.float64) + if self.calib_flags & cv2.CALIB_RATIONAL_MODEL: + self.distortion = numpy.zeros((8, 1), numpy.float64) # rational polynomial + else: + self.distortion = numpy.zeros((4, 1), numpy.float64) + # If FIX_ASPECT_RATIO flag set, enforce focal lengths have 1/1 ratio + self.intrinsics[0,0] = 1.0 + self.intrinsics[1,1] = 1.0 + + calibration_flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC +cv2.fisheye.CALIB_FIX_SKEW + # +cv2.fisheye.CALIB_CHECK_COND + rvecs = [numpy.zeros((1, 1, 3), dtype=numpy.float64) for i in range(len(opts))] + tvecs = [numpy.zeros((1, 1, 3), dtype=numpy.float64) for i in range(len(opts))] + + cv2.fisheye.calibrate( opts, ipts, self.size, self.intrinsics, self.distortion, + rvecs, tvecs, calibration_flags, (cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6)) + + # R is identity matrix for monocular calibration + self.R = numpy.eye(3, dtype=numpy.float64) + self.P = numpy.zeros((3, 4), dtype=numpy.float64) + + self.set_alpha(0.0) + + def set_alpha(self, a): + """ + Set the alpha value for the calibrated camera solution. The alpha + value is a zoom, and ranges from 0 (zoomed in, all pixels in + calibrated image are valid) to 1 (zoomed out, all pixels in + original image are in calibrated image). + """ + + # NOTE: see https://medium.com/@kennethjiang/calibrate-fisheye-lens-using-opencv-part-2-13990f1b157f + ncm = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(self.intrinsics, self.distortion, self.size, numpy.eye(3), balance=a) + for j in range(3): + for i in range(3): + #self.P[j,i] = ncm[j, i] + self.P[j,i] = self.intrinsics[j, i] + + #self.P = self.intrinsics + ##self.mapx, self.mapy = cv2.fisheye.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, ncm, self.size, cv2.CV_32FC1) # Use this to perform cropping + self.mapx, self.mapy = cv2.fisheye.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, self.intrinsics, self.size, cv2.CV_32FC1) ############################# + + def remap(self, src): + """ + :param src: source image + :type src: :class:`cvMat` + + Apply the post-calibration undistortion to the source image + """ + return cv2.remap(src, self.mapx, self.mapy, cv2.INTER_LANCZOS4) + + def undistort_points(self, src): + """ + :param src: N source pixel points (u,v) as an Nx2 matrix + :type src: :class:`cvMat` + + Apply the post-calibration undistortion to the source points + """ + + return cv2.fisheye.undistortPoints(src, self.intrinsics, self.distortion, R = self.R, P = self.P) + + def as_message(self): + """ Return the camera calibration as a CameraInfo message """ + return self.lrmsg(self.distortion, self.intrinsics, self.R, self.P) + + def from_message(self, msg, alpha = 0.0): + """ Initialize the camera calibration from a CameraInfo message """ + + self.size = (msg.width, msg.height) + self.intrinsics = numpy.array(msg.K, dtype=numpy.float64, copy=True).reshape((3, 3)) + self.distortion = numpy.array(msg.D, dtype=numpy.float64, copy=True).reshape((len(msg.D), 1)) + self.R = numpy.array(msg.R, dtype=numpy.float64, copy=True).reshape((3, 3)) + self.P = numpy.array(msg.P, dtype=numpy.float64, copy=True).reshape((3, 4)) + + self.set_alpha(0.0) + + def report(self): + self.lrreport(self.distortion, self.intrinsics, self.R, self.P) + + def ost(self): + return self.lrost(self.name, self.distortion, self.intrinsics, self.R, self.P) + + def yaml(self): + return self.lryaml(self.name, self.distortion, self.intrinsics, self.R, self.P) + + def linear_error_from_image(self, image): + """ + Detect the checkerboard and compute the linear error. + Mainly for use in tests. + """ + _, corners, _, board, _ = self.downsample_and_detect(image) + if corners is None: + return None + + undistorted = self.undistort_points(corners) + return self.linear_error(undistorted, board) + + @staticmethod + def linear_error(corners, b): + + """ + Returns the linear error for a set of corners detected in the unrectified image. + """ + + if corners is None: + return None + + def pt2line(x0, y0, x1, y1, x2, y2): + """ point is (x0, y0), line is (x1, y1, x2, y2) """ + return abs((x2 - x1) * (y1 - y0) - (x1 - x0) * (y2 - y1)) / math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) + + cc = b.n_cols + cr = b.n_rows + errors = [] + for r in range(cr): + (x1, y1) = corners[(cc * r) + 0, 0] + (x2, y2) = corners[(cc * r) + cc - 1, 0] + for i in range(1, cc - 1): + (x0, y0) = corners[(cc * r) + i, 0] + errors.append(pt2line(x0, y0, x1, y1, x2, y2)) + if errors: + return math.sqrt(sum([e**2 for e in errors]) / len(errors)) + else: + return None + + + def handle_msg(self, msg): + """ + Detects the calibration target and, if found and provides enough new information, + adds it to the sample database. + + Returns a MonoDrawable message with the display image and progress info. + """ + gray = self.mkgray(msg) + linear_error = -1 + + # Get display-image-to-be (scrib) and detection of the calibration target + scrib_mono, corners, downsampled_corners, board, (x_scale, y_scale) = self.downsample_and_detect(gray) + + if self.calibrated: + # Show rectified image + # TODO Pull out downsampling code into function + gray_remap = self.remap(gray) + gray_rect = gray_remap + if x_scale != 1.0 or y_scale != 1.0: + gray_rect = cv2.resize(gray_remap, (scrib_mono.shape[1], scrib_mono.shape[0])) + + scrib = cv2.cvtColor(gray_rect, cv2.COLOR_GRAY2BGR) + + if corners is not None: + # Report linear error + undistorted = self.undistort_points(corners) + linear_error = self.linear_error(undistorted, board) + + # Draw rectified corners + scrib_src = undistorted.copy() + scrib_src[:,:,0] /= x_scale + scrib_src[:,:,1] /= y_scale + cv2.drawChessboardCorners(scrib, (board.n_cols, board.n_rows), scrib_src, True) + + else: + scrib = cv2.cvtColor(scrib_mono, cv2.COLOR_GRAY2BGR) + if corners is not None: + # Draw (potentially downsampled) corners onto display image + cv2.drawChessboardCorners(scrib, (board.n_cols, board.n_rows), downsampled_corners, True) + + # Add sample to database only if it's sufficiently different from any previous sample. + params = self.get_parameters(corners, board, (gray.shape[1], gray.shape[0])) + if self.is_good_sample(params): + self.db.append((params, gray)) + self.good_corners.append((corners, board)) + print(("*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" % tuple([len(self.db)] + params))) + + rv = MonoDrawable() + rv.scrib = scrib + rv.params = self.compute_goodenough() + rv.linear_error = linear_error + return rv + + def do_calibration(self, dump = False): + if not self.good_corners: + print("**** Collecting corners for all images! ****") #DEBUG + images = [i for (p, i) in self.db] + self.good_corners = self.collect_corners(images) + # Dump should only occur if user wants it + if dump: + pickle.dump((self.is_mono, self.size, self.good_corners), + open("/tmp/camera_calibration_%08x.pickle" % random.getrandbits(32), "w")) + self.size = (self.db[0][1].shape[1], self.db[0][1].shape[0]) # TODO Needs to be set externally + self.cal_fromcorners(self.good_corners) + self.calibrated = True + # DEBUG + print((self.report())) + print((self.ost())) + + def do_tarfile_save(self, tf): + """ Write images and calibration solution to a tarfile object """ + + def taradd(name, buf): + if isinstance(buf, basestring): + s = StringIO(buf) + else: + s = BytesIO(buf) + ti = tarfile.TarInfo(name) + ti.size = len(s.getvalue()) + ti.uname = 'calibrator' + ti.mtime = int(time.time()) + tf.addfile(tarinfo=ti, fileobj=s) + + ims = [("left-%04d.png" % i, im) for i,(_, im) in enumerate(self.db)] + for (name, im) in ims: + taradd(name, cv2.imencode(".png", im)[1].tostring()) + taradd('ost.yaml', self.yaml()) + taradd('ost.txt', self.ost()) + + def do_tarfile_calibration(self, filename): + archive = tarfile.open(filename, 'r') + + limages = [ image_from_archive(archive, f) for f in archive.getnames() if (f.startswith('left') and (f.endswith('.pgm') or f.endswith('png'))) ] + + self.cal(limages) + +# TODO Replicate MonoCalibrator improvements in stereo +class StereoCalibrator(Calibrator): + """ + Calibration class for stereo cameras:: + + limages = [cv2.imread("left%d.png") for i in range(8)] + rimages = [cv2.imread("right%d.png") for i in range(8)] + sc = StereoCalibrator() + sc.cal(limages, rimages) + print sc.as_message() + """ + + is_mono = False + + def __init__(self, *args, **kwargs): + if 'name' not in kwargs: + kwargs['name'] = 'fisheye' + super(StereoCalibrator, self).__init__(*args, **kwargs) + self.l = MonoCalibrator(*args, **kwargs) + self.r = MonoCalibrator(*args, **kwargs) + # Collecting from two cameras in a horizontal stereo rig, can't get + # full X range in the left camera. + self.param_ranges[0] = 0.4 + + def cal(self, limages, rimages): + """ + :param limages: source left images containing chessboards + :type limages: list of :class:`cvMat` + :param rimages: source right images containing chessboards + :type rimages: list of :class:`cvMat` + + Find chessboards in images, and runs the OpenCV calibration solver. + """ + goodcorners = self.collect_corners(limages, rimages) + self.size = (limages[0].shape[1], limages[0].shape[0]) + self.l.size = self.size + self.r.size = self.size + self.cal_fromcorners(goodcorners) + self.calibrated = True + + def collect_corners(self, limages, rimages): + """ + For a sequence of left and right images, find pairs of images where both + left and right have a chessboard, and return their corners as a list of pairs. + """ + # Pick out (corners, board) tuples + lcorners = [ self.downsample_and_detect(i)[1:4:2] for i in limages] + rcorners = [ self.downsample_and_detect(i)[1:4:2] for i in rimages] + good = [(lco, rco, b) for ((lco, b), (rco, br)) in zip( lcorners, rcorners) + if (lco is not None and rco is not None)] + + if len(good) == 0: + raise CalibrationException("No corners found in images!") + return good + + def cal_fromcorners(self, good): + # Perform monocular calibrations + lcorners = [(l, b) for (l, r, b) in good] + rcorners = [(r, b) for (l, r, b) in good] + self.l.cal_fromcorners(lcorners) # Calibrate each as mono? + self.r.cal_fromcorners(rcorners) + + lipts = [ l for (l, _, _) in good ] + ripts = [ r for (_, r, _) in good ] + boards = [ b for (_, _, b) in good ] + + opts = self.mk_object_points(boards, True) + + flags = cv2.fisheye.CALIB_FIX_INTRINSIC + + # Sanity checks + opts = numpy.array(opts, dtype=numpy.float64) + lipts = numpy.array(lipts, dtype=numpy.float64) + ripts = numpy.array(ripts, dtype=numpy.float64) + + #print("opts.shape ",opts.shape,"lipts.shape " ,lipts.shape,"ripts.shape ",ripts.shape) + + #self.T = numpy.zeros((1,1,3), dtype=numpy.float64) + #self.R = numpy.zeros((1,1,3), dtype=numpy.float64) + + self.T = numpy.zeros((3, 1), dtype=numpy.float64) + self.R = numpy.eye(3, dtype=numpy.float64) + + if LooseVersion(cv2.__version__).version[0] == 2: + # cv2.stereoCalibrate(opts, lipts, ripts, self.size, + # self.l.intrinsics, self.l.distortion, + # self.r.intrinsics, self.r.distortion, + # self.R, # R + # self.T, # T + # criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), + # flags = flags) + print("You need OpenCV >3 to use fisheye camera model") + else: + cv2.fisheye.stereoCalibrate(opts, lipts, ripts, + self.l.intrinsics, self.l.distortion, + self.r.intrinsics, self.r.distortion, + self.size, + self.R, # R + self.T, # T + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), # 30, 1e-6 + flags = flags) + + self.set_alpha(0.0) + + def set_alpha(self, a): + """ + Set the alpha value for the calibrated camera solution. The + alpha value is a zoom, and ranges from 0 (zoomed in, all pixels + in calibrated image are valid) to 1 (zoomed out, all pixels in + original image are in calibrated image). + """ + self.Q = numpy.zeros((4,4), dtype=numpy.float64) + + flags = cv2.CALIB_ZERO_DISPARITY # Operation flags that may be zero or CALIB_ZERO_DISPARITY . + # If the flag is set, the function makes the principal points of each camera have the same pixel coordinates in the rectified views. + # And if the flag is not set, the function may still shift the images in the horizontal or vertical direction + # (depending on the orientation of epipolar lines) to maximize the useful image area. + + cv2.fisheye.stereoRectify(self.l.intrinsics, self.l.distortion, + self.r.intrinsics, self.r.distortion, + self.size, + self.R, self.T, + flags, + self.l.R, self.r.R, + self.l.P, self.r.P, + self.Q, + self.size, + a, + 1.0 ) + # Put the P values of fx,fy,cx and cy to be K's: we keep the Camera matrix, as any rotation will be done by R and the translation (to the new camera frame) will be keept by the last column of P + for j in range(3): + for i in range(3): + self.l.P[j,i] = self.l.intrinsics[j, i] + self.r.P[j,i] = self.r.intrinsics[j, i] + + cv2.fisheye.initUndistortRectifyMap(self.l.intrinsics, self.l.distortion, self.l.R, self.l.intrinsics, self.size, cv2.CV_32FC1, + self.l.mapx, self.l.mapy) + cv2.fisheye.initUndistortRectifyMap(self.r.intrinsics, self.r.distortion, self.r.R, self.r.intrinsics, self.size, cv2.CV_32FC1, + self.r.mapx, self.r.mapy) + + def as_message(self): + """ + Return the camera calibration as a pair of CameraInfo messages, for left + and right cameras respectively. + """ + + return (self.lrmsg(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P), + self.lrmsg(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P)) + + def from_message(self, msgs, alpha = 0.0): + """ Initialize the camera calibration from a pair of CameraInfo messages. """ + self.size = (msgs[0].width, msgs[0].height) + + self.T = numpy.zeros((3, 1), dtype=numpy.float64) + self.R = numpy.eye(3, dtype=numpy.float64) + + self.l.from_message(msgs[0]) + self.r.from_message(msgs[1]) + # Need to compute self.T and self.R here, using the monocular parameters above + if False: + self.set_alpha(0.0) + + def report(self): + print("\nLeft:") + self.lrreport(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P) + print("\nRight:") + self.lrreport(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P) + print("self.T ", numpy.ravel(self.T).tolist()) + print("self.R ", numpy.ravel(self.R).tolist()) + + def ost(self): + return (self.lrost(self.name + "/left", self.l.distortion, self.l.intrinsics, self.l.R, self.l.P) + + self.lrost(self.name + "/right", self.r.distortion, self.r.intrinsics, self.r.R, self.r.P)) + + def yaml(self, suffix, info): + return self.lryaml(self.name + suffix, info.distortion, info.intrinsics, info.R, info.P) + + # TODO Get rid of "from_images" versions of these, instead have function to get undistorted corners + def epipolar_error_from_images(self, limage, rimage): + """ + Detect the checkerboard in both images and compute the epipolar error. + Mainly for use in tests. + """ + lcorners = self.downsample_and_detect(limage)[1] + rcorners = self.downsample_and_detect(rimage)[1] + if lcorners is None or rcorners is None: + return None + + lundistorted = self.l.undistort_points(lcorners) + rundistorted = self.r.undistort_points(rcorners) + + return self.epipolar_error(lundistorted, rundistorted) + + def epipolar_error(self, lcorners, rcorners): + """ + Compute the epipolar error from two sets of matching undistorted points + """ + d = lcorners[:,:,1] - rcorners[:,:,1] + return numpy.sqrt(numpy.square(d).sum() / d.size) + + def chessboard_size_from_images(self, limage, rimage): + _, lcorners, _, board, _ = self.downsample_and_detect(limage) + _, rcorners, _, board, _ = self.downsample_and_detect(rimage) + if lcorners is None or rcorners is None: + return None + + lundistorted = self.l.undistort_points(lcorners) + rundistorted = self.r.undistort_points(rcorners) + + return self.chessboard_size(lundistorted, rundistorted, board) + + def chessboard_size(self, lcorners, rcorners, board, msg = None): + """ + Compute the square edge length from two sets of matching undistorted points + given the current calibration. + :param msg: a tuple of (left_msg, right_msg) + """ + # Project the points to 3d + cam = image_geometry.StereoCameraModel() + if msg == None: + msg = self.as_message() + cam.fromCameraInfo(*msg) + disparities = lcorners[:,:,0] - rcorners[:,:,0] + pt3d = [cam.projectPixelTo3d((lcorners[i,0,0], lcorners[i,0,1]), disparities[i,0]) for i in range(lcorners.shape[0]) ] + def l2(p0, p1): + return math.sqrt(sum([(c0 - c1) ** 2 for (c0, c1) in zip(p0, p1)])) + + # Compute the length from each horizontal and vertical line, and return the mean + cc = board.n_cols + cr = board.n_rows + lengths = ( + [l2(pt3d[cc * r + 0], pt3d[cc * r + (cc - 1)]) / (cc - 1) for r in range(cr)] + + [l2(pt3d[c + 0], pt3d[c + (cc * (cr - 1))]) / (cr - 1) for c in range(cc)]) + return sum(lengths) / len(lengths) + + def handle_msg(self, msg): + # TODO Various asserts that images have same dimension, same board detected... + (lmsg, rmsg) = msg + lgray = self.mkgray(lmsg) + rgray = self.mkgray(rmsg) + epierror = -1 + + # Get display-images-to-be and detections of the calibration target + lscrib_mono, lcorners, ldownsampled_corners, lboard, (x_scale, y_scale) = self.downsample_and_detect(lgray) + rscrib_mono, rcorners, rdownsampled_corners, rboard, _ = self.downsample_and_detect(rgray) + + if self.calibrated: + # Show rectified images + lremap = self.l.remap(lgray) + rremap = self.r.remap(rgray) + lrect = lremap + rrect = rremap + if x_scale != 1.0 or y_scale != 1.0: + lrect = cv2.resize(lremap, (lscrib_mono.shape[1], lscrib_mono.shape[0])) + rrect = cv2.resize(rremap, (rscrib_mono.shape[1], rscrib_mono.shape[0])) + + lscrib = cv2.cvtColor(lrect, cv2.COLOR_GRAY2BGR) + rscrib = cv2.cvtColor(rrect, cv2.COLOR_GRAY2BGR) + + # Draw rectified corners + if lcorners is not None: + lundistorted = self.l.undistort_points(lcorners) + scrib_src = lundistorted.copy() + scrib_src[:,:,0] /= x_scale + scrib_src[:,:,1] /= y_scale + cv2.drawChessboardCorners(lscrib, (lboard.n_cols, lboard.n_rows), scrib_src, True) + + if rcorners is not None: + rundistorted = self.r.undistort_points(rcorners) + scrib_src = rundistorted.copy() + scrib_src[:,:,0] /= x_scale + scrib_src[:,:,1] /= y_scale + cv2.drawChessboardCorners(rscrib, (rboard.n_cols, rboard.n_rows), scrib_src, True) + + # Report epipolar error + if lcorners is not None and rcorners is not None and len(lcorners) == len(rcorners): + epierror = self.epipolar_error(lundistorted, rundistorted) + + else: + lscrib = cv2.cvtColor(lscrib_mono, cv2.COLOR_GRAY2BGR) + rscrib = cv2.cvtColor(rscrib_mono, cv2.COLOR_GRAY2BGR) + # Draw any detected chessboards onto display (downsampled) images + if lcorners is not None: + cv2.drawChessboardCorners(lscrib, (lboard.n_cols, lboard.n_rows), + ldownsampled_corners, True) + if rcorners is not None: + cv2.drawChessboardCorners(rscrib, (rboard.n_cols, rboard.n_rows), + rdownsampled_corners, True) + + # Add sample to database only if it's sufficiently different from any previous sample + if lcorners is not None and rcorners is not None and len(lcorners) == len(rcorners): + params = self.get_parameters(lcorners, lboard, (lgray.shape[1], lgray.shape[0])) + if self.is_good_sample(params): + self.db.append( (params, lgray, rgray) ) + self.good_corners.append( (lcorners, rcorners, lboard) ) + print(("*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" % tuple([len(self.db)] + params))) + + rv = StereoDrawable() + rv.lscrib = lscrib + rv.rscrib = rscrib + rv.params = self.compute_goodenough() + rv.epierror = epierror + return rv + + def do_calibration(self, dump = False): + # TODO MonoCalibrator collects corners if needed here + # Dump should only occur if user wants it + if dump: + pickle.dump((self.is_mono, self.size, self.good_corners), + open("/tmp/camera_calibration_%08x.pickle" % random.getrandbits(32), "w")) + self.size = (self.db[0][1].shape[1], self.db[0][1].shape[0]) # TODO Needs to be set externally + self.l.size = self.size + self.r.size = self.size + self.cal_fromcorners(self.good_corners) + self.calibrated = True + # DEBUG + print((self.report())) + print((self.ost())) + + def do_tarfile_save(self, tf): + """ Write images and calibration solution to a tarfile object """ + ims = ([("left-%04d.png" % i, im) for i,(_, im, _) in enumerate(self.db)] + + [("right-%04d.png" % i, im) for i,(_, _, im) in enumerate(self.db)]) + + def taradd(name, buf): + if isinstance(buf, basestring): + s = StringIO(buf) + else: + s = BytesIO(buf) + ti = tarfile.TarInfo(name) + ti.size = len(s.getvalue()) + ti.uname = 'calibrator' + ti.mtime = int(time.time()) + tf.addfile(tarinfo=ti, fileobj=s) + + for (name, im) in ims: + taradd(name, cv2.imencode(".png", im)[1].tostring()) + taradd('left.yaml', self.yaml("/left", self.l)) + taradd('right.yaml', self.yaml("/right", self.r)) + taradd('ost.txt', self.ost()) + + def do_tarfile_calibration(self, filename): + archive = tarfile.open(filename, 'r') + limages = [ image_from_archive(archive, f) for f in archive.getnames() if (f.startswith('left') and (f.endswith('pgm') or f.endswith('png'))) ] + rimages = [ image_from_archive(archive, f) for f in archive.getnames() if (f.startswith('right') and (f.endswith('pgm') or f.endswith('png'))) ] + + if not len(limages) == len(rimages): + raise CalibrationException("Left, right images don't match. %d left images, %d right" % (len(limages), len(rimages))) + + ##\todo Check that the filenames match and stuff + + self.cal(limages, rimages) diff --git a/camera_calibration_fisheye/src/camera_calibration/camera_calibrator.py b/camera_calibration_fisheye/src/camera_calibration/camera_calibrator.py new file mode 100644 index 000000000..bb9af7c14 --- /dev/null +++ b/camera_calibration_fisheye/src/camera_calibration/camera_calibrator.py @@ -0,0 +1,352 @@ +#!/usr/bin/python +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2009, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import cv2 +import message_filters +import numpy +import os +import rospy +import sensor_msgs.msg +import sensor_msgs.srv +import threading +import time +from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, ChessboardInfo, Patterns +from collections import deque +from message_filters import ApproximateTimeSynchronizer +from std_msgs.msg import String +from std_srvs.srv import Empty + + +class DisplayThread(threading.Thread): + """ + Thread that displays the current images + It is its own thread so that all display can be done + in one thread to overcome imshow limitations and + https://github.com/ros-perception/image_pipeline/issues/85 + """ + def __init__(self, queue, opencv_calibration_node): + threading.Thread.__init__(self) + self.queue = queue + self.opencv_calibration_node = opencv_calibration_node + + def run(self): + cv2.namedWindow("display", cv2.WINDOW_NORMAL) + cv2.setMouseCallback("display", self.opencv_calibration_node.on_mouse) + cv2.createTrackbar("scale", "display", 0, 100, self.opencv_calibration_node.on_scale) + while True: + # wait for an image (could happen at the very beginning when the queue is still empty) + while len(self.queue) == 0: + time.sleep(0.1) + im = self.queue[0] + cv2.imshow("display", im) + k = cv2.waitKey(6) & 0xFF + if k in [27, ord('q')]: + rospy.signal_shutdown('Quit') + elif k == ord('s'): + self.opencv_calibration_node.screendump(im) + +class ConsumerThread(threading.Thread): + def __init__(self, queue, function): + threading.Thread.__init__(self) + self.queue = queue + self.function = function + + def run(self): + while True: + # wait for an image (could happen at the very beginning when the queue is still empty) + while len(self.queue) == 0: + time.sleep(0.1) + self.function(self.queue[0]) + + +class CalibrationNode: + def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0, + pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0): + if service_check: + # assume any non-default service names have been set. Wait for the service to become ready + for svcname in ["camera", "left_camera", "right_camera"]: + remapped = rospy.remap_name(svcname) + if remapped != svcname: + fullservicename = "%s/set_camera_info" % remapped + print("Waiting for service", fullservicename, "...") + try: + rospy.wait_for_service(fullservicename, 5) + print("OK") + except rospy.ROSException: + print("Service not found") + rospy.signal_shutdown('Quit') + + self._boards = boards + self._calib_flags = flags + self._checkerboard_flags = checkerboard_flags + self._pattern = pattern + self._camera_name = camera_name + lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image) + rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image) + ts = synchronizer([lsub, rsub], 4) + ts.registerCallback(self.queue_stereo) + + msub = message_filters.Subscriber('image', sensor_msgs.msg.Image) + msub.registerCallback(self.queue_monocular) + + self.set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), + sensor_msgs.srv.SetCameraInfo) + self.set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), + sensor_msgs.srv.SetCameraInfo) + self.set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), + sensor_msgs.srv.SetCameraInfo) + + self.q_mono = deque([], 1) + self.q_stereo = deque([], 1) + + self.c = None + + mth = ConsumerThread(self.q_mono, self.handle_monocular) + mth.setDaemon(True) + mth.start() + + sth = ConsumerThread(self.q_stereo, self.handle_stereo) + sth.setDaemon(True) + sth.start() + + def redraw_stereo(self, *args): + pass + def redraw_monocular(self, *args): + pass + + def queue_monocular(self, msg): + self.q_mono.append(msg) + + def queue_stereo(self, lmsg, rmsg): + self.q_stereo.append((lmsg, rmsg)) + + def handle_monocular(self, msg): + if self.c == None: + if self._camera_name: + self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name, + checkerboard_flags=self._checkerboard_flags) + else: + self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, + checkerboard_flags=self.checkerboard_flags) + + # This should just call the MonoCalibrator + drawable = self.c.handle_msg(msg) + self.displaywidth = drawable.scrib.shape[1] + self.redraw_monocular(drawable) + + def handle_stereo(self, msg): + if self.c == None: + if self._camera_name: + self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name, + checkerboard_flags=self._checkerboard_flags) + else: + self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, + checkerboard_flags=self._checkerboard_flags) + + drawable = self.c.handle_msg(msg) + self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1] + self.redraw_stereo(drawable) + + + def check_set_camera_info(self, response): + if response.success: + return True + + for i in range(10): + print("!" * 80) + print() + print("Attempt to set camera info failed: " + response.status_message) + print() + for i in range(10): + print("!" * 80) + print() + rospy.logerr('Unable to set camera info for calibration. Failure message: %s' % response.status_message) + return False + + def do_upload(self): + self.c.report() + print(self.c.ost()) + info = self.c.as_message() + + rv = True + if self.c.is_mono: + response = self.set_camera_info_service(info) + rv = self.check_set_camera_info(response) + else: + response = self.set_left_camera_info_service(info[0]) + rv = rv and self.check_set_camera_info(response) + response = self.set_right_camera_info_service(info[1]) + rv = rv and self.check_set_camera_info(response) + return rv + + +class OpenCVCalibrationNode(CalibrationNode): + """ Calibration node with an OpenCV Gui """ + FONT_FACE = cv2.FONT_HERSHEY_SIMPLEX + FONT_SCALE = 0.6 + FONT_THICKNESS = 2 + + def __init__(self, *args, **kwargs): + + CalibrationNode.__init__(self, *args, **kwargs) + + self.queue_display = deque([], 1) + self.display_thread = DisplayThread(self.queue_display, self) + self.display_thread.setDaemon(True) + self.display_thread.start() + + @classmethod + def putText(cls, img, text, org, color = (0,0,0)): + cv2.putText(img, text, org, cls.FONT_FACE, cls.FONT_SCALE, color, thickness = cls.FONT_THICKNESS) + + @classmethod + def getTextSize(cls, text): + return cv2.getTextSize(text, cls.FONT_FACE, cls.FONT_SCALE, cls.FONT_THICKNESS)[0] + + def on_mouse(self, event, x, y, flags, param): + if event == cv2.EVENT_LBUTTONDOWN and self.displaywidth < x: + if self.c.goodenough: + if 180 <= y < 280: + self.c.do_calibration() + if self.c.calibrated: + if 280 <= y < 380: + self.c.do_save() + elif 380 <= y < 480: + # Only shut down if we set camera info correctly, #3993 + if self.do_upload(): + rospy.signal_shutdown('Quit') + + def on_scale(self, scalevalue): + if self.c.calibrated: + self.c.set_alpha(scalevalue / 100.0) + + def button(self, dst, label, enable): + dst.fill(255) + size = (dst.shape[1], dst.shape[0]) + if enable: + color = (155, 155, 80) + else: + color = (224, 224, 224) + cv2.circle(dst, (size[0] // 2, size[1] // 2), min(size) // 2, color, -1) + (w, h) = self.getTextSize(label) + self.putText(dst, label, ((size[0] - w) // 2, (size[1] + h) // 2), (255,255,255)) + + def buttons(self, display): + x = self.displaywidth + self.button(display[180:280,x:x+100], "CALIBRATE", self.c.goodenough) + self.button(display[280:380,x:x+100], "SAVE", self.c.calibrated) + self.button(display[380:480,x:x+100], "COMMIT", self.c.calibrated) + + def y(self, i): + """Set up right-size images""" + return 30 + 40 * i + + def screendump(self, im): + i = 0 + while os.access("/tmp/dump%d.png" % i, os.R_OK): + i += 1 + cv2.imwrite("/tmp/dump%d.png" % i, im) + + def redraw_monocular(self, drawable): + height = drawable.scrib.shape[0] + width = drawable.scrib.shape[1] + + display = numpy.zeros((max(480, height), width + 100, 3), dtype=numpy.uint8) + display[0:height, 0:width,:] = drawable.scrib + display[0:height, width:width+100,:].fill(255) + + + self.buttons(display) + if not self.c.calibrated: + if drawable.params: + for i, (label, lo, hi, progress) in enumerate(drawable.params): + (w,_) = self.getTextSize(label) + self.putText(display, label, (width + (100 - w) // 2, self.y(i))) + color = (0,255,0) + if progress < 1.0: + color = (0, int(progress*255.), 255) + cv2.line(display, + (int(width + lo * 100), self.y(i) + 20), + (int(width + hi * 100), self.y(i) + 20), + color, 4) + + else: + self.putText(display, "lin.", (width, self.y(0))) + linerror = drawable.linear_error + if linerror < 0: + msg = "?" + else: + msg = "%.2f" % linerror + #print "linear", linerror + self.putText(display, msg, (width, self.y(1))) + + self.queue_display.append(display) + + def redraw_stereo(self, drawable): + height = drawable.lscrib.shape[0] + width = drawable.lscrib.shape[1] + + display = numpy.zeros((max(480, height), 2 * width + 100, 3), dtype=numpy.uint8) + display[0:height, 0:width,:] = drawable.lscrib + display[0:height, width:2*width,:] = drawable.rscrib + display[0:height, 2*width:2*width+100,:].fill(255) + + self.buttons(display) + + if not self.c.calibrated: + if drawable.params: + for i, (label, lo, hi, progress) in enumerate(drawable.params): + (w,_) = self.getTextSize(label) + self.putText(display, label, (2 * width + (100 - w) // 2, self.y(i))) + color = (0,255,0) + if progress < 1.0: + color = (0, int(progress*255.), 255) + cv2.line(display, + (int(2 * width + lo * 100), self.y(i) + 20), + (int(2 * width + hi * 100), self.y(i) + 20), + color, 4) + + else: + self.putText(display, "epi.", (2 * width, self.y(0))) + if drawable.epierror == -1: + msg = "?" + else: + msg = "%.2f" % drawable.epierror + self.putText(display, msg, (2 * width, self.y(1))) + # TODO dim is never set anywhere. Supposed to be observed chessboard size? + if drawable.dim != -1: + self.putText(display, "dim", (2 * width, self.y(2))) + self.putText(display, "%.3f" % drawable.dim, (2 * width, self.y(3))) + + self.queue_display.append(display) diff --git a/camera_calibration_fisheye/src/camera_calibration/camera_checker.py b/camera_calibration_fisheye/src/camera_calibration/camera_checker.py new file mode 100644 index 000000000..d27b893e0 --- /dev/null +++ b/camera_calibration_fisheye/src/camera_calibration/camera_checker.py @@ -0,0 +1,200 @@ +#!/usr/bin/python +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2009, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import cv2 +import cv_bridge +import functools +import message_filters +import numpy +import rospy +import sensor_msgs.msg +import sensor_msgs.srv +import threading + +from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, ChessboardInfo +from message_filters import ApproximateTimeSynchronizer + +try: + from queue import Queue +except ImportError: + from Queue import Queue + + +def mean(seq): + return sum(seq) / len(seq) + +def lmin(seq1, seq2): + """ Pairwise minimum of two sequences """ + return [min(a, b) for (a, b) in zip(seq1, seq2)] + +def lmax(seq1, seq2): + """ Pairwise maximum of two sequences """ + return [max(a, b) for (a, b) in zip(seq1, seq2)] + +class ConsumerThread(threading.Thread): + def __init__(self, queue, function): + threading.Thread.__init__(self) + self.queue = queue + self.function = function + + def run(self): + while not rospy.is_shutdown(): + while not rospy.is_shutdown(): + m = self.queue.get() + if self.queue.empty(): + break + self.function(m) + +class CameraCheckerNode: + + def __init__(self, chess_size, dim, approximate=0): + self.board = ChessboardInfo() + self.board.n_cols = chess_size[0] + self.board.n_rows = chess_size[1] + self.board.dim = dim + + # make sure n_cols is not smaller than n_rows, otherwise error computation will be off + if self.board.n_cols < self.board.n_rows: + self.board.n_cols, self.board.n_rows = self.board.n_rows, self.board.n_cols + + image_topic = rospy.resolve_name("monocular") + "/image_rect" + camera_topic = rospy.resolve_name("monocular") + "/camera_info" + + tosync_mono = [ + (image_topic, sensor_msgs.msg.Image), + (camera_topic, sensor_msgs.msg.CameraInfo), + ] + + if approximate <= 0: + sync = message_filters.TimeSynchronizer + else: + sync = functools.partial(ApproximateTimeSynchronizer, slop=approximate) + + tsm = sync([message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10) + tsm.registerCallback(self.queue_monocular) + + left_topic = rospy.resolve_name("stereo") + "/left/image_rect" + left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info" + right_topic = rospy.resolve_name("stereo") + "/right/image_rect" + right_camera_topic = rospy.resolve_name("stereo") + "/right/camera_info" + + tosync_stereo = [ + (left_topic, sensor_msgs.msg.Image), + (left_camera_topic, sensor_msgs.msg.CameraInfo), + (right_topic, sensor_msgs.msg.Image), + (right_camera_topic, sensor_msgs.msg.CameraInfo) + ] + + tss = sync([message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo], 10) + tss.registerCallback(self.queue_stereo) + + self.br = cv_bridge.CvBridge() + + self.q_mono = Queue() + self.q_stereo = Queue() + + mth = ConsumerThread(self.q_mono, self.handle_monocular) + mth.setDaemon(True) + mth.start() + + sth = ConsumerThread(self.q_stereo, self.handle_stereo) + sth.setDaemon(True) + sth.start() + + self.mc = MonoCalibrator([self.board]) + self.sc = StereoCalibrator([self.board]) + + def queue_monocular(self, msg, cmsg): + self.q_mono.put((msg, cmsg)) + + def queue_stereo(self, lmsg, lcmsg, rmsg, rcmsg): + self.q_stereo.put((lmsg, lcmsg, rmsg, rcmsg)) + + def mkgray(self, msg): + return self.mc.mkgray(msg) + + def image_corners(self, im): + (ok, corners, b) = self.mc.get_corners(im) + if ok: + return corners + else: + return None + + def handle_monocular(self, msg): + + (image, camera) = msg + gray = self.mkgray(image) + C = self.image_corners(gray) + if C is not None: + linearity_rms = self.mc.linear_error(C, self.board) + + # Add in reprojection check + image_points = C + object_points = self.mc.mk_object_points([self.board], use_board_size=True)[0] + dist_coeffs = numpy.zeros((4, 1)) + camera_matrix = numpy.array( [ [ camera.P[0], camera.P[1], camera.P[2] ], + [ camera.P[4], camera.P[5], camera.P[6] ], + [ camera.P[8], camera.P[9], camera.P[10] ] ] ) + ok, rot, trans = cv2.solvePnP(object_points, image_points, camera_matrix, dist_coeffs) + # Convert rotation into a 3x3 Rotation Matrix + rot3x3, _ = cv2.Rodrigues(rot) + # Reproject model points into image + object_points_world = numpy.asmatrix(rot3x3) * numpy.asmatrix(object_points.squeeze().T) + numpy.asmatrix(trans) + reprojected_h = camera_matrix * object_points_world + reprojected = (reprojected_h[0:2, :] / reprojected_h[2, :]) + reprojection_errors = image_points.squeeze().T - reprojected + + reprojection_rms = numpy.sqrt(numpy.sum(numpy.array(reprojection_errors) ** 2) / numpy.product(reprojection_errors.shape)) + + # Print the results + print("Linearity RMS Error: %.3f Pixels Reprojection RMS Error: %.3f Pixels" % (linearity_rms, reprojection_rms)) + else: + print('no chessboard') + + def handle_stereo(self, msg): + + (lmsg, lcmsg, rmsg, rcmsg) = msg + lgray = self.mkgray(lmsg) + rgray = self.mkgray(rmsg) + + L = self.image_corners(lgray) + R = self.image_corners(rgray) + if L is not None and R is not None: + epipolar = self.sc.epipolar_error(L, R) + + dimension = self.sc.chessboard_size(L, R, self.board, msg=(lcmsg, rcmsg)) + + print("epipolar error: %f pixels dimension: %f m" % (epipolar, dimension)) + else: + print("no chessboard") diff --git a/camera_calibration_fisheye/test/directed.py b/camera_calibration_fisheye/test/directed.py new file mode 100644 index 000000000..43a7aaa4e --- /dev/null +++ b/camera_calibration_fisheye/test/directed.py @@ -0,0 +1,274 @@ +#!/usr/bin/env python +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2009, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import roslib +import rosunit +import rospy +import cv2 + +import collections +import copy +import numpy +import os +import sys +import tarfile +import unittest + +from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, \ + Patterns, CalibrationException, ChessboardInfo, image_from_archive + +board = ChessboardInfo() +board.n_cols = 8 +board.n_rows = 6 +board.dim = 0.108 + +class TestDirected(unittest.TestCase): + def setUp(self): + tar_path = roslib.packages.find_resource('camera_calibration', 'camera_calibration.tar.gz')[0] + self.tar = tarfile.open(tar_path, 'r') + self.limages = [image_from_archive(self.tar, "wide/left%04d.pgm" % i) for i in range(3, 15)] + self.rimages = [image_from_archive(self.tar, "wide/right%04d.pgm" % i) for i in range(3, 15)] + self.l = {} + self.r = {} + self.sizes = [(320,240), (640,480), (800,600), (1024,768)] + for dim in self.sizes: + self.l[dim] = [] + self.r[dim] = [] + for li,ri in zip(self.limages, self.rimages): + rli = cv2.resize(li, (dim[0], dim[1])) + rri = cv2.resize(ri, (dim[0], dim[1])) + self.l[dim].append(rli) + self.r[dim].append(rri) + + def assert_good_mono(self, c, dim, max_err): + #c.report() + self.assert_(len(c.ost()) > 0) + lin_err = 0 + n = 0 + for img in self.l[dim]: + lin_err_local = c.linear_error_from_image(img) + if lin_err_local: + lin_err += lin_err_local + n += 1 + if n > 0: + lin_err /= n + self.assert_(0.0 < lin_err, 'lin_err is %f' % lin_err) + self.assert_(lin_err < max_err, 'lin_err is %f' % lin_err) + + flat = c.remap(img) + self.assertEqual(img.shape, flat.shape) + + def test_monocular(self): + # Run the calibrator, produce a calibration, check it + mc = MonoCalibrator([ board ], cv2.CALIB_FIX_K3) + max_errs = [0.1, 0.2, 0.4, 0.7] + for i, dim in enumerate(self.sizes): + mc.cal(self.l[dim]) + self.assert_good_mono(mc, dim, max_errs[i]) + + # Make another calibration, import previous calibration as a message, + # and assert that the new one is good. + + mc2 = MonoCalibrator([board]) + mc2.from_message(mc.as_message()) + self.assert_good_mono(mc2, dim, max_errs[i]) + + def test_stereo(self): + epierrors = [0.1, 0.2, 0.45, 1.0] + for i, dim in enumerate(self.sizes): + print("Dim =", dim) + sc = StereoCalibrator([board], cv2.CALIB_FIX_K3) + sc.cal(self.l[dim], self.r[dim]) + + sc.report() + #print sc.ost() + + # NOTE: epipolar error currently increases with resolution. + # At highest res expect error ~0.75 + epierror = 0 + n = 0 + for l_img, r_img in zip(self.l[dim], self.r[dim]): + epierror_local = sc.epipolar_error_from_images(l_img, r_img) + if epierror_local: + epierror += epierror_local + n += 1 + epierror /= n + self.assert_(epierror < epierrors[i], + 'Epipolar error is %f for resolution i = %d' % (epierror, i)) + + self.assertAlmostEqual(sc.chessboard_size_from_images(self.l[dim][0], self.r[dim][0]), .108, 2) + + #print sc.as_message() + + img = self.l[dim][0] + flat = sc.l.remap(img) + self.assertEqual(img.shape, flat.shape) + flat = sc.r.remap(img) + self.assertEqual(img.shape, flat.shape) + + sc2 = StereoCalibrator([board]) + sc2.from_message(sc.as_message()) + # sc2.set_alpha(1.0) + #sc2.report() + self.assert_(len(sc2.ost()) > 0) + + def test_nochecker(self): + # Run with same images, but looking for an incorrect chessboard size (8, 7). + # Should raise an exception because of lack of input points. + new_board = copy.deepcopy(board) + new_board.n_cols = 8 + new_board.n_rows = 7 + + sc = StereoCalibrator([new_board]) + self.assertRaises(CalibrationException, lambda: sc.cal(self.limages, self.rimages)) + mc = MonoCalibrator([new_board]) + self.assertRaises(CalibrationException, lambda: mc.cal(self.limages)) + + + +class TestArtificial(unittest.TestCase): + Setup = collections.namedtuple('Setup', ['pattern', 'cols', 'rows', 'lin_err', 'K_err']) + + def setUp(self): + # Define some image transforms that will simulate a camera position + M = [] + cv2.getPerspectiveTransform + self.K = numpy.array([[500, 0, 250], [0, 500, 250], [0, 0, 1]], numpy.float32) + self.D = numpy.array([]) + # physical size of the board + self.board_width_dim = 1 + + # Generate data for different grid types. For each grid type, define the different sizes of + # grid that are recognized (n row, n col) + # Patterns.Circles, Patterns.ACircles + self.setups = [ self.Setup(pattern=Patterns.Chessboard, cols=7, rows=8, lin_err=0.2, K_err=8.2), + self.Setup(pattern=Patterns.Circles, cols=7, rows=8, lin_err=0.1, K_err=4), + self.Setup(pattern=Patterns.ACircles, cols=3, rows=5, lin_err=0.1, K_err=8) ] + self.limages = [] + self.rimages = [] + for setup in self.setups: + self.limages.append([]) + self.rimages.append([]) + + # Create the pattern + if setup.pattern == Patterns.Chessboard: + pattern = numpy.zeros((50*(setup.rows+3), 50*(setup.cols+3), 1), numpy.uint8) + pattern.fill(255) + for j in range(1, setup.rows+2): + for i in range(1+(j%2), setup.cols+2, 2): + pattern[50*j:50*(j+1), 50*i:50*(i+1)].fill(0) + elif setup.pattern == Patterns.Circles: + pattern = numpy.zeros((50*(setup.rows+2), 50*(setup.cols+2), 1), numpy.uint8) + pattern.fill(255) + for j in range(1, setup.rows+1): + for i in range(1, setup.cols+1): + cv2.circle(pattern, (50*i + 25, 50*j + 25), 15, (0,0,0), -1 ) + elif setup.pattern == Patterns.ACircles: + x = 60 + pattern = numpy.zeros((x*(setup.rows+2), x*(setup.cols+5), 1), numpy.uint8) + pattern.fill(255) + for j in range(1, setup.rows+1): + for i in range(0, setup.cols): + cv2.circle(pattern, (x*(1 + 2*i + (j%2)) + x/2, x*j + x/2), x/3, (0,0,0), -1) + + rows, cols, _ = pattern.shape + object_points_2d = numpy.array([[0, 0], [0, cols-1], [rows-1, cols-1], [rows-1, 0]], numpy.float32) + object_points_3d = numpy.array([[0, 0, 0], [0, cols-1, 0], [rows-1, cols-1, 0], [rows-1, 0, 0]], numpy.float32) + object_points_3d *= self.board_width_dim/float(cols) + + # create the artificial view points + rvec = [ [0, 0, 0], [0, 0, 0.4], [0, 0.4, 0], [0.4, 0, 0], [0.4, 0.4, 0], [0.4, 0, 0.4], [0, 0.4, 0.4], [0.4, 0.4, 0.4] ] + tvec = [ [-0.5, -0.5, 3], [-0.5, -0.5, 3], [-0.5, -0.1, 3], [-0.1, -0.5, 3], [-0.1, -0.1, 3], [-0.1, -0.5, 3], [-0.5, -0.1, 3], [-0.1, 0.1, 3] ] + + dsize = (480, 640) + for i in range(len(rvec)): + R = numpy.array(rvec[i], numpy.float32) + T = numpy.array(tvec[i], numpy.float32) + + image_points, _ = cv2.projectPoints(object_points_3d, R, T, self.K, self.D) + + # deduce the perspective transform + M.append(cv2.getPerspectiveTransform(object_points_2d, image_points)) + + # project the pattern according to the different cameras + pattern_warped = cv2.warpPerspective(pattern, M[i], dsize) + self.limages[-1].append(pattern_warped) + + def assert_good_mono(self, c, images, max_err): + #c.report() + self.assert_(len(c.ost()) > 0) + lin_err = 0 + n = 0 + for img in images: + lin_err_local = c.linear_error_from_image(img) + if lin_err_local: + lin_err += lin_err_local + n += 1 + if n > 0: + lin_err /= n + print("linear error is %f" % lin_err) + self.assert_(0.0 < lin_err, 'lin_err is %f' % lin_err) + self.assert_(lin_err < max_err, 'lin_err is %f' % lin_err) + + flat = c.remap(img) + self.assertEqual(img.shape, flat.shape) + + def test_monocular(self): + # Run the calibrator, produce a calibration, check it + for i, setup in enumerate(self.setups): + board = ChessboardInfo() + board.n_cols = setup.cols + board.n_rows = setup.rows + board.dim = self.board_width_dim + + mc = MonoCalibrator([ board ], flags=cv2.CALIB_FIX_K3, pattern=setup.pattern) + + if 0: + # display the patterns viewed by the camera + for pattern_warped in self.limages[i]: + cv2.imshow("toto", pattern_warped) + cv2.waitKey(0) + + mc.cal(self.limages[i]) + self.assert_good_mono(mc, self.limages[i], setup.lin_err) + + # Make sure the intrinsics are similar + err_intrinsics = numpy.linalg.norm(mc.intrinsics - self.K, ord=numpy.inf) + self.assert_(err_intrinsics < setup.K_err, + 'intrinsics error is %f for resolution i = %d' % (err_intrinsics, i)) + print('intrinsics error is %f' % numpy.linalg.norm(mc.intrinsics - self.K, ord=numpy.inf)) + +if __name__ == '__main__': + #rosunit.unitrun('camera_calibration', 'directed', TestDirected) + rosunit.unitrun('camera_calibration', 'artificial', TestArtificial) diff --git a/camera_calibration_fisheye/test/multiple_boards.py b/camera_calibration_fisheye/test/multiple_boards.py new file mode 100644 index 000000000..a726a703a --- /dev/null +++ b/camera_calibration_fisheye/test/multiple_boards.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2009, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import roslib +import rostest +import rospy +import unittest +import tarfile +import copy +import os, sys + +from camera_calibration.calibrator import StereoCalibrator, ChessboardInfo, image_from_archive + +# Large board used for PR2 calibration +board = ChessboardInfo() +board.n_cols = 7 +board.n_rows = 6 +board.dim = 0.108 + +class TestMultipleBoards(unittest.TestCase): + def test_multiple_boards(self): + small_board = ChessboardInfo() + small_board.n_cols = 5 + small_board.n_rows = 4 + small_board.dim = 0.025 + + stereo_cal = StereoCalibrator([board, small_board]) + + my_archive_name = roslib.packages.find_resource('camera_calibration', 'multi_board_calibration.tar.gz')[0] + stereo_cal.do_tarfile_calibration(my_archive_name) + + stereo_cal.report() + stereo_cal.ost() + + # Check error for big image + archive = tarfile.open(my_archive_name) + l1_big = image_from_archive(archive, "left-0000.png") + r1_big = image_from_archive(archive, "right-0000.png") + epi_big = stereo_cal.epipolar_error_from_images(l1_big, r1_big) + self.assert_(epi_big < 1.0, "Epipolar error for large checkerboard > 1.0. Error: %.2f" % epi_big) + + # Small checkerboard has larger error threshold for now + l1_sm = image_from_archive(archive, "left-0012-sm.png") + r1_sm = image_from_archive(archive, "right-0012-sm.png") + epi_sm = stereo_cal.epipolar_error_from_images(l1_sm, r1_sm) + self.assert_(epi_sm < 2.0, "Epipolar error for small checkerboard > 2.0. Error: %.2f" % epi_sm) + + + +if __name__ == '__main__': + if 1: + rostest.unitrun('camera_calibration', 'test_multi_board_cal', TestMultipleBoards) + else: + suite = unittest.TestSuite() + suite.addTest(TestMultipleBoards('test_multi_board_cal')) + unittest.TextTestRunner(verbosity=2).run(suite) diff --git a/stereo_image_proc/cfg/Disparity.cfg b/stereo_image_proc/cfg/Disparity.cfg deleted file mode 100755 index 4c502e747..000000000 --- a/stereo_image_proc/cfg/Disparity.cfg +++ /dev/null @@ -1,39 +0,0 @@ -#! /usr/bin/env python - -# Declare parameters that control stereo processing - -PACKAGE='stereo_image_proc' - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -stereo_algo_enum = gen.enum([gen.const("StereoBM", int_t, 0, "Block Matching"), - gen.const("StereoSGBM", int_t, 1, "SemiGlobal Block Matching")], - "stereo algorithm") -gen.add("stereo_algorithm", int_t, 0, "sterel algorithm", 0, 0, 1, - edit_method = stereo_algo_enum) -# disparity block matching pre-filtering parameters -gen.add("prefilter_size", int_t, 0, "Normalization window size, pixels", 9, 5, 255) -gen.add("prefilter_cap", int_t, 0, "Bound on normalized pixel values", 31, 1, 63) - -# disparity block matching correlation parameters -gen.add("correlation_window_size", int_t, 0, "SAD correlation window width, pixels", 15, 5, 255) -gen.add("min_disparity", int_t, 0, "Disparity to begin search at, pixels (may be negative)", 0, -128, 128) -gen.add("disparity_range", int_t, 0, "Number of disparities to search, pixels", 64, 32, 256) -# TODO What about trySmallerWindows? - -# disparity block matching post-filtering parameters -# NOTE: Making uniqueness_ratio int_t instead of double_t to work around dynamic_reconfigure gui issue -gen.add("uniqueness_ratio", double_t, 0, "Filter out if best match does not sufficiently exceed the next-best match", 15, 0, 100) -gen.add("texture_threshold", int_t, 0, "Filter out if SAD window response does not exceed texture threshold", 10, 0, 10000) -gen.add("speckle_size", int_t, 0, "Reject regions smaller than this size, pixels", 100, 0, 1000) -gen.add("speckle_range", int_t, 0, "Max allowed difference between detected disparities", 4, 0, 31) -gen.add("fullDP", bool_t, 0, "Run the full variant of the algorithm, only available in SGBM", False) -gen.add("P1", double_t, 0, "The first parameter controlling the disparity smoothness, only available in SGBM", 200, 0, 4000) -gen.add("P2", double_t, 0, "The second parameter controlling the disparity smoothness., only available in SGBM", 400, 0, 4000) -gen.add("disp12MaxDiff", int_t, 0, "Maximum allowed difference (in integer pixel units) in the left-right disparity check, only available in SGBM", 0, 0, 128) -# First string value is node name, used only for generating documentation -# Second string value ("Disparity") is name of class and generated -# .h file, with "Config" added, so class DisparityConfig -exit(gen.generate(PACKAGE, "stereo_image_proc", "Disparity")) From 9f0772e0a5d49b38832f5ef8f297e8e15bffd379 Mon Sep 17 00:00:00 2001 From: DavidTorresOcana Date: Mon, 28 Oct 2019 00:58:07 +0100 Subject: [PATCH 02/10] Add calibration for fisheye cameras Fix #146 --- camera_calibration/CHANGELOG.rst | 0 camera_calibration/CMakeLists.txt | 0 camera_calibration/button.jpg | Bin camera_calibration/doc/conf.py | 0 camera_calibration/doc/index.rst | 0 camera_calibration/mainpage.dox | 0 camera_calibration/package.xml | 0 camera_calibration/rosdoc.yaml | 0 camera_calibration/setup.py | 0 .../src/camera_calibration/__init__.py | 0 .../src/camera_calibration/calibrator.py | 206 ++- .../camera_calibration/camera_calibrator.py | 4 + camera_calibration/test/directed.py | 0 camera_calibration/test/multiple_boards.py | 0 camera_calibration_fisheye/CHANGELOG.rst | 170 --- camera_calibration_fisheye/CMakeLists.txt | 31 - camera_calibration_fisheye/README.md | 55 - camera_calibration_fisheye/button.jpg | Bin 21110 -> 0 bytes camera_calibration_fisheye/doc/conf.py | 201 --- camera_calibration_fisheye/doc/index.rst | 18 - .../fiseye_cal_reference/mono/calibrate.py | 95 -- .../mono/test_undistort.py | 42 - .../mono/test_undistorted_full.py | 54 - .../stereo/CMakeLists.txt | 12 - .../fiseye_cal_reference/stereo/README.md | 38 - .../fiseye_cal_reference/stereo/calibrate.cpp | 142 -- .../fiseye_cal_reference/stereo/popt_pp.h | 39 - camera_calibration_fisheye/mainpage.dox | 59 - .../nodes/cameracalibrator.py | 154 --- .../nodes/cameracheck.py | 57 - camera_calibration_fisheye/package.xml | 31 - camera_calibration_fisheye/rosdoc.yaml | 4 - .../scripts/tarfile_calibration.py | 235 ---- camera_calibration_fisheye/setup.py | 9 - .../src/camera_calibration/__init__.py | 0 .../src/camera_calibration/calibrator.py | 1182 ----------------- .../camera_calibration/camera_calibrator.py | 352 ----- .../src/camera_calibration/camera_checker.py | 200 --- camera_calibration_fisheye/test/directed.py | 274 ---- .../test/multiple_boards.py | 87 -- 40 files changed, 147 insertions(+), 3604 deletions(-) mode change 100644 => 100755 camera_calibration/CHANGELOG.rst mode change 100644 => 100755 camera_calibration/CMakeLists.txt mode change 100644 => 100755 camera_calibration/button.jpg mode change 100644 => 100755 camera_calibration/doc/conf.py mode change 100644 => 100755 camera_calibration/doc/index.rst mode change 100644 => 100755 camera_calibration/mainpage.dox mode change 100644 => 100755 camera_calibration/package.xml mode change 100644 => 100755 camera_calibration/rosdoc.yaml mode change 100644 => 100755 camera_calibration/setup.py mode change 100644 => 100755 camera_calibration/src/camera_calibration/__init__.py mode change 100644 => 100755 camera_calibration/src/camera_calibration/calibrator.py mode change 100644 => 100755 camera_calibration/test/directed.py mode change 100644 => 100755 camera_calibration/test/multiple_boards.py delete mode 100644 camera_calibration_fisheye/CHANGELOG.rst delete mode 100644 camera_calibration_fisheye/CMakeLists.txt delete mode 100644 camera_calibration_fisheye/README.md delete mode 100644 camera_calibration_fisheye/button.jpg delete mode 100644 camera_calibration_fisheye/doc/conf.py delete mode 100644 camera_calibration_fisheye/doc/index.rst delete mode 100644 camera_calibration_fisheye/fiseye_cal_reference/mono/calibrate.py delete mode 100644 camera_calibration_fisheye/fiseye_cal_reference/mono/test_undistort.py delete mode 100644 camera_calibration_fisheye/fiseye_cal_reference/mono/test_undistorted_full.py delete mode 100644 camera_calibration_fisheye/fiseye_cal_reference/stereo/CMakeLists.txt delete mode 100644 camera_calibration_fisheye/fiseye_cal_reference/stereo/README.md delete mode 100644 camera_calibration_fisheye/fiseye_cal_reference/stereo/calibrate.cpp delete mode 100644 camera_calibration_fisheye/fiseye_cal_reference/stereo/popt_pp.h delete mode 100644 camera_calibration_fisheye/mainpage.dox delete mode 100644 camera_calibration_fisheye/nodes/cameracalibrator.py delete mode 100644 camera_calibration_fisheye/nodes/cameracheck.py delete mode 100644 camera_calibration_fisheye/package.xml delete mode 100644 camera_calibration_fisheye/rosdoc.yaml delete mode 100644 camera_calibration_fisheye/scripts/tarfile_calibration.py delete mode 100644 camera_calibration_fisheye/setup.py delete mode 100644 camera_calibration_fisheye/src/camera_calibration/__init__.py delete mode 100644 camera_calibration_fisheye/src/camera_calibration/calibrator.py delete mode 100644 camera_calibration_fisheye/src/camera_calibration/camera_calibrator.py delete mode 100644 camera_calibration_fisheye/src/camera_calibration/camera_checker.py delete mode 100644 camera_calibration_fisheye/test/directed.py delete mode 100644 camera_calibration_fisheye/test/multiple_boards.py diff --git a/camera_calibration/CHANGELOG.rst b/camera_calibration/CHANGELOG.rst old mode 100644 new mode 100755 diff --git a/camera_calibration/CMakeLists.txt b/camera_calibration/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/camera_calibration/button.jpg b/camera_calibration/button.jpg old mode 100644 new mode 100755 diff --git a/camera_calibration/doc/conf.py b/camera_calibration/doc/conf.py old mode 100644 new mode 100755 diff --git a/camera_calibration/doc/index.rst b/camera_calibration/doc/index.rst old mode 100644 new mode 100755 diff --git a/camera_calibration/mainpage.dox b/camera_calibration/mainpage.dox old mode 100644 new mode 100755 diff --git a/camera_calibration/package.xml b/camera_calibration/package.xml old mode 100644 new mode 100755 diff --git a/camera_calibration/rosdoc.yaml b/camera_calibration/rosdoc.yaml old mode 100644 new mode 100755 diff --git a/camera_calibration/setup.py b/camera_calibration/setup.py old mode 100644 new mode 100755 diff --git a/camera_calibration/src/camera_calibration/__init__.py b/camera_calibration/src/camera_calibration/__init__.py old mode 100644 new mode 100755 diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py old mode 100644 new mode 100755 index 0bd58c873..b2fc01364 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -238,7 +238,7 @@ def __init__(self, boards, flags=0, pattern=Patterns.Chessboard, name='', self.checkerboard_flags = checkerboard_flags self.pattern = pattern self.br = cv_bridge.CvBridge() - + self.distortion_model = "pinhole" # self.db is list of (parameters, image) samples for use in calibration. parameters has form # (X, Y, size, skew) all normalized to [0,1], to keep track of what sort of samples we've taken # and ensure enough variety. @@ -292,6 +292,8 @@ def get_parameters(self, corners, board, size): skew = _get_skew(corners, board) params = [p_x, p_y, p_size, skew] return params + def set_distmodel(self,modelname): + self.distortion_model = modelname def is_slow_moving(self, corners, last_frame_corners): """ @@ -454,14 +456,18 @@ def downsample_and_detect(self, img): @staticmethod - def lrmsg(d, k, r, p, size): + def lrmsg(d, k, r, p, size, distortion_model): """ Used by :meth:`as_message`. Return a CameraInfo message for the given calibration matrices """ msg = sensor_msgs.msg.CameraInfo() msg.width, msg.height = size - if d.size > 5: - msg.distortion_model = "rational_polynomial" - else: - msg.distortion_model = "plumb_bob" + if "pinhole" in distortion_model: + if d.size > 5: + msg.distortion_model = "rational_polynomial" + else: + msg.distortion_model = "plumb_bob" + elif "fisheye" in distortion_model: + msg.distortion_model = "fisheye" + msg.D = numpy.ravel(d).copy().tolist() msg.K = numpy.ravel(k).copy().tolist() msg.R = numpy.ravel(r).copy().tolist() @@ -517,13 +523,22 @@ def lrost(name, d, k, r, p, size): return calmessage @staticmethod - def lryaml(name, d, k, r, p, size): + def lryaml(name, d, k, r, p, size,dist_model): def format_mat(x, precision): return ("[%s]" % ( numpy.array2string(x, precision=precision, suppress_small=True, separator=", ") .replace("[", "").replace("]", "").replace("\n", "\n ") )) + # Select dist model + if "pinhole" in dist_model: + if d.size > 5: + distortion_model = "rational_polynomial" + else: + distortion_model = "plumb_bob" + elif "fisheye" in dist_model: + distortion_model = "fisheye" + assert k.shape == (3, 3) assert r.shape == (3, 3) assert p.shape == (3, 4) @@ -535,7 +550,7 @@ def format_mat(x, precision): " rows: 3", " cols: 3", " data: " + format_mat(k, 5), - "distortion_model: " + ("rational_polynomial" if d.size > 5 else "plumb_bob"), + "distortion_model: " + distortion_model, "distortion_coefficients:", " rows: 1", " cols: %d" % d.size, @@ -583,7 +598,6 @@ def __init__(self): ImageDrawable.__init__(self) self.scrib = None self.linear_error = -1.0 - class StereoDrawable(ImageDrawable): def __init__(self): @@ -647,19 +661,28 @@ def cal_fromcorners(self, good): ipts = [ points for (points, _) in good ] opts = self.mk_object_points(boards) - # If FIX_ASPECT_RATIO flag set, enforce focal lengths have 1/1 ratio intrinsics_in = numpy.eye(3, dtype=numpy.float64) - reproj_err, self.intrinsics, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera( - opts, ipts, - self.size, - intrinsics_in, - None, - flags = self.calib_flags) - # OpenCV returns more than 8 coefficients (the additional ones all zeros) when CALIB_RATIONAL_MODEL is set. - # The extra ones include e.g. thin prism coefficients, which we are not interested in. - self.distortion = dist_coeffs.flat[:8].reshape(-1, 1) + if "pinhole" in self.distortion_model: + reproj_err, self.intrinsics, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera( + opts, ipts, + self.size, + intrinsics_in, + None, + flags = self.calib_flags) + # OpenCV returns more than 8 coefficients (the additional ones all zeros) when CALIB_RATIONAL_MODEL is set. + # The extra ones include e.g. thin prism coefficients, which we are not interested in. + self.distortion = dist_coeffs.flat[:8].reshape(-1, 1) + elif "fisheye" in self.distortion_model: + calibration_flags = cv2.fisheye.CALIB_FIX_SKEW + self.calib_flags # Add user flags + reproj_err, self.intrinsics, self.distortion, rvecs, tvecs = cv2.fisheye.calibrate( + opts, ipts, + self.size, + intrinsics_in, None, flags = calibration_flags, + criteria = (cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6)) + else: + print("Something went wrong when selecting a model") # R is identity matrix for monocular calibration self.R = numpy.eye(3, dtype=numpy.float64) self.P = numpy.zeros((3, 4), dtype=numpy.float64) @@ -674,14 +697,27 @@ def set_alpha(self, a): original image are in calibrated image). """ - # NOTE: Prior to Electric, this code was broken such that we never actually saved the new - # camera matrix. In effect, this enforced P = [K|0] for monocular cameras. - # TODO: Verify that OpenCV #1199 gets applied (improved GetOptimalNewCameraMatrix) - ncm, _ = cv2.getOptimalNewCameraMatrix(self.intrinsics, self.distortion, self.size, a) - for j in range(3): - for i in range(3): - self.P[j,i] = ncm[j, i] - self.mapx, self.mapy = cv2.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, ncm, self.size, cv2.CV_32FC1) + if "pinhole" in self.distortion_model: + # NOTE: Prior to Electric, this code was broken such that we never actually saved the new + # camera matrix. In effect, this enforced P = [K|0] for monocular cameras. + # TODO: Verify that OpenCV #1199 gets applied (improved GetOptimalNewCameraMatrix) + ncm, _ = cv2.getOptimalNewCameraMatrix(self.intrinsics, self.distortion, self.size, a) + for j in range(3): + for i in range(3): + self.P[j,i] = ncm[j, i] + self.mapx, self.mapy = cv2.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, ncm, self.size, cv2.CV_32FC1) + elif "fisheye" in self.distortion_model: + # TODO: Implement re-computation of P given users balance value + # ncm = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(self.intrinsics, self.distortion, self.size, self.R, balance=a) + for j in range(3): + for i in range(3): + self.P[j,i] = self.intrinsics[j, i] + # self.P[j,i] = ncm[j, i] + + self.mapx, self.mapy = cv2.fisheye.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, self.P, self.size, cv2.CV_32FC1) + else: + print("Something went wrong when selecting a model") + def remap(self, src): """ @@ -704,7 +740,7 @@ def undistort_points(self, src): def as_message(self): """ Return the camera calibration as a CameraInfo message """ - return self.lrmsg(self.distortion, self.intrinsics, self.R, self.P, self.size) + return self.lrmsg(self.distortion, self.intrinsics, self.R, self.P, self.size,self.distortion_model) def from_message(self, msg, alpha = 0.0): """ Initialize the camera calibration from a CameraInfo message """ @@ -724,7 +760,7 @@ def ost(self): return self.lrost(self.name, self.distortion, self.intrinsics, self.R, self.P, self.size) def yaml(self): - return self.lryaml(self.name, self.distortion, self.intrinsics, self.R, self.P, self.size) + return self.lryaml(self.name, self.distortion, self.intrinsics, self.R, self.P, self.size,self.distortion_model) def linear_error_from_image(self, image): """ @@ -936,23 +972,39 @@ def cal_fromcorners(self, good): self.T = numpy.zeros((3, 1), dtype=numpy.float64) self.R = numpy.eye(3, dtype=numpy.float64) - if LooseVersion(cv2.__version__).version[0] == 2: - cv2.stereoCalibrate(opts, lipts, ripts, self.size, - self.l.intrinsics, self.l.distortion, - self.r.intrinsics, self.r.distortion, - self.R, # R - self.T, # T - criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), - flags = flags) + + if "pinhole" in self.distortion_model: + if LooseVersion(cv2.__version__).version[0] == 2: + cv2.stereoCalibrate(opts, lipts, ripts, self.size, + self.l.intrinsics, self.l.distortion, + self.r.intrinsics, self.r.distortion, + self.R, # R + self.T, # T + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), + flags = flags) + else: + cv2.stereoCalibrate(opts, lipts, ripts, + self.l.intrinsics, self.l.distortion, + self.r.intrinsics, self.r.distortion, + self.size, + self.R, # R + self.T, # T + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), + flags = flags) + elif "fisheye" in self.distortion_model: + if LooseVersion(cv2.__version__).version[0] == 2: + print("You need OpenCV >3 to use fisheye camera model") + else: + cv2.fisheye.stereoCalibrate(opts, lipts, ripts, + self.l.intrinsics, self.l.distortion, + self.r.intrinsics, self.r.distortion, + self.size, + self.R, # R + self.T, # T + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), # 30, 1e-6 + flags = flags) else: - cv2.stereoCalibrate(opts, lipts, ripts, - self.l.intrinsics, self.l.distortion, - self.r.intrinsics, self.r.distortion, - self.size, - self.R, # R - self.T, # T - criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), - flags = flags) + print("Something went wrong when selecting a model") self.set_alpha(0.0) @@ -963,21 +1015,49 @@ def set_alpha(self, a): in calibrated image are valid) to 1 (zoomed out, all pixels in original image are in calibrated image). """ - - cv2.stereoRectify(self.l.intrinsics, - self.l.distortion, - self.r.intrinsics, - self.r.distortion, - self.size, - self.R, - self.T, - self.l.R, self.r.R, self.l.P, self.r.P, - alpha = a) - - cv2.initUndistortRectifyMap(self.l.intrinsics, self.l.distortion, self.l.R, self.l.P, self.size, cv2.CV_32FC1, - self.l.mapx, self.l.mapy) - cv2.initUndistortRectifyMap(self.r.intrinsics, self.r.distortion, self.r.R, self.r.P, self.size, cv2.CV_32FC1, - self.r.mapx, self.r.mapy) + if "pinhole" in self.distortion_model: + cv2.stereoRectify(self.l.intrinsics, + self.l.distortion, + self.r.intrinsics, + self.r.distortion, + self.size, + self.R, + self.T, + self.l.R, self.r.R, self.l.P, self.r.P, + alpha = a) + + cv2.initUndistortRectifyMap(self.l.intrinsics, self.l.distortion, self.l.R, self.l.P, self.size, cv2.CV_32FC1, + self.l.mapx, self.l.mapy) + cv2.initUndistortRectifyMap(self.r.intrinsics, self.r.distortion, self.r.R, self.r.P, self.size, cv2.CV_32FC1, + self.r.mapx, self.r.mapy) + + elif "fisheye" in self.distortion_model: + self.Q = numpy.zeros((4,4), dtype=numpy.float64) + + flags = cv2.CALIB_ZERO_DISPARITY # Operation flags that may be zero or CALIB_ZERO_DISPARITY . + # If the flag is set, the function makes the principal points of each camera have the same pixel coordinates in the rectified views. + # And if the flag is not set, the function may still shift the images in the horizontal or vertical direction + # (depending on the orientation of epipolar lines) to maximize the useful image area. + + cv2.fisheye.stereoRectify(self.l.intrinsics, self.l.distortion, + self.r.intrinsics, self.r.distortion, + self.size, + self.R, self.T, + flags, + self.l.R, self.r.R, + self.l.P, self.r.P, + self.Q, + self.size, + a, + 1.0 ) + self.l.P[:3,:3] = numpy.dot(self.l.intrinsics,self.l.R) + self.r.P[:3,:3] = numpy.dot(self.r.intrinsics,self.r.R) + cv2.fisheye.initUndistortRectifyMap(self.l.intrinsics, self.l.distortion, self.l.R, self.l.intrinsics, self.size, cv2.CV_32FC1, + self.l.mapx, self.l.mapy) + cv2.fisheye.initUndistortRectifyMap(self.r.intrinsics, self.r.distortion, self.r.R, self.r.intrinsics, self.size, cv2.CV_32FC1, + self.r.mapx, self.r.mapy) + else: + print("Something went wrong when selecting a model") def as_message(self): """ @@ -985,8 +1065,8 @@ def as_message(self): and right cameras respectively. """ - return (self.lrmsg(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P, self.size), - self.lrmsg(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P, self.size)) + return (self.lrmsg(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P, self.size,self.l.distortion_model), + self.lrmsg(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P, self.size,seld.r.distortion_model)) def from_message(self, msgs, alpha = 0.0): """ Initialize the camera calibration from a pair of CameraInfo messages. """ @@ -1014,7 +1094,7 @@ def ost(self): self.lrost(self.name + "/right", self.r.distortion, self.r.intrinsics, self.r.R, self.r.P, self.size)) def yaml(self, suffix, info): - return self.lryaml(self.name + suffix, info.distortion, info.intrinsics, info.R, info.P, self.size) + return self.lryaml(self.name + suffix, info.distortion, info.intrinsics, info.R, info.P, self.size,self.distortion_model) # TODO Get rid of "from_images" versions of these, instead have function to get undistorted corners def epipolar_error_from_images(self, limage, rimage): diff --git a/camera_calibration/src/camera_calibration/camera_calibrator.py b/camera_calibration/src/camera_calibration/camera_calibrator.py index 79f89b0f9..9e0102891 100755 --- a/camera_calibration/src/camera_calibration/camera_calibrator.py +++ b/camera_calibration/src/camera_calibration/camera_calibrator.py @@ -81,7 +81,9 @@ def __init__(self, queue, opencv_calibration_node): def run(self): cv2.namedWindow("display", cv2.WINDOW_NORMAL) cv2.setMouseCallback("display", self.opencv_calibration_node.on_mouse) + cv2.createTrackbar("Camera type: \n 0 : pinhole \n 1 : fisheye", "display", 0,1, self.opencv_calibration_node.on_model_change) cv2.createTrackbar("scale", "display", 0, 100, self.opencv_calibration_node.on_scale) + while True: if self.queue.qsize() > 0: self.image = self.queue.get() @@ -273,6 +275,8 @@ def on_mouse(self, event, x, y, flags, param): # Only shut down if we set camera info correctly, #3993 if self.do_upload(): rospy.signal_shutdown('Quit') + def on_model_change(self,model_select_val): + self.c.set_distmodel( "pinhole" if model_select_val < 0.5 else "fisheye") def on_scale(self, scalevalue): if self.c.calibrated: diff --git a/camera_calibration/test/directed.py b/camera_calibration/test/directed.py old mode 100644 new mode 100755 diff --git a/camera_calibration/test/multiple_boards.py b/camera_calibration/test/multiple_boards.py old mode 100644 new mode 100755 diff --git a/camera_calibration_fisheye/CHANGELOG.rst b/camera_calibration_fisheye/CHANGELOG.rst deleted file mode 100644 index aa32b37e8..000000000 --- a/camera_calibration_fisheye/CHANGELOG.rst +++ /dev/null @@ -1,170 +0,0 @@ -1.12.23 (2018-05-10) --------------------- -* camera_checker: Ensure cols + rows are in correct order (`#319 `_) - Without this commit, specifying a smaller column than row size lead to - huge reported errors: - ``` - $ rosrun camera_calibration cameracheck.py --size 6x7 --square 0.0495 - Linearity RMS Error: 13.545 Pixels Reprojection RMS Error: 22.766 Pixels - $ rosrun camera_calibration cameracheck.py --size 7x6 --square 0.0495 - Linearity RMS Error: 0.092 Pixels Reprojection RMS Error: 0.083 Pixels - ``` - This commit switches columns and rows around if necessary. -* Contributors: Martin Günther - -1.12.22 (2017-12-08) --------------------- -* Changed flags CV_LOAD_IMAGE_COLOR by IMREAD_COLOR to adapt to Opencv3. (`#252 `_) -* Fixed stereo calibration problem with chessboard with the same number of rows and cols by rotating the corners to same direction. -* Contributors: jbosch - -1.12.21 (2017-11-05) --------------------- -* re-add the calibration nodes but now using the Python modules. - Fixes `#298 `_ -* Move nodes to Python module. -* Contributors: Vincent Rabaud - -1.12.20 (2017-04-30) --------------------- -* properly save bytes buffer as such - This is useful for Python 3 and fixes `#256 `_. -* Get tests slightly looser. - OpenCV 3.2 gives slightly different results apparently. -* Use floor division where necessary. (`#247 `_) -* Fix and Improve Camera Calibration Checker Node (`#254 `_) - * Fix according to calibrator.py API - * Add approximate to cameracheck -* Force first corner off chessboard to be uppler left. - Fixes `#140 `_ -* fix doc jobs - This is a proper fix for `#233 `_ -* During stereo calibration check that the number of corners detected in the left and right images are the same. This fixes `ros-perception/image_pipeline#225 `_ -* Contributors: Leonard Gerard, Martin Peris, Vincent Rabaud, hgaiser - -1.12.19 (2016-07-24) --------------------- -* Fix array check in camerachecky.py - This closes `#205 `_ -* Contributors: Vincent Rabaud - -1.12.18 (2016-07-12) --------------------- - -1.12.17 (2016-07-11) --------------------- -* fix typo np -> numpy -* fix failing tests -* Contributors: Shingo Kitagawa, Vincent Rabaud - -1.12.16 (2016-03-19) --------------------- -* clean OpenCV dependency in package.xml -* Contributors: Vincent Rabaud - -1.12.15 (2016-01-17) --------------------- -* better 16 handling in mkgray - This re-uses `#150 `_ and therefore closes `#150 `_ -* fix OpenCV2 compatibility -* fix tests with OpenCV3 -* [Calibrator]: add yaml file with calibration data in output -* Contributors: Vincent Rabaud, sambrose - -1.12.14 (2015-07-22) --------------------- -* remove camera_hammer and install Python nodes properly - camera_hammer was just a test for camera info, nothing to do with - calibration. Plus the test was basic. -* Correct three errors that prevented the node to work properly. -* Contributors: Filippo Basso, Vincent Rabaud - -1.12.13 (2015-04-06) --------------------- -* replace Queue by deque of fixed size for simplicity - That is a potential fix for `#112 `_ -* Contributors: Vincent Rabaud - -1.12.12 (2014-12-31) --------------------- -* try to improve `#112 `_ -* Contributors: Vincent Rabaud - -1.12.11 (2014-10-26) --------------------- - -1.12.10 (2014-09-28) --------------------- -* Update calibrator.py - bugfix: stereo calibrator crashed after the signature of the method for the computation of the epipolar error changed but the function call was not updated -* Contributors: Volker Grabe - -1.12.9 (2014-09-21) -------------------- -* fix bad Python -* only analyze the latest image - fixes `#97 `_ -* flips width and height during resize to give correct aspect ratio -* Contributors: Russell Toris, Vincent Rabaud - -1.12.8 (2014-08-19) -------------------- -* install scripts in the local bin (they are now rosrun-able again) - fixes `#93 `_ -* fix default Constructor for OpenCV flags - this does not change anything in practice as the flag is set by the node. - It just fixes the test. -* Contributors: Vincent Rabaud - -1.12.6 (2014-07-27) -------------------- -* make sure the GUI is started in its processing thread and fix a typo - This fully fixes `#85 `_ -* fix bad call to save an image -* have display be in its own thread - that could be a fix for `#85 `_ -* fix bad usage of Numpy - fixes `#89 `_ -* fix asymmetric circle calibration - fixes `#35 `_ -* add more tests -* improve unittests to include all patterns -* install Python scripts properly - and fixes `#86 `_ -* fix typo that leads to segfault - fixes `#84 `_ -* also print self.report() on calibrate ... allows to use the params without having to commit them (e.g. for extrensic calibration between to cameras not used as stereo pair) -* fixes `#76 `_ - Move Python approximate time synchronizer to ros_comm -* remove all trace of cv in Python (use cv2) -* remove deprecated file (as mentioned in its help) -* fixes `#25 `_ - This is just removing deprecated options that were around since diamondback -* fixes `#74 `_ - calibrator.py is now using the cv2 only API when using cv_bridge. - The API got changed too but it seems to only be used internally. -* Contributors: Vincent Rabaud, ahb - -1.12.5 (2014-05-11) -------------------- -* Fix `#68 `_: StringIO issues in calibrator.py -* fix architecture independent -* Contributors: Miquel Massot, Vincent Rabaud - -1.12.4 (2014-04-28) -------------------- - -1.12.3 (2014-04-12) -------------------- -* camera_calibration: Fix Python import order -* Contributors: Scott K Logan - -1.12.2 (2014-04-08) -------------------- -* Fixes a typo on stereo camera info service calls - Script works after correcting the call names. -* Contributors: JoonasMelin - -1.11.4 (2013-11-23 13:10:55 +0100) ----------------------------------- -- add visualization during calibration and several calibration flags (#48) diff --git a/camera_calibration_fisheye/CMakeLists.txt b/camera_calibration_fisheye/CMakeLists.txt deleted file mode 100644 index 552452725..000000000 --- a/camera_calibration_fisheye/CMakeLists.txt +++ /dev/null @@ -1,31 +0,0 @@ -cmake_minimum_required(VERSION 2.8) -project(camera_calibration_fisheye) - -find_package(catkin REQUIRED) -catkin_package() - -catkin_python_setup() - -# if(CATKIN_ENABLE_TESTING) - # # Unit test of calibrator.py - # catkin_add_nosetests(test/directed.py) - - # # Tests simple calibration dataset - # catkin_download_test_data(camera_calibration.tar.gz http://download.ros.org/data/camera_calibration/camera_calibration.tar.gz - # DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests - # MD5 6da43ea314640a4c15dd7a90cbc3aee0 - # ) - - # # Tests multiple checkerboards - # catkin_download_test_data(multi_board_calibration.tar.gz http://download.ros.org/data/camera_calibration/multi_board_calibration.tar.gz - # DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests - # MD5 ddc0f69582d140e33f9d3bfb681956bb - # ) - # catkin_add_nosetests(test/multiple_boards.py) -# endif() - -catkin_install_python(PROGRAMS nodes/cameracalibrator.py - nodes/cameracheck.py - scripts/tarfile_calibration.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/camera_calibration_fisheye/README.md b/camera_calibration_fisheye/README.md deleted file mode 100644 index f761bdfc9..000000000 --- a/camera_calibration_fisheye/README.md +++ /dev/null @@ -1,55 +0,0 @@ -# camera_calibration_fisheye - -Same as [camera_calibration](http://wiki.ros.org/camera_calibration) but implementing OpenCV3 fisheye calibration functionality for monoCameras - - -# Usage -Camera Calibrator -To run the cameracalibrator.py node for a monocular camera using an 8x6 chessboard with 108mm squares: - - -``` -rosrun camera_calibration_fisheye cameracalibrator.py --size 8x6 --square 0.108 image:=/my_camera/image camera:=/my_camera -``` -When you click on the "Save" button after a succesfull calibration, the data (calibration data and images used for calibration) will be written to /tmp/calibrationdata.tar.gz. - -To run the cameracalibrator.py node for a stereo camera: - -``` -rosrun camera_calibration_fisheye cameracalibrator.py --size 8x6 --square 0.108 right:=/my_stereo/right/image_raw left:=/my_stereo/left/image_raw left_camera:=/my_stereo/left right_camera:=/my_stereo/right -``` - -cameracalibrator.py supports the following options: - -``` - Chessboard Options: - You must specify one or more chessboards as pairs of --size and - --square options. - - -p PATTERN, --pattern=PATTERN - calibration pattern to detect - 'chessboard', - 'circles', 'acircles' - -s SIZE, --size=SIZE - chessboard size as NxM, counting interior corners - (e.g. a standard chessboard is 7x7) - -q SQUARE, --square=SQUARE - chessboard square size in meters - - ROS Communication Options: - --approximate=APPROXIMATE - allow specified slop (in seconds) when pairing images - from unsynchronized stereo cameras - --no-service-check disable check for set_camera_info services at startup - - Calibration Optimizer Options: - --fix-principal-point - fix the principal point at the image center - --fix-aspect-ratio enforce focal lengths (fx, fy) are equal - --zero-tangent-dist - set tangential distortion coefficients (p1, p2) to - zero - -k NUM_COEFFS, --k-coefficients=NUM_COEFFS - number of radial distortion coefficients to use (up to - 6, default 2) - -``` diff --git a/camera_calibration_fisheye/button.jpg b/camera_calibration_fisheye/button.jpg deleted file mode 100644 index 494a6f6fc5247fdb882536643408f1c5abf7c8a1..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 21110 zcmeIaby!qg*FS!S?gph>I%Z(#kZ$P)0bzil8M;JJQCg5t1eETO1{IVJ1(hyAK@kw7 zQ{j6CSer*64Ltgp7gSgn^5_Tt^c2+j=toPU%Y*rob^>XV+#h)rgDpb(ldcA7vfLlC zXqAxsA2dh;7(Zz+I{@=+oF1u85B6O|GX zM)%Y|cz`YLPYL{=?;pA>e*XfA|NjB56)FO^u)&7#CW#Yo4@#7M`$z``NO#lptV&cMJe&ciPZ6%`d_=8}|^ zfXN7oh{DiHAo%$BM1(}=iHXm{SQ%Jh|Lf=UIY5pF&R0832pfPw4#6acoOS}tU^%hT zD;)$~ld}hcfr*8UgNuhxKnMcr$p8!pCME_JCN?$}*a(m?Fde`m$EIL~D&SD++u^c# zQ^6vVa`4y{AHSeBc(cJFas?HMPe60-JS`pP1ukwLUQxK1xP+vXlCp}bn!1Ljp^>qP zshPQjy#vD0$rr0}Bfi3m0842u1)HFv+p7S)n)-3i`Nq-jr;x2s|pqq@2eu@YzKSHmI+l-Vo4m zh`#6CL|5&sX8&2mBL7b{`(3fW>NN!rVM4&p!z2gffqh=?+-n5f#g&4O>TdVRl+lrV zamKLVd(gb;7SeBSDYLc`NNrqSarKhHE3)vqx-YoaDk#26*Ec=_F;b^MO;efwJArG} zS_45Wo>mU}wo+2O+=8qvx3#ch*_du#a!FEY{P$fd-ThzG6qEeGWPWRtdP;j%8r3=dQ_i7Llojgzw%&8T7$- z2Xv*=lLj^5Sr$(-1cumQFURQjy~b>k3gmCE&+#}$yPmVX{^{GwU8~EQ0kWOt;qTy% z?*=4#%T^sQb}CJIWGA=kz5U9h2X*f2TfOU?&TC55J?^f{*~FU1YP%_DzspV}PiN9# zyk=Y&;jNgNS4&{lzhnLWLi`gX9mCw`+T^_Ztez|K6@F8yk4Y*#%vJmDMzhNrodP$5 z!k)dH3hT|F=^&ZCy#s%mo~_kUK0##r!R*!wUuo8*Fs7w3M00YNpvE%iLG~>bC08HD zkL3a64EZtV=`!kM^>SmlflAA+036p?SvcvOC-mfcfgikmWq5tluOocu{59rl7cwaQ zO(kbht7{g~(WL9POcs!}u~=!h`6PX0n&C?Ni8cY9DQER1zC0=F-4tT>b^@+<+%F#Q zc6mwnZ8Kw75>nd z-t?HMp53w1F4eYl?luw-~ikJ5Vhye+@{`6#s^k2>y+d$wnjxK}uxd)qBP=_j$p zY|-LV4U?#OY`mQ^2)e%`bP(a)oA3 zkX>YkjGd6hRzqS%%EWhB?0)GXB~YuxLIfEkmUstieiOBKFZfOIX!$8nu+Nk_zgkn~ zQ`CG47)=jgacn9pEjRC@e2dT55kp0nm2R7e1Q}h5eaKBeDI;+yzrCD*Voc(+u~Wb- z@M;LbcI;S)fJ*+&NgCZO^Gi~%_lF1bUfrLYyqEFe)8d;QIjE_b+Lw>#UeQdAUz*B# za@dj{X`85vqrrCXexA~{Js0y!!=gFFBcxdRQ|tQI2*s-it8rrLFMF?A@Uz8sO}dl> zW`?ys_3=G91%e)2NSDm$?jHZRU!M2TA^5=%Ny!H!KNqV9|0jy)JuUkdB}K&htpfUx z%)=W&7Z<|ceR%q+>H=lZJDxQ3pjby%EE(@ zVdGMgCa${FNCUwn&ik;!UM+`SCI4?-wt+E7&P|;K4chprt9rZCstbC}$tp=pNm-I6 z{2Dd|3T|80D;mwQ&2Gt;G)gXSKNdA@%qXIMS~a{QORB(GAM3w^^NjlH=PLP)mzKNw z2}xD*7CVsV)H6emY0#Zu9(-~yqcRm<>t~lL7bitGlF!ag1^H03o*`b-L@)U03Qkz~ z^D(n~c~l;F^W!-OLmi~3@H+k|L8{P>+C-(xo?a&*)L!;YmuM}k4)@Df-Y$)L1W6{C zJ(hoZtLrc=^tGCWy}GO`PnFEwXjJP`0nQ=b@#^6?d#vR{5gATRkGCd5GOfFquf7^_ zw#KV!$H>#=PetGV_QZM0NjAm%@OAEF1p~*YH7aK-Ykp@fy2;lSuj+`yQNnBc=MD+_ zhj$)w$Icokq<*w##68GZ4;9JNt+?FdBZCmImy9P3_pvrw-dfq0djN?r^Lr%}yCuzx zi~Ne6K6QclS<1TH=%|sGVs&S69_Uxr%aWnzHy9w55#vNKzSvFih_(8QjyFS_Z>e79 zYsjkrLibAp15Agl)0|}vdP057gph7#hhsIWA)7Wjx(Mv{56ZYJd>0nlgWZ+{FqW3J z8MBv$9#1U0C~Pqf6~1-7JoxUD0sHXIR`baP>MxrI(pqtg7u!;i)6RSTZ7c8fcc>yb z8e|qOZZLLr_W6`NMoAI`6YLD|oiEQZThId-OZ$uUuR*mgVpHk=kAeAg^7K9FXJ=nu zFDW4*4Ykp+Gw?A*i6R5FjHT=w)Z`ituH*f^b55$Z~ys(#FMtbdcpT zh3g3Gcqt*Ak?O%HgkkU{Bl}=idr1c_c{u`^K&e1?FL#8m9ZR6Qn}?57pe)y!aw(8T zvxT@o5X!+(%0OB5Cj|T@%k{G?0RaJm0V0B)C?_GPq@<*fFiZ#r695qcK0zM7c7XyO zKJ1_d0+ur!$_O8O6w=EV>FM!PmoL)sFJ*tL7&_&wW}r9=iwydwl!m*V6T%ef;Oi_0 zlYj}!2>mPf??`QghZ7nq35ER${mK4~Rq}NALLq#7z*5OUe**vH{RZked-{5Uje&7xr(sj-4w)?rgvOv+ie%zmh#2z;-!H|GAU?1v^Xljk^T4 zg+D?W>4fkB^Ms4>!{FjzkN|@uKTHh94-*#!1IUvQ<%f$1g8>TCAYVd+A0{crFCq-( z7lBHEL6RR11A_<*iNOZfQSxP8`ov$a#$kWqJP653@vYbsvA7@W579~Rw z7E`2$gJ*!x5BWdAe^Y??PjWw#{(<}@tOKqE4tBnF|B3kDu;AKw){FmHVTK~Q^ z(J6l{nCPEU=x<;q2-MlqhW>IRBlHh8y8LHMRW~~yXD?*nZ#=jJ{KWrj(r+0hPd87L zA!v6Hast1U|H1o>LpO>M(j5UVbf9grbNgHG{``FMt9Q=|^ACZ4r~;=m(ia(waQKgP z{Uz~RnM$CSr;i`%tZZ^nA*k4|8viZfH(d3SpZgUAN)84S`c>OstlyYll6ojlCzPE# z+M@hy^B-CL;{O&fboTQFjftTb0^wj{=jMly6O|AVfk_C7!Qmp{3_p|pC+N4lf)@hi z3)bO>EL=iT0w(%5*}nsR%l#^-BGS(1hph1LTKzNax0tG@y`9@n6Ds+a#GmZHW6`Du z@uvw96%msFjf)Ue6bk<}z|czn0sJ?5{M$Nw%oPWyy z3*fg3B|jg~1u#H(I3Q5q(EBx9{)PP;{;M6qbq(>4k#NTR4K;N}`XUtVz{Ub6)2~7E zH|B4sp*_L_p=9TU?scOeFU0R+{3GGF$e9aarVLI&kF&+#hx>HaX#bk@cNz0PWX}Ak zzhwSM`YrP}g9i4qm?*!fu%wvqSqvIcaVYFZ3_Iieh($m?9Qp$z%8!;3KZ`+w3mQ|9 z1I8dnR01x17K6qXq(M^)#-MQ)7XeK-ItKAzEQyXKKqMHW^AZ6KIXXt?A_5wCbPURZ z@tG`W-bJC{j5~|L(t`94j`$fZfsQ4?a)R+0MiLIv;xIH{9CQW17<3p!p^~6Zbc~jj zlt9Z$N}w^45@-x)_RnJIS&Y^ud8SQL5}l*unNCS`NnpZIF@Dgg;0GNMeldP=ehGd_ zez0kvP<|+kA1cBR73Bw=3=jkr=LbC!7>EV^6wplpJs3FX%z(ZK=(d2~h_r}=lB%kb zu%fsER8mXIvPb-I3x3XQv~dDU@xL*Z4)(tdrI#Pd z?QE0cU@wGlLx8iu*9WwfQ1tEww0{ovQjVS|cROD>BzlJ-Jk`elg`@hTA3%yN1 zdmX>C|C`tIJNXY|{hKFsW@L|Kk1cY7Kf|Iy(Qp#=rbLclV#|eTJ3N@bK|PuXY;Ba(;eD z2Pp|K!mHDAe+0)(+-K`qR zXTu5ga}3FH{ZOs+->c?g5jqFNBk%$109sy|%H$QhINFRVSl{>=D2RvCM zLi2qQ_Rb(J4AMj>BLgLnmH_}fBBwv-D}T_wh(NHM;7P2fR}kosI{UJ4+w-uXCk=}_ zBESvd>norKeoC=JIk2F=1K4>4fv2Wtbw*186zH~P0UKEiTv){fV1i)t|9<}4#NVj@ zo{_^Ulw$u+JpNEKNO1( zdPl(`@UMgT|19`NvHmCrzX8G#^y0u3h{XgPWuTn{yW7J7+*N>|r;r~1RS*9k#r`M* zn*1@YL4q>>1fa4J1jvS{0L-M})fS!4#%u7GUJxGJ;XYt=6 zj70D!h7Zz-122zyNVT8n_6k0Gfa< zU<8;0mjQdg8E^+sKmZU5L;z7hEN}-%1u}q#Kt50cR01_XBk&Y>0dxZcz-wR>mDD7*rUH7#A>vFeES(Ftjj?Fsw10FuXB> zF>YWaVBEvV#VE&Uz<7Z%fH8_OkFkoeiwR&7V$xu;VhUhNU@BqiVp?E2VxlmwV#Z*m zVdi31VzyxRU=Cx>V}8Lr#KOU%#A3k`z>>yN$1=vU$MVLyiWP^IfmMvvfYpgLjP)LC z1M38v7@Gl`4_gXb1KSMS2|EBg3OfzE5W60`3wsp%BlaE+4h{_t7mhfNI*vJx3r+}5 z98MNa70z>X?Uf0t$1(omhg`8$?!SxrSNs}9q@zj6Y=x$TkwbQKjI$~kP~nb$PyS4xDZ4T z+#{$Y=pvXQ*d`<-WFwR$)F(s|UL(9mSWVbV_@3~Ph=PchNQuab$d4$2sEFt}(In9} zF){H4;)}!<#JqnAiYc)Oqxbo zOFBgQg$$qU0+|w-9obc~2V~7;<7B(!l;luyU2=EwIPy~RKJw2LxD*#CR45!MZcyY= zbWki%Vp6hGDpA@~UZ>2b?4tZgg+s+fr9tIF6-!k?HAuBVO+hV6Z9*ML{eb!z^*jwG z%>^0_8aJ9mni`rhnxk`!=M>H%&PAWAJoon8{(1WI7tbTk$DFS^KYIR%mW5V@)|EDi zwvl$04vUVLPM{kLY_seH>=Nvb>?!Q+>>C`+96B7K9Az9+ocNp)oX(u-oIRZT7q~8%UbuCk z@xo`W^IRHS!CYlrGu*`7^4#9sx!fZ>I6M+Ot~^;huX!#lnkA3Oouf3grsxio%L0#d^hkB`KvarRU0+ z%4*6n%7ZEtD#j`qD(_U;RGn1IR5#S%Y9VUR)v?vJ)RWZ5H5fJQHA*x#G{rT;G&{A3 zv<$T}v_5F_YNNDUbTD)@b?)lST;jasaj8)k(AChrt2?X5rRS~pL?1_APyd1bCj+QK zs6me*rJ;>sso|cHqEVvJv@y4_pK-ehsfm?IiOIgHvT3sEyqS<$nAw0iojKCH$pY8H z#G=q**HXnY)$*ei+$zdy%$mzO(7Nw3-DS7S&umC->}+anv29IlOKrc|>DuMm?OxHi zl6hsrUdcY)e%0ZkLyE&PLKcyXSaOtgOm_V2B{~;)%I}q@EhSDuc=)tjv$C|jd&F)9GM)sbKUfM%MHdG*KRD_RJ&OgMG}RI8owod z>)~yz+sNC4(IV0JqfcTSV)|o+W7A^4#o5R8$3x@q#h)fPCJZKuC1&5jx#MwXEJ;48 zIGH>-IC*52`X5 zGNUv1vmCPCW?#&%e8~7P_TkYZu`@fHVX(Y&1GI&NrDiy=m5LZf}upsedB)r06N@(+AI}pCz^uwBBev zZ3}MOeeU)A%L}I$3+*=T(;a3VBc1x4gI!u(z1^za9X$#?ZN0L+Py3|$n)@aC8wbP& z8eYO**1v+ksvi^^Y#0(BYI-gCy5)_`o7T7TZ(j^64|k7fj0}uk8htZnJT@_IH9kM# zF!6cPZE|zUf9iPp+6?wg%q-b#+B=4KxpO>oRr6x=&)zG)fBC`i!}Nmv!s?>W;>`)<#D?|47vfcBvHP~x!X$n z=KuOWyV>-A75*jn<7Sf+;D8AMFZKMo*~A3;G+5|6Kxj|p$AiAqgoRCj3Bdqw2uYEF z=|65Z;b5Y#)1mMGfES+tTyi`LR!S&76}1AJz8yOU%sV29hEwsJh?2n-RL6i@~5))XEam3PErYIazXh+@tL z4abW&oA{vq>!*La;H#G7C8?FSHz55fIJpomfI`<+oNH%T5BwIxYRP^>YKO?o z(nII&rC_;dcrH)qb`RHx;$mL1Xs??(yB}$;-X7d1)%i%KTE&)gf8R}2X2~COrud7@ z_pYnnh4=0=dhbdmU)_%MU1T4ZJh+$Ss`0?8p^lly(8T&)3O;6{U(M88H3H1*rR~Wn zZljDkU*fL!RFkOLANsG0Cy*{3dLFw{C)Bj`R&cHkX1jjsQX}xDZo2Bd&6pV(VqMw# zY;7JzReQNQfgmNR$0I5^Wr2)>l z=}x$e|Gux-zF$deTVs%)pt)HpXS1MjKSAo{qhuqtX%m?=x8+a%k$EI<86*|hn`&^g z%w;_3eOppxyFs@rI1 zZ0-|eP#kB<&fp!J2-b=q@ZQg~^tM$|8g_iz>1Z-vLZ!=hL*ee?f&Thc8C!?si$IpV z_DWuWF6?Dv&Hc4EE9Ye5YoE~K*n}}bX+313R~5Xy?S_rgPFzL$tlS%M$BGov21p8a z-wumYHz!$CSlW0z9)7w@(H()R;CPUR5?SspC49no3Z#(SC68m=j8Q!7juz!#EwUDSEm+EgdR zR4cxrUE9~=QaE|$UAf1a3L5Fuw-~GlL|bxJLhl5(W4u+o84ADJ$56WV(xq>5p`pFH z=kBoV#a#~BlaCQ*3bagwM~h2MQipjjPl4|WRjGzXmWRe2BMrf1mn9CaM_;kL`Oa=a zR|gs8`95t6uh*hah|%?}IqMaDbH1xNM|TD;TgArD50P1MYB{ETQiY$3)R4v;oB4=;U?-qqmW*L*GV9XhzdbHD?M7u)OEml#hHnloV7{vdhT@|I}epkPgK zZk_dtn`n>pPK+gaqah$|+m-Ncay-j%avd`3Ooz&yL zeyY-70G=64e#>dL z=5%bvdFPReWysrsbst&?OJi+#)M($Wo$fI1Br##;IbPBvw)s#&A&K(=b!XMMVa+DI z_BJ~Y;mu-W91E#y6uv$FdZgOLB(vVkjccoTuj0c*7sp4$1rDcG=Qf`8eXiPh9bIX~ z6+~`m!^3_#iDN^#M&_I*HR{pW^vC;bq!gnC{9ENxHQA|0&z%A<2)PBwwbIZ(8`WQI zam~v)sH0t^n^#_q(`hcVD)twSq*>eJgpQZH=%sTiUu|!`@A;zl6(n)pDrLxLR^tna zbRw{p-H92uF9vwXd`?o`>>SC~Wy5gkt}=2wU4y;T&VCY`SdN5R@+o5r=n zr1*@9Jo-5+DzehJU$C+naw@Me%7|#D3O#+&mAOlqC7JTkK$h-vgqOFr0lon@luo&X zC*IPOnw9w6vkOy)(-?0ZF8S5**fr3_)5&L5iI)zHS{jW?65x|v%s(>PT;fein4CC= zlA4Xu=mrkx2jzUd!=(ZX^KLh#RV)<7e~js|i!>*$eBt4Q&BOh{wsc#n6z=Z&sJQaJ zax!&1YZU)CEsuAZmIfKDTnV~Jjzory(8H5QB(mRMblvFw0t>mZ6-soBc?x8@#=X+J zbEgTW8KU~+q%vTc85iqYz{ri_sWggy-ToXaNPk-Pdbm8-^KaisWP9db*)ik?eBLvr zCpzTtY?WEupTjjMwc-`G`AFPr%qW3-{P=s=cKmM2-aD1s>V^UwC$|A+7VrJVldGZO zwsvm>!#lrqllTwyV)n%CZI&7+SjAx)NqnuA-P`Z!`MTws+}!d0$VJ^9Umy1-AR+H1 z!E_27f4@4F@4%}mKASze6|&UkI5_deE7aa(F0P~q#rVt!mx$iKdp6O#;gUEmeVlt? z+=DpYE#ebtgkZ*pQUlI$bJN!1tq+>c$?5do3{0aWdJ8V=?qewpS&9!V-a2V;7A8w& zAUUD}@scI&uaxJLDe+;>5JlqAVwGVC!>cDf8$+-Mr#nNXeadU!3HXtZS$c?y)XN%k zDu<(zIl?9xdA))u5Q;MU66i4x2hJ>f;D|?gyZFlI82So!aw-NDk89yNvf2;(nhh@) z%vts4I2p+~1cQTNGQ18J7VcY~2L z-)I!|O--E6*kta#w50ZSbi+IC)b0w)gw{!dcHwQD!QC7F%!c3Ie)148_(1AvQ1)?B zRbS8IwI}x-(SkuJ-J*m0$TZQ3)=1bE4!F##9u9UaaLGa2;+S!}{c(-f#yo76r>`)| z>8q>v_~#aK#6^>jX@`V$Jd2t+jvl;`J^sd+!q9lMwaVvC@|Vxgx?~_dLD9i31EtgU zS8Q{DCc`_sC~p2sq%&;M$?)@>X8kg&3pV3(VxkI3WQdmD*WL5cDA9L(<*vcqjK6Te98F=RRO zHnGz;M$BNYhkPN;YMfci3Oxmii=o)+QI26rx`(QBK|_s*uXpEc{Y0U02BNCwa>)w# zObZi`p~Rzbq2|5)rP$AQ{HlkJ>z8JqwFw+w4OxeWcr+}*G}5kUiBP6p@xD79>O?!| zer?NCSDKEJ1C{igJrM}pKY30PBtwd8O1#`nudtTCPRbqD}fjKaw_)SUPCx#l8( zD{I7jobq+=75V0Vjx5LZx9`pDQd~<=hW8rf41Bnv0zb^WJ@TL0iP=<*yVz{z(p&Uo zdeI=5VMu*&=hMXjY&WBNUaaaIn)kzF-jt3~PkUPL3dnz14R7Zv!>~2#t z^7C!v<<*KO>N8Za(bBb6Go;0lim_-)&dm;-{?^A&!4XlTg#i!l)3Y$h`SJ+wM9oC1 zqG}Ai1u+;m7BB8Q`rht6K0|o@+S(Y?%6WZW_b1YXFN=g~^c5J}o|za9i*3l<((7<9 zOIk3e)8;pzT;gCpe;!vp?cm$41gl;x8I{9h*~o}yd3w*yheJl|6VGNV-u5nynoK#+ z;<(p78AnGY zmEydmbw&C4MH-IxdMIxsoKNcSI!};u4q2jB7ZP!OEcdE$<7A~cah0|t?3n6VVO%PA zMEk?X5w!WK+&A*6Nfw?1*>K}xS34Yw7^x;QTigu%yo=IA=@(`8`d&yJoYzVhYC391 zG;a!FddEjbiJ6U6?suFkv*vZ!$<4?(I?sxI?^>I`78%YlvMbMVjwS7tppcI1of)~C zG`ye73N0pNQb@ZS-akhy6Sy-(e&`5`UgXhKmntF95xKqmJ{d{xmq6u&f;KN-e04bQ z|FvwDx^lad`p%RDYRI>Bw$RjBpuvUUNtSQSJ(!GdioULNE(T$nt|DeSFSPL3pM5FB zx8eM&O`;+ae-|qwDu)ph-zXOgps6^jo8PyOct5|%!k{Cp?<;0fDLiGguDrxhM_=3W zaW*X*KMsscsxstj+{sbHhZBm0LCWH&^q2#!=s*L_ddDILB}CJdJXtZszYxB&f!~gE zxk|ZB&ER@myz1HuSFgvtaGFU~_H=uyL-q?MvaWaR$EoTku{;%J9^T7;U{4|%Vj^c@ zq`_2o&c+SOE8TVZ!K)Jg;TLc=_E)+xuG}jz8dp*&?DiV;XN|4B`ZW|gh{QVt%LOyp zJ3~d(+?#EhJia3fo_4HGd7O)t?Vi=}i+vm81FsD3545d_eOnS~V{reIjb@<3kaBSs zp}1!w@=H`1wX{>yE#9`0qcsu}P#1Q07Y*MM2-m+!`#J2f1Gntd zK!NBhfzJLrS1HPMc!M85=lMjT!L&4@-Xir*hL7nHtyfW4f{-r8=EAJAo7#1W>dWhO z@?8RHo(KNx9_f5e+Ajq~#ontO^tUj@1WgzZ_i)OwrbI(2cM5%;mAwmPDj4`O=Mo@J znnPMkG>ed6GE6jLGrFT3*k9977EKJ}2zDi^UA<-f#qO}HctWKH-zcrUP9+41XlUI;O+Yzre(%t*&bDzl*ZOd zSQzF~mcy!73zuaf)xLd8=hqRp6?1C(H?<$qMx+@x2E^+2a=Rn#9O|m-89%viE{l<~ z*ZNo-Q{+CKy@^`v^lm1SNH@Bb!I$RD6=66Q<(%xW%O1q$Yi=*m(LPvO!%BpQ;Bbfw z7;$LgJq7=qWQqKQmRSKKsG7C%vZw*44qqQ-55HTb z$wlqyuI-?-2eh@#PXp3-U$t8CY}ydW<)OqHC9g}xL&7_y*;v?bBok06Y!1mq&QIMn z?(qK<`bol}Nb>8jF8>{F`4jJ|wuaWN@BB6w+O{auc8w`$opS4p%&aFCZyy9NSvuxp zWxb&CHOj+UZ{oTFAAKinJDMefUtTUy0=4f&YOq8OuW^pGE?%F=>6HuGUL^BNX2*&T zXq|fJLIr<}4_(kWKYS9q=iS z!z{gjZHwdb^y2)#SlYeq>eM%I z>ny4N%JV4Y<0!FWjjl>}t-;GU;Q@Yf(=Og>>B!2sqD4mQwCS%TD~r~l9*#ru-hS!s z*VBV4{Iz0c(gioCWc&pf(%D+3DGj~_-!82u<9j=-a&MSuSHZg|?nb_cuirRNU_-;Z zw*&efNZ#b&pz!-ddUzTJ)Z5F6hB<&9bq5v8uKSm4yicTXk5j-RG-x0uwqPzs(#G89 zLkm4P8m%YS_Xt%I=mnI9MfFVhH>T#rp=wIYO3Ml+e8e0{y%!*QMghY9ZH01;wPA6| z^!?spOV2uNZ48CR8}p}IzFcR#MPVIU7o5!A8LRIa27N%2LU=jy%k>w%#I0S=U=`Tu z(JKPw%9~c7iW1EB(|D;eI4?!f+U77_;h?`pxCD)aS5g%eejH-ES{`^e%=RYbj*dBgC+|ar$WBf+gxgQ?f&zT$JY&P341j|}E2->w5 z4-phV^}FGOArfW~=58foX zlTfwN-x!iIsDX2ALzCWy;_SRN9e@()UoyLM0WnBVH`auCTj*Q&k;2jwAF(Cx57OcI zV|rs=?x#RBuT!%XPt^c}$W@YrHLI=BUV};t07??)H-suwO5Yi_5gLGd=H6or0L=?N5Dl zJ#LRB;b7>BU%07;zRK?1UOLU<47vplJf~Uchu*M zxi+!ig|$8Nl39eA!a{6o&aglG}Hop`yK_=k*nq zFldjpZoVb{SYD@J-PY9$+4Rq*O6@FJl9Q|05?JX6M(kxBuJB;whmckubU(edOW*ZP zHb_S8O6_*YgQu3LTM6sBWddf?;D_{N@HVscD)HNN_AyRyE)MDwG2H^*wWk@r#l-Cm9qRYgge zprsp-Ma<{S%M0e?CwbF5YgMY^K8FwIE_LUl_jAP$K13&IwKpEyiq{bGS-DO<(5b5n zXng!QJZ1{sbdQL-2_JXm#h3TFR@{&#J3N`PuJ7y^3Ab8xTt+;S58906^-(#9kc zefK>MGg3jGKiL3}?mdHwu54_yi&GJs*zQzo(xh=p`FCxvN-6aOB@Q!bjE9x2wBHpb z18*gT>+e@`z0Xad_6)k4a9~mCVpfX0Vfv}Kq%w1>vYX{f;(dx5sR!x3<=~`o~Je0ut#$F3V;u2rYL_1%K)rP9ry|c)D0h zc&m#)tnR$&qc`Sv6;!&1oR7{VkN>DlfuV{rj_95yMp~)t1`Qp5$8H}Lr&V$-XyH$q z$*(r!H<3E(OwtriURLl(SfYf{m;?tmaqxC&rE<_XIR@v=4nFj7TW{#$h1*!g+@_@# zJCzHprAh2FQAEF* zSEH#4T&57pUq-dUUV4-3K5MpUwsGj}_Nsko<1}qVG03%-7CI%^QCFxxDzKxu@hBsT$%(Bb`k zdz+@~ccUDE%xm~#121igNxjdj z{rr&>QYD{?VjM+OFf_XrrsUgh|o?1`w+4joUuDhB> z#n34liYv0jx2SnhHh%Bg(+6%_ymiQ4^C}LeEl9z4f78WC-+eoZ^V)UtVO`OUWVHR5vdtC@ydWB z`BE|mr+I|oqC8n;fk!|a)e2LGaDISqb@?R^%ETEivL3pKbIjSE#=IhR70-1X8q-kH z9+SGNC?&fc|MFk~ENhLd5d(?soIh+tahtBkv?Ej3 z^h!zN3@^`$OB;6k9v{N;m5xEGw?tKLZd8d6jIUq$`WRN(A{2-1>$uUAyk`@ihCe)s z4YG04czlmv_Q`bjmA9j#l7`V^&r7S~NgQGhUdp<23NO39Zf+pFYB2J4tSBVQtAx`S z(dCR>vJ-mGswripK1yrPNS>RVqm}eJk6(*IBs1%>$tKtNZJLLX5?9yw132EhT9e{$ z6mY_+Wkt(WOVpIq)L-e)X_)d6xR~7-PEVcG67hq+Tw4BAs4QFC@m^}`4cnNGfUm#l zbA$X#nbD|gm@~Af*TdQ2^W51I!4QU7&TV_QHF$RpWMXxkG@|P?w+g*}}g|1d)6;&9UkR~eX&G(d4IV1$l z=kkRnkS4w0xN4R7c+tiGB=6|z)C7I=x53OTtzyuJRl3yT&uD$o_xQfuZkdaqIrhCt zM|{T*fz-_n{73HGOoq`KLFAlIr$_yB?yfx3x3^=IITtkQY?FI2lrgb)dwW`bciNDr zgEXst;MHQ6$Gf?h{YOvCo29)4Kf!nqtOAo_w*B5u$8l1wY2Hkbcj=6!D~yaeXSJF% zpQEjm&MLJ}yK(qbuRsO1YpNf)ar3JvKP34fli5Ngi_f>N&*yigZl%eU2c4gJH-B&4 zpI2XqZXweJ&TF*$iQ;Up#ynKE%z6>>yZ)iQ&8mIRotZD>TCZFnW{bvoRqqJ7%gt%UpHMDoSH^^idNtmD1?=UVRd z%4@T;yiZ|SoCnl3_7fis4U!w!)R4$v8C)yO+D{zCVmncXj7cGM7C{*kjJVg!qY6v- z91TBToYmhY&U(&X*07TQ-e-d_*)Z-te10fx+Fyi-78NFW6C}XeG8f86bzm-)q#|!&v+$AIyDW8uVXbS*oUY&+3~Y z&+=U|CKrCxlW+%6LIN1Nn2DHoUu?*bsL24N4aH8<$6w3b-F(4gc~>Sw{N(mYGtJ0| zd2SR&Yv=X(!-by53)g#x9}hh|MpnAWU0`HKJh9H6*cGWYHY`x8_q(+(LS3LaeBs4s z;)14TKSDb`r69PKLrxK$0o8;4LdzQ)VH0az_rtMr(sO#;Z}qIs&4HgJsbBBf23gt9 zL@avFkTTvrnJNfv_w#V2emYL!&s%<>{QgExVtTAQl-Iz4mm$FT#4?ycvo)MCrEoF* z^;TIbl&Ce$H=ApIVqJjKy)BondtiXs*(Cu&T+MpHlV44{C;$@evk+d)oWH6DD=ho) zj^G<(dfca#j0Ghz);e#FrgunMs@Slpi*hUVO1kD3h+S0pG_2@?zqjl9JO=-)fmFCn zQ;F|m<3bBQcH@K%iSWG4)-8%u@1=O}qX9o-h6-XM3MIBh-USg%a}s;*kP&q`!U1GRr!Y6I+Jt!`0)w;-6Ctv-+Qiq z5uKV{(#xsYi8eI1i_)uqdi<3&jZRUG*I?FLLsJjnNKMK2?%N!!vM=;U7#nD?U@IhR z4C3IRH1EhHRnyH0$UyZCIlPA74v215tLMgWrKzF3N|SPHMJTI{i8Q9Mmh*CZMdEj! zo-Z7F5v&cDyFE^SolQErd-=W%kI%A)Kt0>L&PPHYzjH{E@vy2>s`Ib<)okSNdL$d= zB#=kyEz+#jW+K#@`tGdD4PURivUPnqYBjW^;E8t}%##02jixg_kEQ8brC>iSGNipN z;H=+9^E}l~=Y+M+y|8_CPpFsWw;Av-nQ9cwArleip<4QTHzwVz`Q$~2sWv1pQQYLcjau$ZA?xh0CXDcQK88J zTzzw`M>Q8E7)b5rU92^J-içZyBLlB)R_IlzzjXjC*HYqEgQn7_P%S{Xj!&YiY z!A0`kkG!`z7~+*FNK`vr?=wBQN^O1#9u(dZCMA<`Xu{bSaL}GxXiU5mz}Y!9!dHLm zvcW|+9oxq|8d~nntlbZ^6t^Vzx~bL@?l$yHe7)YhVY)TwXYD{*Yb!S4eobOJ{7U$o zyU(i@Vdm0W|MY@_=J#4QB8EWwdwpLn;G8!2P~DBp|a_;=+En zVq`<0)Zj84y6fbJ#K;V>yMIx(LBB1cwMlk**)ruEPeyPEr%J(0nw^rh3ERrd_d6QW z3Ut@6QL`DJACz&9e;jfcT74@(GmXlxnB$3Y{PVk9bqBoa_m0c4Nk{EiPSoNpg)m>M zKCh)&%Y2e`>-ny3Uqt28r>EXswX{KO@mo}wgvb4W!-J|PoPljJ> zUKt8*RJ@GfmFie7de&(9IOYXbl<-WLhla%=1FKFddsD*R^ZfzC$qFM}R(q^K!YHoR z@0|Ib*UU~@R%K)doK71zN&#MsbNWH&p!v7@bnS{qh2RLOTY%J1YyM=bFSw%UyG+Kdyp3e`rq9 zmkJfQA$1c2&p=_3*(-=i?Gq_Wf&v4h=m_ha$8|Bw;12^jdpt%v(fDSDeYe|9j2?y4 zVqo3EbA0|NhSqaT<}WMu+e_=9E2aE-zvH( zcHJno#D%!i>h?`0BQ>VPdEA7cHT})!T#0sMS?xQSclXkCxlVxuneedMOd{>eFkyR4 zuS;S>tsRWV0dsBFL|-X_16ttgB@c6tMPXeZzWVGf0>v1`Fj#J~F4Ih)2E#@&oR34N zSuo^_`R(oXcVP_^dn2UzJ;gceBvKFZzh;fga)tG>pP$Ax*mjb`v6Q{l7cI>a^T3qO z!lbU{TCO@vveB05kT;XME_-BH6BZUSvRLG6e9wx{@$y(%i|XwFtee36xA^A=2h4Yx z1ag+`UB#{v1vU+eTjEtu>7~=fP55j?EH+V4_bpuSabDY;Ui@tKP~w0oTvlB}c4&FQ zKqt+@uCA`1Vj2lx+ejP8CMSKzJq1LEZ7Ti- ItU8_if8pd9>Hq)$ diff --git a/camera_calibration_fisheye/doc/conf.py b/camera_calibration_fisheye/doc/conf.py deleted file mode 100644 index 82ad7fe0b..000000000 --- a/camera_calibration_fisheye/doc/conf.py +++ /dev/null @@ -1,201 +0,0 @@ -# -*- coding: utf-8 -*- -# -# camera_calibration documentation build configuration file, created by -# sphinx-quickstart on Mon Jun 1 14:21:53 2009. -# -# This file is execfile()d with the current directory set to its containing dir. -# -# Note that not all possible configuration values are present in this -# autogenerated file. -# -# All configuration values have a default; values that are commented out -# serve to show the default. - -import sys, os - -# If extensions (or modules to document with autodoc) are in another directory, -# add these directories to sys.path here. If the directory is relative to the -# documentation root, use os.path.abspath to make it absolute, like shown here. -#sys.path.append(os.path.abspath('.')) - -# -- General configuration ----------------------------------------------------- - -# Add any Sphinx extension module names here, as strings. They can be extensions -# coming with Sphinx (named 'sphinx.ext.*') or your custom ones. -extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.pngmath'] - -# Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] - -# The suffix of source filenames. -source_suffix = '.rst' - -# The encoding of source files. -#source_encoding = 'utf-8' - -# The master toctree document. -master_doc = 'index' - -# General information about the project. -project = 'camera_calibration' -copyright = '2009, Willow Garage, Inc.' - -# The version info for the project you're documenting, acts as replacement for -# |version| and |release|, also used in various other places throughout the -# built documents. -# -# The short X.Y version. -version = '0.1' -# The full version, including alpha/beta/rc tags. -release = '0.1.0' - -# The language for content autogenerated by Sphinx. Refer to documentation -# for a list of supported languages. -#language = None - -# There are two options for replacing |today|: either, you set today to some -# non-false value, then it is used: -#today = '' -# Else, today_fmt is used as the format for a strftime call. -#today_fmt = '%B %d, %Y' - -# List of documents that shouldn't be included in the build. -#unused_docs = [] - -# List of directories, relative to source directory, that shouldn't be searched -# for source files. -exclude_trees = ['_build'] - -# The reST default role (used for this markup: `text`) to use for all documents. -#default_role = None - -# If true, '()' will be appended to :func: etc. cross-reference text. -#add_function_parentheses = True - -# If true, the current module name will be prepended to all description -# unit titles (such as .. function::). -#add_module_names = True - -# If true, sectionauthor and moduleauthor directives will be shown in the -# output. They are ignored by default. -#show_authors = False - -# The name of the Pygments (syntax highlighting) style to use. -pygments_style = 'sphinx' - -# A list of ignored prefixes for module index sorting. -#modindex_common_prefix = [] - - -# -- Options for HTML output --------------------------------------------------- - -# The theme to use for HTML and HTML Help pages. Major themes that come with -# Sphinx are currently 'default' and 'sphinxdoc'. -html_theme = 'default' - -# Theme options are theme-specific and customize the look and feel of a theme -# further. For a list of options available for each theme, see the -# documentation. -#html_theme_options = {} - -# Add any paths that contain custom themes here, relative to this directory. -#html_theme_path = [] - -# The name for this set of Sphinx documents. If None, it defaults to -# " v documentation". -#html_title = None - -# A shorter title for the navigation bar. Default is the same as html_title. -#html_short_title = None - -# The name of an image file (relative to this directory) to place at the top -# of the sidebar. -#html_logo = None - -# The name of an image file (within the static path) to use as favicon of the -# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 -# pixels large. -#html_favicon = None - -# Add any paths that contain custom static files (such as style sheets) here, -# relative to this directory. They are copied after the builtin static files, -# so a file named "default.css" will overwrite the builtin "default.css". -#html_static_path = ['_static'] - -# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, -# using the given strftime format. -#html_last_updated_fmt = '%b %d, %Y' - -# If true, SmartyPants will be used to convert quotes and dashes to -# typographically correct entities. -#html_use_smartypants = True - -# Custom sidebar templates, maps document names to template names. -#html_sidebars = {} - -# Additional templates that should be rendered to pages, maps page names to -# template names. -#html_additional_pages = {} - -# If false, no module index is generated. -#html_use_modindex = True - -# If false, no index is generated. -#html_use_index = True - -# If true, the index is split into individual pages for each letter. -#html_split_index = False - -# If true, links to the reST sources are added to the pages. -#html_show_sourcelink = True - -# If true, an OpenSearch description file will be output, and all pages will -# contain a tag referring to it. The value of this option must be the -# base URL from which the finished HTML is served. -#html_use_opensearch = '' - -# If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). -#html_file_suffix = '' - -# Output file base name for HTML help builder. -htmlhelp_basename = 'camera_calibrationdoc' - - -# -- Options for LaTeX output -------------------------------------------------- - -# The paper size ('letter' or 'a4'). -#latex_paper_size = 'letter' - -# The font size ('10pt', '11pt' or '12pt'). -#latex_font_size = '10pt' - -# Grouping the document tree into LaTeX files. List of tuples -# (source start file, target name, title, author, documentclass [howto/manual]). -latex_documents = [ - ('index', 'camera_calibration.tex', 'stereo\\_utils Documentation', - 'James Bowman', 'manual'), -] - -# The name of an image file (relative to this directory) to place at the top of -# the title page. -#latex_logo = None - -# For "manual" documents, if this is true, then toplevel headings are parts, -# not chapters. -#latex_use_parts = False - -# Additional stuff for the LaTeX preamble. -#latex_preamble = '' - -# Documents to append as an appendix to all manuals. -#latex_appendices = [] - -# If false, no module index is generated. -#latex_use_modindex = True - -# Example configuration for intersphinx: refer to the Python standard library. -intersphinx_mapping = { - 'http://docs.python.org/': None, - 'http://docs.scipy.org/doc/numpy' : None, - 'http://www.ros.org/doc/api/tf/html/python/' : None - } diff --git a/camera_calibration_fisheye/doc/index.rst b/camera_calibration_fisheye/doc/index.rst deleted file mode 100644 index 741e4e5b7..000000000 --- a/camera_calibration_fisheye/doc/index.rst +++ /dev/null @@ -1,18 +0,0 @@ -camera_calibration -================== - -The camera_calibration package contains a user-friendly calibration tool, -cameracalibrator. This tool uses the following Python classes, which -conveniently hide some of the complexities of using OpenCV's calibration -process and chessboard detection, and the details of constructing a ROS -CameraInfo message. These classes are documented here for people who -need to extend or make a new calibration tool. - -For details on the camera model and camera calibration process, see -http://docs.opencv.org/master/d9/d0c/group__calib3d.html - -.. autoclass:: camera_calibration.calibrator.MonoCalibrator - :members: - -.. autoclass:: camera_calibration.calibrator.StereoCalibrator - :members: diff --git a/camera_calibration_fisheye/fiseye_cal_reference/mono/calibrate.py b/camera_calibration_fisheye/fiseye_cal_reference/mono/calibrate.py deleted file mode 100644 index c2d641b0c..000000000 --- a/camera_calibration_fisheye/fiseye_cal_reference/mono/calibrate.py +++ /dev/null @@ -1,95 +0,0 @@ -import sys -import cv2 -import numpy as np -import os -from os import listdir - -def lryaml( name, d, k, r, p): # camera_name, distortion, intrinsics, R, P ... name, d, k, r, p - calmessage = ("" - + "image_width: " + str( size[0]) + "\n" - + "image_height: " + str( size[1]) + "\n" - + "camera_name: " + name + "\n" - + "camera_matrix:\n" - + " rows: 3\n" - + " cols: 3\n" - + " data: [" + ", ".join(["%8f" % i for i in k.reshape(1,9)[0]]) + "]\n" - + "distortion_model: " + ("rational_polynomial" if d.size > 5 else "plumb_bob") + "\n" - + "distortion_coefficients:\n" - + " rows: 1\n" - + " cols: 5\n" - + " data: [" + ", ".join(["%8f" % d[i,0] for i in range(d.shape[0])]) + "]\n" - + "rectification_matrix:\n" - + " rows: 3\n" - + " cols: 3\n" - + " data: [" + ", ".join(["%8f" % i for i in r.reshape(1,9)[0]]) + "]\n" - + "projection_matrix:\n" - + " rows: 3\n" - + " cols: 4\n" - + " data: [" + ", ".join(["%8f" % i for i in p.reshape(1,12)[0]]) + "]\n" - + "") - return calmessage - - -CHECKERBOARD = (6,8) -subpix_criteria = (cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1) -calibration_flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC+cv2.fisheye.CALIB_CHECK_COND+cv2.fisheye.CALIB_FIX_SKEW -objp = np.zeros((1, CHECKERBOARD[0]*CHECKERBOARD[1], 3), np.float32) -objp[0,:,:2] = np.mgrid[0:CHECKERBOARD[0], 0:CHECKERBOARD[1]].T.reshape(-1, 2) - -objpoints = [] # 3d point in real world space -imgpoints = [] # 2d points in image plane. - -origin_dir = 'calibrationdata/' - -images = os.listdir(origin_dir) - -_img_shape = None -for fname in images: - if fname.endswith(".png"): - img = cv2.imread(origin_dir+fname) - if _img_shape == None: - _img_shape = img.shape[:2] - else: - assert _img_shape == img.shape[:2], "All images must share the same size." - - gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) - # Find the chess board corners - ret, corners = cv2.findChessboardCorners(gray, CHECKERBOARD, cv2.CALIB_CB_ADAPTIVE_THRESH+cv2.CALIB_CB_FAST_CHECK+cv2.CALIB_CB_NORMALIZE_IMAGE) - - # If found, add object points, image points (after refining them) - if ret == True: - objpoints.append(objp) - cv2.cornerSubPix(gray,corners,(3,3),(-1,-1),subpix_criteria) - imgpoints.append(corners) - -N_OK = len(objpoints) -K = np.zeros((3, 3)) -D = np.zeros((4, 1)) -rvecs = [np.zeros((1, 1, 3), dtype=np.float64) for i in range(N_OK)] -tvecs = [np.zeros((1, 1, 3), dtype=np.float64) for i in range(N_OK)] - -rms, _, _, _, _ = cv2.fisheye.calibrate( - objpoints, - imgpoints, - gray.shape[::-1], - K, - D, - rvecs, - tvecs, - calibration_flags, - (cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6) - ) - -# R is identity matrix for monocular calibration -R = np.eye(3, dtype=np.float64) -P = np.zeros((3, 4), dtype=np.float64) - -print("Found " + str(N_OK) + " valid images for calibration") -print("DIM=" + str(_img_shape[::-1])) -print("K=np.array(" + str(K.tolist()) + ")") -print("D=np.array(" + str(D.tolist()) + ")") -print("R=np.array(" + str(R) + ")") -print("T=np.array(" + str(P) + ")") - - -# Write to Yaml diff --git a/camera_calibration_fisheye/fiseye_cal_reference/mono/test_undistort.py b/camera_calibration_fisheye/fiseye_cal_reference/mono/test_undistort.py deleted file mode 100644 index 7d4c4c54f..000000000 --- a/camera_calibration_fisheye/fiseye_cal_reference/mono/test_undistort.py +++ /dev/null @@ -1,42 +0,0 @@ -import sys -#sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages') -import cv2 -assert cv2.__version__[0] == '3', 'The fisheye module requires opencv version >= 3.0.0' -import numpy as np -import os -from os import listdir -import glob - -# You should replace these 3 lines with the output in calibration step -DIM=(800, 503) -K=np.array([[290.0609844653627, 0.0, 399.7119405014219], [0.0, 289.1594924628207, 274.1984860354366], [0.0, 0.0, 1.0]]) -D=np.array([[-0.03122721497580657], [-0.020698242172871275], [0.0035143644102214144], [0.0006258984511625903]]) - -# DIM=(1920, 1208) -# K=np.array([[696.0621148872598, 0.0, 947.6971496853826], [0.0, 696.9356605571118, 651.8101521495024], [0.0, 0.0, 1.0]]) -# D=np.array([[-0.02814215088649154], [-0.03164955633645717], [0.016420714751802366], [-0.003441866971663222]]) - -methods = [("area", cv2.INTER_AREA), - ("nearest", cv2.INTER_NEAREST), - ("linear", cv2.INTER_LINEAR), - ("cubic", cv2.INTER_CUBIC), - ("lanczos4", cv2.INTER_LANCZOS4)] - -def undistort(img_path): - img = cv2.imread(img_path) - h,w = img.shape[:2] - - map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM, cv2.CV_16SC2) - undistorted_img = [] - for i,m in enumerate(methods): - print "interpolation ", m[0] - undistorted_img = cv2.remap(img, map1, map2, interpolation=m[1], borderMode=cv2.BORDER_CONSTANT) - img = np.hstack((img, undistorted_img)) - print img.shape - - cv2.imshow("undistorted", img) - cv2.waitKey(0) - cv2.destroyAllWindows() -if __name__ == '__main__': - for p in sys.argv[1:]: - undistort(p) diff --git a/camera_calibration_fisheye/fiseye_cal_reference/mono/test_undistorted_full.py b/camera_calibration_fisheye/fiseye_cal_reference/mono/test_undistorted_full.py deleted file mode 100644 index 8287e98d1..000000000 --- a/camera_calibration_fisheye/fiseye_cal_reference/mono/test_undistorted_full.py +++ /dev/null @@ -1,54 +0,0 @@ -import sys -#sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages') -import cv2 -assert cv2.__version__[0] == '3', 'The fisheye module requires opencv version >= 3.0.0' -import numpy as np -import os -from os import listdir -import glob - -# You should replace these 3 lines with the output in calibration step -# DIM=(800, 503) -# K=np.array([[290.0609844653627, 0.0, 399.7119405014219], [0.0, 289.1594924628207, 274.1984860354366], [0.0, 0.0, 1.0]]) -# D=np.array([[-0.03122721497580657], [-0.020698242172871275], [0.0035143644102214144], [0.0006258984511625903]]) - -DIM=(1920, 1208) -K=np.array([[696.0621148872598, 0.0, 947.6971496853826], [0.0, 696.9356605571118, 651.8101521495024], [0.0, 0.0, 1.0]]) -D=np.array([[-0.02814215088649154], [-0.03164955633645717], [0.016420714751802366], [-0.003441866971663222]]) - -def undistort(img_path, balance=0.0, dim2=None, dim3=None): - img = cv2.imread(img_path) - dim1 = img.shape[:2][::-1] #dim1 is the dimension of input image to un-distort - assert dim1[0]/dim1[1] == DIM[0]/DIM[1], "Image to undistort needs to have same aspect ratio as the ones used in calibration" - if not dim2: - dim2 = dim1 - - if not dim3: - dim3 = dim1 - - scaled_K = K * dim1[0] / DIM[0] # The values of K is to scale with image dimension. - scaled_K[2][2] = 1.0 # Except that K[2][2] is always 1.0 - # This is how scaled_K, dim2 and balance are used to determine the final K used to un-distort image. OpenCV document failed to make this clear! - print "dim1 ",dim1, "dim2 ",dim2, "dim3 ",dim3 - new_K = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(scaled_K, D, dim2, np.eye(3), balance=1.0) - new_K[0][2] = int(dim2[0]/2) - 0# Set manually the center of the img in New Camera matrix. X axis - new_K[1][2] = int(dim2[1]/2) + 0 # Y-axis - - ###### - map1, map2 = cv2.fisheye.initUndistortRectifyMap(scaled_K, D, np.eye(3), scaled_K, dim3, cv2.CV_16SC2) - #print new_K - undistorted_img = cv2.remap(img, map1, map2, interpolation=cv2.INTER_CUBIC, borderMode=cv2.BORDER_CONSTANT) - - ## - ##undistorted_img = cv2.resize(undistorted_img,(800, 503),interpolation=cv2.INTER_CUBIC) - - cv2.imshow("undistorted", undistorted_img) - cv2.waitKey(0) - cv2.destroyAllWindows() -if __name__ == '__main__': - if len(sys.argv)==2: - undistort( sys.argv[1]) - elif len(sys.argv)==3: - undistort( sys.argv[1],dim2=tuple( map(int, sys.argv[2].split(',') ) ) ) - elif len(sys.argv)==4: - undistort( sys.argv[1],dim2=tuple( map(int, sys.argv[2].split(',') ) ),dim3=tuple( map(int, sys.argv[3].split(',') ) ) ) \ No newline at end of file diff --git a/camera_calibration_fisheye/fiseye_cal_reference/stereo/CMakeLists.txt b/camera_calibration_fisheye/fiseye_cal_reference/stereo/CMakeLists.txt deleted file mode 100644 index ffc67b7cb..000000000 --- a/camera_calibration_fisheye/fiseye_cal_reference/stereo/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -cmake_minimum_required(VERSION 2.8.11) -project(FISHEYE_STEREO) - -set(CMAKE_INCLUDE_CURRENT_DIR ON) -set(CMAKE_CXX_FLAGS "-O3 -Wall -Wextra") -set(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -Wextra") - -find_package(OpenCV 3.4 EXACT REQUIRED PATHS /usr/local/) -include_directories($(OpenCV_INCLUDE_DIRS)) - -add_executable(calibrate calibrate.cpp) -target_link_libraries(calibrate ${OpenCV_LIBS} "-lpopt") \ No newline at end of file diff --git a/camera_calibration_fisheye/fiseye_cal_reference/stereo/README.md b/camera_calibration_fisheye/fiseye_cal_reference/stereo/README.md deleted file mode 100644 index 026a1e588..000000000 --- a/camera_calibration_fisheye/fiseye_cal_reference/stereo/README.md +++ /dev/null @@ -1,38 +0,0 @@ -## OpenCV C++ Stereo Fisheye Calibration - -This contains a source file to calibrate a stereo system comprising of fisheye lenses. It calibrates the extrinsics and the intrinsics of the cameras without any initial guesses. If you are looking for stereo calibration with lenses which follow the pinhole model check [here](https://github.com/sourishg/stereo_calibration). - -### Dependencies - -- OpenCV -- popt - -### Compilation - -Compile all the files using the following commands. - -```bash -mkdir build && cd build -cmake .. -make -``` - -Make sure your are in the `build` folder to run the executables. - -### Data - -Some sample calibration images are stored in the `imgs` folder. - -### Running calibration - -Run the executable with the following command - -```bash -./calibrate -w [board_width] -h [board_height] -s [square_size] -n [num_imgs] -d [img_dir] -l [left_img_prefix] -r [right_img_prefix] -o [calib_file] -``` - -For example if you use the images in the `imgs` folder run the following command - -```bash -./calibrate -w 9 -h 6 -s 0.02423 -n 29 -d ../imgs/ -l left -r right -o cam_stereo.yml -``` \ No newline at end of file diff --git a/camera_calibration_fisheye/fiseye_cal_reference/stereo/calibrate.cpp b/camera_calibration_fisheye/fiseye_cal_reference/stereo/calibrate.cpp deleted file mode 100644 index 4a050aef3..000000000 --- a/camera_calibration_fisheye/fiseye_cal_reference/stereo/calibrate.cpp +++ /dev/null @@ -1,142 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include "popt_pp.h" - -using namespace std; -using namespace cv; - -vector< vector< Point3d > > object_points; -vector< vector< Point2f > > imagePoints1, imagePoints2; -vector< Point2f > corners1, corners2; -vector< vector< Point2d > > left_img_points, right_img_points; - -Mat img1, img2, gray1, gray2, spl1, spl2; - -void load_image_points(int board_width, int board_height, float square_size, int num_imgs, - char* img_dir, char* leftimg_filename, char* rightimg_filename) { - Size board_size = Size(board_width, board_height); - int board_n = board_width * board_height; - - for (int i = 1; i <= num_imgs; i++) { - char left_img[100], right_img[100]; - sprintf(left_img, "%s%s%02d.png", img_dir, leftimg_filename, i); - sprintf(right_img, "%s%s%02d.png", img_dir, rightimg_filename, i); - img1 = imread(left_img, CV_LOAD_IMAGE_COLOR); - img2 = imread(right_img, CV_LOAD_IMAGE_COLOR); - cv::cvtColor(img1, gray1, CV_BGR2GRAY); - cv::cvtColor(img2, gray2, CV_BGR2GRAY); - - bool found1 = false, found2 = false; - - found1 = cv::findChessboardCorners(img1, board_size, corners1, - CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS); - found2 = cv::findChessboardCorners(img2, board_size, corners2, - CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS); - - if (found1) - { - cv::cornerSubPix(gray1, corners1, cv::Size(5, 5), cv::Size(-1, -1), - cv::TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1)); - cv::drawChessboardCorners(gray1, board_size, corners1, found1); - } - if (found2) - { - cv::cornerSubPix(gray2, corners2, cv::Size(5, 5), cv::Size(-1, -1), - cv::TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1)); - cv::drawChessboardCorners(gray2, board_size, corners2, found2); - } - - vector obj; - for( int i = 0; i < board_height; ++i ) - for( int j = 0; j < board_width; ++j ) - obj.push_back(Point3d(double( (float)j * square_size ), double( (float)i * square_size ), 0)); - - if (found1 && found2) { - cout << i << ". Found corners!" << endl; - imagePoints1.push_back(corners1); - imagePoints2.push_back(corners2); - object_points.push_back(obj); - } - } - for (int i = 0; i < imagePoints1.size(); i++) { - vector< Point2d > v1, v2; - for (int j = 0; j < imagePoints1[i].size(); j++) { - v1.push_back(Point2d((double)imagePoints1[i][j].x, (double)imagePoints1[i][j].y)); - v2.push_back(Point2d((double)imagePoints2[i][j].x, (double)imagePoints2[i][j].y)); - } - left_img_points.push_back(v1); - right_img_points.push_back(v2); - } -} - -int main(int argc, char const *argv[]) -{ - int board_width, board_height, num_imgs; - float square_size; - char* img_dir; - char* leftimg_filename; - char* rightimg_filename; - char* out_file; - - static struct poptOption options[] = { - { "board_width",'w',POPT_ARG_INT,&board_width,0,"Checkerboard width","NUM" }, - { "board_height",'h',POPT_ARG_INT,&board_height,0,"Checkerboard height","NUM" }, - { "square_size",'s',POPT_ARG_FLOAT,&square_size,0,"Checkerboard square size","NUM" }, - { "num_imgs",'n',POPT_ARG_INT,&num_imgs,0,"Number of checkerboard images","NUM" }, - { "img_dir",'d',POPT_ARG_STRING,&img_dir,0,"Directory containing images","STR" }, - { "leftimg_filename",'l',POPT_ARG_STRING,&leftimg_filename,0,"Left image prefix","STR" }, - { "rightimg_filename",'r',POPT_ARG_STRING,&rightimg_filename,0,"Right image prefix","STR" }, - { "out_file",'o',POPT_ARG_STRING,&out_file,0,"Output calibration filename (YML)","STR" }, - POPT_AUTOHELP - { NULL, 0, 0, NULL, 0, NULL, NULL } - }; - - POpt popt(NULL, argc, argv, options, 0); - int c; - while((c = popt.getNextOpt()) >= 0) {} - - load_image_points(board_width, board_height, square_size, num_imgs, img_dir, leftimg_filename, rightimg_filename); - - printf("Starting Calibration\n"); - cv::Matx33d K1, K2, R; - cv::Vec3d T; - cv::Vec4d D1, D2; - int flag = 0; - flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC; - flag |= cv::fisheye::CALIB_CHECK_COND; - flag |= cv::fisheye::CALIB_FIX_SKEW; - //flag |= cv::fisheye::CALIB_FIX_K2; - //flag |= cv::fisheye::CALIB_FIX_K3; - //flag |= cv::fisheye::CALIB_FIX_K4; - cv::fisheye::stereoCalibrate(object_points, left_img_points, right_img_points, - K1, D1, K2, D2, img1.size(), R, T, flag, - cv::TermCriteria(3, 12, 0)); - - cv::FileStorage fs1(out_file, cv::FileStorage::WRITE); - fs1 << "K1" << Mat(K1); - fs1 << "K2" << Mat(K2); - fs1 << "D1" << D1; - fs1 << "D2" << D2; - fs1 << "R" << Mat(R); - fs1 << "T" << T; - printf("Done Calibration\n"); - - printf("Starting Rectification\n"); - - cv::Mat R1, R2, P1, P2, Q; - cv::fisheye::stereoRectify(K1, D1, K2, D2, img1.size(), R, T, R1, R2, P1, P2, -Q, CV_CALIB_ZERO_DISPARITY, img1.size(), 0.0, 1.1); - - fs1 << "R1" << R1; - fs1 << "R2" << R2; - fs1 << "P1" << P1; - fs1 << "P2" << P2; - fs1 << "Q" << Q; - - printf("Done Rectification\n"); - return 0; -} diff --git a/camera_calibration_fisheye/fiseye_cal_reference/stereo/popt_pp.h b/camera_calibration_fisheye/fiseye_cal_reference/stereo/popt_pp.h deleted file mode 100644 index 61c5bdb82..000000000 --- a/camera_calibration_fisheye/fiseye_cal_reference/stereo/popt_pp.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef _INCLUDED_POPT_PP_H_ -#define _INCLUDED_POPT_PP_H_ - -#include - -class POpt{ -protected: - poptContext con; -public: - // creation and deletion - POpt(const char *name, int argc, const char **argv, - const poptOption *options, int flags) - {con = poptGetContext(name,argc,argv,options,flags);} - POpt(const char *name, int argc, char **argv, - const poptOption *options, int flags) - {con = poptGetContext(name,argc,(const char **)argv,options,flags);} - ~POpt() - {poptFreeContext(con);} - - // functions for processing options - int getNextOpt() - {return(poptGetNextOpt(con));} - void ignoreOptions() - {while(getNextOpt() >= 0);} - const char *getOptArg() - {return(poptGetOptArg(con));} - const char *strError(int error) - {return(poptStrerror(error));} - const char *badOption(int flags = POPT_BADOPTION_NOALIAS) - {return(poptBadOption(con,flags));} - - // processing other arguments - const char *getArg() - {return(poptGetArg(con));} - void ignoreArgs() - {while(getArg());} -}; - -#endif diff --git a/camera_calibration_fisheye/mainpage.dox b/camera_calibration_fisheye/mainpage.dox deleted file mode 100644 index dbcfe690e..000000000 --- a/camera_calibration_fisheye/mainpage.dox +++ /dev/null @@ -1,59 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b The camera_calibration package contains tools for calibrating monocular and stereo cameras. - -\section codeapi Code API - -camera_calibration does not have a code API. - -\section rosapi ROS API - -List of nodes: -- \b calibrationnode - - - -
- -\subsection node_name calibrationnode - -calibrationnode subscribes to ROS raw image topics, and presents a -calibration window. It can run in both monocular and stereo modes. -The calibration window shows the current images from the cameras, -highlighting the checkerboard. When the user presses the "CALIBRATE" -button, the node computes the camera calibration parameters. When the -user clicks "UPLOAD", the node uploads these new calibration parameters -to the camera driver using a service call. - -\subsubsection Usage -\verbatim -$ node_type1 [standard ROS args] -\endverbatim - -\par Example - -\verbatim -$ rosrun camera_calibration cal.py right:=/my_stereo/right/image_raw left:=/my_stereo/left/image_raw left_camera:=/my_stereo/left right_camera:=/my_stereo/right -\endverbatim - - -\subsubsection topics ROS topics - -Subscribes to: -- \b "left": [sensor_msgs/Image] left raw image topic, for stereo cameras -- \b "right": [sensor_msgs/Image] left raw image topic, for stereo cameras -- \b "image": [sensor_msgs/Image] raw image topic, for monocular cameras - -Makes service calls to: - -\subsubsection services ROS services -- \b "foo_service": [std_srvs/FooType] description of foo_service -- \b "camera/set_camera_info": [sensor_msgs/SetCameraInfo] node of the camera for monocular -- \b "left_camera/set_camera_info": [sensor_msgs/SetCameraInfo] node of the left stereo camera -- \b "right_camera/set_camera_info": [sensor_msgs/SetCameraInfo] node of the left stereo camera - - - -*/ diff --git a/camera_calibration_fisheye/nodes/cameracalibrator.py b/camera_calibration_fisheye/nodes/cameracalibrator.py deleted file mode 100644 index 206962e1d..000000000 --- a/camera_calibration_fisheye/nodes/cameracalibrator.py +++ /dev/null @@ -1,154 +0,0 @@ -#!/usr/bin/python -# -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the Willow Garage nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import cv2 -import functools -import message_filters -import os -import rospy -from camera_calibration.camera_calibrator import OpenCVCalibrationNode -from camera_calibration.calibrator import ChessboardInfo, Patterns -from message_filters import ApproximateTimeSynchronizer - - -def main(): - from optparse import OptionParser, OptionGroup - parser = OptionParser("%prog --size SIZE1 --square SQUARE1 [ --size SIZE2 --square SQUARE2 ]", - description=None) - parser.add_option("-c", "--camera_name", - type="string", default='narrow_stereo', - help="name of the camera to appear in the calibration file") - group = OptionGroup(parser, "Chessboard Options", - "You must specify one or more chessboards as pairs of --size and --square options.") - group.add_option("-p", "--pattern", - type="string", default="chessboard", - help="calibration pattern to detect - 'chessboard', 'circles', 'acircles'") - group.add_option("-s", "--size", - action="append", default=[], - help="chessboard size as NxM, counting interior corners (e.g. a standard chessboard is 7x7)") - group.add_option("-q", "--square", - action="append", default=[], - help="chessboard square size in meters") - parser.add_option_group(group) - group = OptionGroup(parser, "ROS Communication Options") - group.add_option("--approximate", - type="float", default=0.0, - help="allow specified slop (in seconds) when pairing images from unsynchronized stereo cameras") - group.add_option("--no-service-check", - action="store_false", dest="service_check", default=True, - help="disable check for set_camera_info services at startup") - parser.add_option_group(group) - group = OptionGroup(parser, "Calibration Optimizer Options") - group.add_option("--fix-principal-point", - action="store_true", default=False, - help="fix the principal point at the image center") - group.add_option("--fix-aspect-ratio", - action="store_true", default=False, - help="enforce focal lengths (fx, fy) are equal") - group.add_option("--zero-tangent-dist", - action="store_true", default=False, - help="set tangential distortion coefficients (p1, p2) to zero") - group.add_option("-k", "--k-coefficients", - type="int", default=2, metavar="NUM_COEFFS", - help="number of radial distortion coefficients to use (up to 6, default %default)") - group.add_option("--disable_calib_cb_fast_check", action='store_true', default=False, - help="uses the CALIB_CB_FAST_CHECK flag for findChessboardCorners") - parser.add_option_group(group) - options, args = parser.parse_args() - - if len(options.size) != len(options.square): - parser.error("Number of size and square inputs must be the same!") - - if not options.square: - options.square.append("0.108") - options.size.append("8x6") - - boards = [] - for (sz, sq) in zip(options.size, options.square): - size = tuple([int(c) for c in sz.split('x')]) - boards.append(ChessboardInfo(size[0], size[1], float(sq))) - - if options.approximate == 0.0: - sync = message_filters.TimeSynchronizer - else: - sync = functools.partial(ApproximateTimeSynchronizer, slop=options.approximate) - - num_ks = options.k_coefficients - - calib_flags = 0 - if options.fix_principal_point: - calib_flags |= cv2.CALIB_FIX_PRINCIPAL_POINT - if options.fix_aspect_ratio: - calib_flags |= cv2.CALIB_FIX_ASPECT_RATIO - if options.zero_tangent_dist: - calib_flags |= cv2.CALIB_ZERO_TANGENT_DIST - if (num_ks > 3): - calib_flags |= cv2.CALIB_RATIONAL_MODEL - if (num_ks < 6): - calib_flags |= cv2.CALIB_FIX_K6 - if (num_ks < 5): - calib_flags |= cv2.CALIB_FIX_K5 - if (num_ks < 4): - calib_flags |= cv2.CALIB_FIX_K4 - if (num_ks < 3): - calib_flags |= cv2.CALIB_FIX_K3 - if (num_ks < 2): - calib_flags |= cv2.CALIB_FIX_K2 - if (num_ks < 1): - calib_flags |= cv2.CALIB_FIX_K1 - - pattern = Patterns.Chessboard - if options.pattern == 'circles': - pattern = Patterns.Circles - elif options.pattern == 'acircles': - pattern = Patterns.ACircles - elif options.pattern != 'chessboard': - print('Unrecognized pattern %s, defaulting to chessboard' % options.pattern) - - if options.disable_calib_cb_fast_check: - checkerboard_flags = 0 - else: - checkerboard_flags = cv2.CALIB_CB_FAST_CHECK - - rospy.init_node('cameracalibrator') - node = OpenCVCalibrationNode(boards, options.service_check, sync, calib_flags, pattern, options.camera_name, - checkerboard_flags=checkerboard_flags) - rospy.spin() - -if __name__ == "__main__": - try: - main() - except Exception as e: - import traceback - traceback.print_exc() diff --git a/camera_calibration_fisheye/nodes/cameracheck.py b/camera_calibration_fisheye/nodes/cameracheck.py deleted file mode 100644 index 6af00d621..000000000 --- a/camera_calibration_fisheye/nodes/cameracheck.py +++ /dev/null @@ -1,57 +0,0 @@ -#!/usr/bin/python -# -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the Willow Garage nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import rospy -from camera_calibration.camera_checker import CameraCheckerNode - - -def main(): - from optparse import OptionParser - rospy.init_node('cameracheck') - parser = OptionParser() - parser.add_option("-s", "--size", default="8x6", help="specify chessboard size as nxm [default: %default]") - parser.add_option("-q", "--square", default=".108", help="specify chessboard square size in meters [default: %default]") - parser.add_option("--approximate", - type="float", default=0.0, - help="allow specified slop (in seconds) when pairing images from unsynchronized stereo cameras") - - options, args = parser.parse_args() - size = tuple([int(c) for c in options.size.split('x')]) - dim = float(options.square) - approximate = float(options.approximate) - CameraCheckerNode(size, dim, approximate) - rospy.spin() - -if __name__ == "__main__": - main() diff --git a/camera_calibration_fisheye/package.xml b/camera_calibration_fisheye/package.xml deleted file mode 100644 index 971dbf2f6..000000000 --- a/camera_calibration_fisheye/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - camera_calibration_fisheye - 1.12.23 - - camera_calibration allows easy calibration of monocular or stereo - cameras using a checkerboard calibration target. - - James Bowman - Patrick Mihelich - David Torres - David Torres - - catkin - - rostest - - cv_bridge - image_geometry - message_filters - rospy - std_srvs - sensor_msgs - - BSD - http://www.ros.org/wiki/camera_calibration - - - - - - diff --git a/camera_calibration_fisheye/rosdoc.yaml b/camera_calibration_fisheye/rosdoc.yaml deleted file mode 100644 index 0136cd7a2..000000000 --- a/camera_calibration_fisheye/rosdoc.yaml +++ /dev/null @@ -1,4 +0,0 @@ - - builder: sphinx - name: Python API - output_dir: python - sphinx_root_dir: doc diff --git a/camera_calibration_fisheye/scripts/tarfile_calibration.py b/camera_calibration_fisheye/scripts/tarfile_calibration.py deleted file mode 100644 index 5d552ac5f..000000000 --- a/camera_calibration_fisheye/scripts/tarfile_calibration.py +++ /dev/null @@ -1,235 +0,0 @@ -#!/usr/bin/env python -# -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the Willow Garage nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import sys -import numpy - -import cv2 -import cv_bridge -import tarfile - -from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, CalibrationException, ChessboardInfo - -import rospy -import sensor_msgs.srv - -def waitkey(): - k = cv2.waitKey(6) - return k - -def display(win_name, img): - cv2.namedWindow(win_name, cv2.WINDOW_NORMAL) - cv2.imshow( win_name, numpy.asarray( img[:,:] )) - k=-1 - while k ==-1: - k=waitkey() - cv2.destroyWindow(win_name) - if k in [27, ord('q')]: - rospy.signal_shutdown('Quit') - - -def cal_from_tarfile(boards, tarname, mono = False, upload = False, calib_flags = 0, visualize = False, alpha=1.0): - if mono: - calibrator = MonoCalibrator(boards, calib_flags) - else: - calibrator = StereoCalibrator(boards, calib_flags) - - calibrator.do_tarfile_calibration(tarname) - - print(calibrator.ost()) - - if upload: - info = calibrator.as_message() - if mono: - set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo) - - response = set_camera_info_service(info) - if not response.success: - raise RuntimeError("connected to set_camera_info service, but failed setting camera_info") - else: - set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo) - set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo) - - response1 = set_left_camera_info_service(info[0]) - response2 = set_right_camera_info_service(info[1]) - if not (response1.success and response2.success): - raise RuntimeError("connected to set_camera_info service, but failed setting camera_info") - - if visualize: - - #Show rectified images - calibrator.set_alpha(alpha) - - archive = tarfile.open(tarname, 'r') - if mono: - for f in archive.getnames(): - if f.startswith('left') and (f.endswith('.pgm') or f.endswith('png')): - filedata = archive.extractfile(f).read() - file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8) - im=cv2.imdecode(file_bytes,cv2.IMREAD_COLOR) - - bridge = cv_bridge.CvBridge() - try: - msg=bridge.cv2_to_imgmsg(im, "bgr8") - except cv_bridge.CvBridgeError as e: - print(e) - - #handle msg returns the recitifed image with corner detection once camera is calibrated. - drawable=calibrator.handle_msg(msg) - vis=numpy.asarray( drawable.scrib[:,:]) - #Display. Name of window:f - display(f, vis) - else: - limages = [ f for f in archive.getnames() if (f.startswith('left') and (f.endswith('pgm') or f.endswith('png'))) ] - limages.sort() - rimages = [ f for f in archive.getnames() if (f.startswith('right') and (f.endswith('pgm') or f.endswith('png'))) ] - rimages.sort() - - if not len(limages) == len(rimages): - raise RuntimeError("Left, right images don't match. %d left images, %d right" % (len(limages), len(rimages))) - - for i in range(len(limages)): - l=limages[i] - r=rimages[i] - - if l.startswith('left') and (l.endswith('.pgm') or l.endswith('png')) and r.startswith('right') and (r.endswith('.pgm') or r.endswith('png')): - # LEFT IMAGE - filedata = archive.extractfile(l).read() - file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8) - im_left=cv2.imdecode(file_bytes,cv2.IMREAD_COLOR) - - bridge = cv_bridge.CvBridge() - try: - msg_left=bridge.cv2_to_imgmsg(im_left, "bgr8") - except cv_bridge.CvBridgeError as e: - print(e) - - #RIGHT IMAGE - filedata = archive.extractfile(r).read() - file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8) - im_right=cv2.imdecode(file_bytes,cv2.IMREAD_COLOR) - try: - msg_right=bridge.cv2_to_imgmsg(im_right, "bgr8") - except cv_bridge.CvBridgeError as e: - print(e) - - drawable=calibrator.handle_msg([ msg_left,msg_right] ) - - h, w = numpy.asarray(drawable.lscrib[:,:]).shape[:2] - vis = numpy.zeros((h, w*2, 3), numpy.uint8) - vis[:h, :w ,:] = numpy.asarray(drawable.lscrib[:,:]) - vis[:h, w:w*2, :] = numpy.asarray(drawable.rscrib[:,:]) - - display(l+" "+r,vis) - - -if __name__ == '__main__': - from optparse import OptionParser - parser = OptionParser("%prog TARFILE [ opts ]") - parser.add_option("--mono", default=False, action="store_true", dest="mono", - help="Monocular calibration only. Calibrates left images.") - parser.add_option("-s", "--size", default=[], action="append", dest="size", - help="specify chessboard size as NxM [default: 8x6]") - parser.add_option("-q", "--square", default=[], action="append", dest="square", - help="specify chessboard square size in meters [default: 0.108]") - parser.add_option("--upload", default=False, action="store_true", dest="upload", - help="Upload results to camera(s).") - parser.add_option("--fix-principal-point", action="store_true", default=False, - help="fix the principal point at the image center") - parser.add_option("--fix-aspect-ratio", action="store_true", default=False, - help="enforce focal lengths (fx, fy) are equal") - parser.add_option("--zero-tangent-dist", action="store_true", default=False, - help="set tangential distortion coefficients (p1, p2) to zero") - parser.add_option("-k", "--k-coefficients", type="int", default=2, metavar="NUM_COEFFS", - help="number of radial distortion coefficients to use (up to 6, default %default)") - parser.add_option("--visualize", action="store_true", default=False, - help="visualize rectified images after calibration") - parser.add_option("-a", "--alpha", type="float", default=1.0, metavar="ALPHA", - help="zoom for visualization of rectifies images. Ranges from 0 (zoomed in, all pixels in calibrated image are valid) to 1 (zoomed out, all pixels in original image are in calibrated image). default %default)") - - options, args = parser.parse_args() - - if len(options.size) != len(options.square): - parser.error("Number of size and square inputs must be the same!") - - if not options.square: - options.square.append("0.108") - options.size.append("8x6") - - boards = [] - for (sz, sq) in zip(options.size, options.square): - info = ChessboardInfo() - info.dim = float(sq) - size = tuple([int(c) for c in sz.split('x')]) - info.n_cols = size[0] - info.n_rows = size[1] - - boards.append(info) - - if not boards: - parser.error("Must supply at least one chessboard") - - if not args: - parser.error("Must give tarfile name") - if not os.path.exists(args[0]): - parser.error("Tarfile %s does not exist. Please select valid tarfile" % args[0]) - - tarname = args[0] - - num_ks = options.k_coefficients - - calib_flags = 0 - if options.fix_principal_point: - calib_flags |= cv2.CALIB_FIX_PRINCIPAL_POINT - if options.fix_aspect_ratio: - calib_flags |= cv2.CALIB_FIX_ASPECT_RATIO - if options.zero_tangent_dist: - calib_flags |= cv2.CALIB_ZERO_TANGENT_DIST - if (num_ks > 3): - calib_flags |= cv2.CALIB_RATIONAL_MODEL - if (num_ks < 6): - calib_flags |= cv2.CALIB_FIX_K6 - if (num_ks < 5): - calib_flags |= cv2.CALIB_FIX_K5 - if (num_ks < 4): - calib_flags |= cv2.CALIB_FIX_K4 - if (num_ks < 3): - calib_flags |= cv2.CALIB_FIX_K3 - if (num_ks < 2): - calib_flags |= cv2.CALIB_FIX_K2 - if (num_ks < 1): - calib_flags |= cv2.CALIB_FIX_K1 - - cal_from_tarfile(boards, tarname, options.mono, options.upload, calib_flags, options.visualize, options.alpha) diff --git a/camera_calibration_fisheye/setup.py b/camera_calibration_fisheye/setup.py deleted file mode 100644 index fd51af2ee..000000000 --- a/camera_calibration_fisheye/setup.py +++ /dev/null @@ -1,9 +0,0 @@ -#!/usr/bin/env python -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup() -d['packages'] = ['camera_calibration'] -d['package_dir'] = {'':'src'} - -setup(**d) diff --git a/camera_calibration_fisheye/src/camera_calibration/__init__.py b/camera_calibration_fisheye/src/camera_calibration/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/camera_calibration_fisheye/src/camera_calibration/calibrator.py b/camera_calibration_fisheye/src/camera_calibration/calibrator.py deleted file mode 100644 index eabe4b485..000000000 --- a/camera_calibration_fisheye/src/camera_calibration/calibrator.py +++ /dev/null @@ -1,1182 +0,0 @@ -#!/usr/bin/env python -# -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the Willow Garage nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -try: - from cStringIO import StringIO -except ImportError: - from io import StringIO -from io import BytesIO -import cv2 -assert cv2.__version__[0] == '3', 'The fisheye module requires opencv version >= 3.0.0' - -import cv_bridge -import image_geometry -import math -import numpy.linalg -import pickle -import random -import sensor_msgs.msg -import tarfile -import time -from distutils.version import LooseVersion - - -# Supported calibration patterns -class Patterns: - Chessboard, Circles, ACircles = list(range(3)) - -class CalibrationException(Exception): - pass - -# TODO: Make pattern per-board? -class ChessboardInfo(object): - def __init__(self, n_cols = 0, n_rows = 0, dim = 0.0): - self.n_cols = n_cols - self.n_rows = n_rows - self.dim = dim - -# Make all private!!!!! -def lmin(seq1, seq2): - """ Pairwise minimum of two sequences """ - return [min(a, b) for (a, b) in zip(seq1, seq2)] - -def lmax(seq1, seq2): - """ Pairwise maximum of two sequences """ - return [max(a, b) for (a, b) in zip(seq1, seq2)] - -def _pdist(p1, p2): - """ - Distance bwt two points. p1 = (x, y), p2 = (x, y) - """ - return math.sqrt(math.pow(p1[0] - p2[0], 2) + math.pow(p1[1] - p2[1], 2)) - -def _get_outside_corners(corners, board): - """ - Return the four corners of the board as a whole, as (up_left, up_right, down_right, down_left). - """ - xdim = board.n_cols - ydim = board.n_rows - - if corners.shape[1] * corners.shape[0] != xdim * ydim: - raise Exception("Invalid number of corners! %d corners. X: %d, Y: %d" % (corners.shape[1] * corners.shape[0], - xdim, ydim)) - - up_left = corners[0,0] - up_right = corners[xdim - 1,0] - down_right = corners[-1,0] - down_left = corners[-xdim,0] - - return (up_left, up_right, down_right, down_left) - -def _get_skew(corners, board): - """ - Get skew for given checkerboard detection. - Scaled to [0,1], which 0 = no skew, 1 = high skew - Skew is proportional to the divergence of three outside corners from 90 degrees. - """ - # TODO Using three nearby interior corners might be more robust, outside corners occasionally - # get mis-detected - up_left, up_right, down_right, _ = _get_outside_corners(corners, board) - - def angle(a, b, c): - """ - Return angle between lines ab, bc - """ - ab = a - b - cb = c - b - return math.acos(numpy.dot(ab,cb) / (numpy.linalg.norm(ab) * numpy.linalg.norm(cb))) - - skew = min(1.0, 2. * abs((math.pi / 2.) - angle(up_left, up_right, down_right))) - return skew - -def _get_area(corners, board): - """ - Get 2d image area of the detected checkerboard. - The projected checkerboard is assumed to be a convex quadrilateral, and the area computed as - |p X q|/2; see http://mathworld.wolfram.com/Quadrilateral.html. - """ - (up_left, up_right, down_right, down_left) = _get_outside_corners(corners, board) - a = up_right - up_left - b = down_right - up_right - c = down_left - down_right - p = b + c - q = a + b - return abs(p[0]*q[1] - p[1]*q[0]) / 2. - -def _get_corners(img, board, refine = True, checkerboard_flags=0): - """ - Get corners for a particular chessboard for an image - """ - h = img.shape[0] - w = img.shape[1] - if len(img.shape) == 3 and img.shape[2] == 3: - mono = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - else: - mono = img - (ok, corners) = cv2.findChessboardCorners(mono, (board.n_cols, board.n_rows), flags = cv2.CALIB_CB_ADAPTIVE_THRESH | - cv2.CALIB_CB_NORMALIZE_IMAGE | checkerboard_flags) - if not ok: - return (ok, corners) - - # If any corners are within BORDER pixels of the screen edge, reject the detection by setting ok to false - # NOTE: This may cause problems with very low-resolution cameras, where 8 pixels is a non-negligible fraction - # of the image size. See http://answers.ros.org/question/3155/how-can-i-calibrate-low-resolution-cameras - BORDER = 8 - if not all([(BORDER < corners[i, 0, 0] < (w - BORDER)) and (BORDER < corners[i, 0, 1] < (h - BORDER)) for i in range(corners.shape[0])]): - ok = False - - # Ensure that all corner-arrays are going from top to bottom. - if board.n_rows!=board.n_cols: - if corners[0, 0, 1] > corners[-1, 0, 1]: - corners = numpy.copy(numpy.flipud(corners)) - else: - direction_corners=(corners[-1]-corners[0])>=numpy.array([[0.0,0.0]]) - - if not numpy.all(direction_corners): - if not numpy.any(direction_corners): - corners = numpy.copy(numpy.flipud(corners)) - elif direction_corners[0][0]: - corners=numpy.rot90(corners.reshape(board.n_rows,board.n_cols,2)).reshape(board.n_cols*board.n_rows,1,2) - else: - corners=numpy.rot90(corners.reshape(board.n_rows,board.n_cols,2),3).reshape(board.n_cols*board.n_rows,1,2) - - if refine and ok: - # Use a radius of half the minimum distance between corners. This should be large enough to snap to the - # correct corner, but not so large as to include a wrong corner in the search window. - min_distance = float("inf") - for row in range(board.n_rows): - for col in range(board.n_cols - 1): - index = row*board.n_rows + col - min_distance = min(min_distance, _pdist(corners[index, 0], corners[index + 1, 0])) - for row in range(board.n_rows - 1): - for col in range(board.n_cols): - index = row*board.n_rows + col - min_distance = min(min_distance, _pdist(corners[index, 0], corners[index + board.n_cols, 0])) - radius = int(math.ceil(min_distance * 0.5)) - cv2.cornerSubPix(mono, corners, (radius,radius), (-1,-1), - ( cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1 )) - - return (ok, corners) - -def _get_circles(img, board, pattern): - """ - Get circle centers for a symmetric or asymmetric grid - """ - h = img.shape[0] - w = img.shape[1] - if len(img.shape) == 3 and img.shape[2] == 3: - mono = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - else: - mono = img - - flag = cv2.CALIB_CB_SYMMETRIC_GRID - if pattern == Patterns.ACircles: - flag = cv2.CALIB_CB_ASYMMETRIC_GRID - mono_arr = numpy.array(mono) - (ok, corners) = cv2.findCirclesGrid(mono_arr, (board.n_cols, board.n_rows), flags=flag) - - # In symmetric case, findCirclesGrid does not detect the target if it's turned sideways. So we try - # again with dimensions swapped - not so efficient. - # TODO Better to add as second board? Corner ordering will change. - if not ok and pattern == Patterns.Circles: - (ok, corners) = cv2.findCirclesGrid(mono_arr, (board.n_rows, board.n_cols), flags=flag) - - return (ok, corners) - - -# TODO self.size needs to come from CameraInfo, full resolution -class Calibrator(object): - """ - Base class for calibration system - """ - def __init__(self, boards, flags=0, pattern=Patterns.Chessboard, name='', checkerboard_flags=cv2.CALIB_CB_FAST_CHECK): - # Ordering the dimensions for the different detectors is actually a minefield... - if pattern == Patterns.Chessboard: - # Make sure n_cols > n_rows to agree with OpenCV CB detector output - self._boards = [ChessboardInfo(max(i.n_cols, i.n_rows), min(i.n_cols, i.n_rows), i.dim) for i in boards] - elif pattern == Patterns.ACircles: - # 7x4 and 4x7 are actually different patterns. Assume square-ish pattern, so n_rows > n_cols. - self._boards = [ChessboardInfo(min(i.n_cols, i.n_rows), max(i.n_cols, i.n_rows), i.dim) for i in boards] - elif pattern == Patterns.Circles: - # We end up having to check both ways anyway - self._boards = boards - - # Set to true after we perform calibration - self.calibrated = False - self.calib_flags = flags - self.checkerboard_flags = checkerboard_flags - self.pattern = pattern - self.br = cv_bridge.CvBridge() - - # self.db is list of (parameters, image) samples for use in calibration. parameters has form - # (X, Y, size, skew) all normalized to [0,1], to keep track of what sort of samples we've taken - # and ensure enough variety. - self.db = [] - # For each db sample, we also record the detected corners. - self.good_corners = [] - # Set to true when we have sufficiently varied samples to calibrate - self.goodenough = False - self.param_ranges = [0.7, 0.7, 0.4, 0.5] - self.name = name - - def mkgray(self, msg): - """ - Convert a message into a 8-bit 1 channel monochrome OpenCV image - """ - # as cv_bridge automatically scales, we need to remove that behavior - # TODO: get a Python API in cv_bridge to check for the image depth. - if self.br.encoding_to_dtype_with_channels(msg.encoding)[0] in ['uint16', 'int16']: - mono16 = self.br.imgmsg_to_cv2(msg, '16UC1') - mono8 = numpy.array(numpy.clip(mono16, 0, 255), dtype=numpy.uint8) - return mono8 - elif 'FC1' in msg.encoding: - # floating point image handling - img = self.br.imgmsg_to_cv2(msg, "passthrough") - _, max_val, _, _ = cv2.minMaxLoc(img) - if max_val > 0: - scale = 255.0 / max_val - mono_img = (img * scale).astype(numpy.uint8) - else: - mono_img = img.astype(numpy.uint8) - return mono_img - else: - return self.br.imgmsg_to_cv2(msg, "mono8") - - def get_parameters(self, corners, board, size): - """ - Return list of parameters [X, Y, size, skew] describing the checkerboard view. - """ - (width, height) = size - Xs = corners[:,:,0] - Ys = corners[:,:,1] - area = _get_area(corners, board) - border = math.sqrt(area) - # For X and Y, we "shrink" the image all around by approx. half the board size. - # Otherwise large boards are penalized because you can't get much X/Y variation. - p_x = min(1.0, max(0.0, (numpy.mean(Xs) - border / 2) / (width - border))) - p_y = min(1.0, max(0.0, (numpy.mean(Ys) - border / 2) / (height - border))) - p_size = math.sqrt(area / (width * height)) - skew = _get_skew(corners, board) - params = [p_x, p_y, p_size, skew] - return params - - def is_good_sample(self, params): - """ - Returns true if the checkerboard detection described by params should be added to the database. - """ - if not self.db: - return True - - def param_distance(p1, p2): - return sum([abs(a-b) for (a,b) in zip(p1, p2)]) - - db_params = [sample[0] for sample in self.db] - d = min([param_distance(params, p) for p in db_params]) - #print "d = %.3f" % d #DEBUG - # TODO What's a good threshold here? Should it be configurable? - return d > 0.2 - - _param_names = ["X", "Y", "Size", "Skew"] - - def compute_goodenough(self): - if not self.db: - return None - - # Find range of checkerboard poses covered by samples in database - all_params = [sample[0] for sample in self.db] - min_params = all_params[0] - max_params = all_params[0] - for params in all_params[1:]: - min_params = lmin(min_params, params) - max_params = lmax(max_params, params) - # Don't reward small size or skew - min_params = [min_params[0], min_params[1], 0., 0.] - - # For each parameter, judge how much progress has been made toward adequate variation - progress = [min((hi - lo) / r, 1.0) for (lo, hi, r) in zip(min_params, max_params, self.param_ranges)] - # If we have lots of samples, allow calibration even if not all parameters are green - # TODO Awkward that we update self.goodenough instead of returning it - self.goodenough = (len(self.db) >= 40) or all([p == 1.0 for p in progress]) - - return list(zip(self._param_names, min_params, max_params, progress)) - - def mk_object_points(self, boards, use_board_size = False): - opts = [] - for i, b in enumerate(boards): - num_pts = b.n_cols * b.n_rows - opts_loc = numpy.zeros((num_pts, 1, 3), numpy.float32) - for j in range(num_pts): - opts_loc[j, 0, 0] = (j / b.n_cols) - if self.pattern == Patterns.ACircles: - opts_loc[j, 0, 1] = 2*(j % b.n_cols) + (opts_loc[j, 0, 0] % 2) - else: - opts_loc[j, 0, 1] = (j % b.n_cols) - opts_loc[j, 0, 2] = 0 - if use_board_size: - opts_loc[j, 0, :] = opts_loc[j, 0, :] * b.dim - opts.append(opts_loc) - return opts - - def get_corners(self, img, refine = True): - """ - Use cvFindChessboardCorners to find corners of chessboard in image. - - Check all boards. Return corners for first chessboard that it detects - if given multiple size chessboards. - - Returns (ok, corners, board) - """ - - for b in self._boards: - if self.pattern == Patterns.Chessboard: - (ok, corners) = _get_corners(img, b, refine, self.checkerboard_flags) - else: - (ok, corners) = _get_circles(img, b, self.pattern) - if ok: - return (ok, corners, b) - return (False, None, None) - - def downsample_and_detect(self, img): - """ - Downsample the input image to approximately VGA resolution and detect the - calibration target corners in the full-size image. - - Combines these apparently orthogonal duties as an optimization. Checkerboard - detection is too expensive on large images, so it's better to do detection on - the smaller display image and scale the corners back up to the correct size. - - Returns (scrib, corners, downsampled_corners, board, (x_scale, y_scale)). - """ - # Scale the input image down to ~VGA size - height = img.shape[0] - width = img.shape[1] - scale = math.sqrt( (width*height) / (640.*480.) ) - if scale > 1.0: - scrib = cv2.resize(img, (int(width / scale), int(height / scale))) - else: - scrib = img - # Due to rounding, actual horizontal/vertical scaling may differ slightly - x_scale = float(width) / scrib.shape[1] - y_scale = float(height) / scrib.shape[0] - - if self.pattern == Patterns.Chessboard: - # Detect checkerboard - (ok, downsampled_corners, board) = self.get_corners(scrib, refine = True) - - # Scale corners back to full size image - corners = None - if ok: - if scale > 1.0: - # Refine up-scaled corners in the original full-res image - # TODO Does this really make a difference in practice? - corners_unrefined = downsampled_corners.copy() - corners_unrefined[:, :, 0] *= x_scale - corners_unrefined[:, :, 1] *= y_scale - radius = int(math.ceil(scale)) - if len(img.shape) == 3 and img.shape[2] == 3: - mono = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - else: - mono = img - cv2.cornerSubPix(mono, corners_unrefined, (radius,radius), (-1,-1), - ( cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1 )) - corners = corners_unrefined - else: - corners = downsampled_corners - else: - # Circle grid detection is fast even on large images - (ok, corners, board) = self.get_corners(img) - # Scale corners to downsampled image for display - downsampled_corners = None - if ok: - if scale > 1.0: - downsampled_corners = corners.copy() - downsampled_corners[:,:,0] /= x_scale - downsampled_corners[:,:,1] /= y_scale - else: - downsampled_corners = corners - - return (scrib, corners, downsampled_corners, board, (x_scale, y_scale)) - - - def lrmsg(self, d, k, r, p): - """ Used by :meth:`as_message`. Return a CameraInfo message for the given calibration matrices """ - msg = sensor_msgs.msg.CameraInfo() - (msg.width, msg.height) = self.size - if d.size > 5: - msg.distortion_model = "rational_polynomial" - else: - msg.distortion_model = "plumb_bob" - msg.D = numpy.ravel(d).copy().tolist() - msg.K = numpy.ravel(k).copy().tolist() - msg.R = numpy.ravel(r).copy().tolist() - msg.P = numpy.ravel(p).copy().tolist() - return msg - - def lrreport(self, d, k, r, p): - print("D = ", numpy.ravel(d).tolist()) - print("K = ", numpy.ravel(k).tolist()) - print("R = ", numpy.ravel(r).tolist()) - print("P = ", numpy.ravel(p).tolist()) - - def lrost(self, name, d, k, r, p): - calmessage = ( - "# oST version 5.0 parameters\n" - + "\n" - + "\n" - + "[image]\n" - + "\n" - + "width\n" - + str(self.size[0]) + "\n" - + "\n" - + "height\n" - + str(self.size[1]) + "\n" - + "\n" - + "[%s]" % name + "\n" - + "\n" - + "camera matrix\n" - + " ".join(["%8f" % k[0,i] for i in range(3)]) + "\n" - + " ".join(["%8f" % k[1,i] for i in range(3)]) + "\n" - + " ".join(["%8f" % k[2,i] for i in range(3)]) + "\n" - + "\n" - + "distortion\n" - + " ".join(["%8f" % d[i,0] for i in range(d.shape[0])]) + "\n" - + "\n" - + "rectification\n" - + " ".join(["%8f" % r[0,i] for i in range(3)]) + "\n" - + " ".join(["%8f" % r[1,i] for i in range(3)]) + "\n" - + " ".join(["%8f" % r[2,i] for i in range(3)]) + "\n" - + "\n" - + "projection\n" - + " ".join(["%8f" % p[0,i] for i in range(4)]) + "\n" - + " ".join(["%8f" % p[1,i] for i in range(4)]) + "\n" - + " ".join(["%8f" % p[2,i] for i in range(4)]) + "\n" - + "\n") - assert len(calmessage) < 525, "Calibration info must be less than 525 bytes" - return calmessage - - def lryaml(self, name, d, k, r, p): - calmessage = ("" - + "image_width: " + str(self.size[0]) + "\n" - + "image_height: " + str(self.size[1]) + "\n" - + "camera_name: " + name + "\n" - + "camera_matrix:\n" - + " rows: 3\n" - + " cols: 3\n" - + " data: [" + ", ".join(["%8f" % i for i in k.reshape(1,9)[0]]) + "]\n" - + "distortion_model: " + ("rational_polynomial" if d.size > 5 else "plumb_bob") + "\n" - + "distortion_coefficients:\n" - + " rows: 1\n" - + " cols: 4\n" - + " data: [" + ", ".join(["%8f" % d[i,0] for i in range(d.shape[0])]) + "]\n" - + "rectification_matrix:\n" - + " rows: 3\n" - + " cols: 3\n" - + " data: [" + ", ".join(["%8f" % i for i in r.reshape(1,9)[0]]) + "]\n" - + "projection_matrix:\n" - + " rows: 3\n" - + " cols: 4\n" - + " data: [" + ", ".join(["%8f" % i for i in p.reshape(1,12)[0]]) + "]\n" - + "") - return calmessage - - def do_save(self): - filename = '/tmp/calibrationdata.tar.gz' - tf = tarfile.open(filename, 'w:gz') - self.do_tarfile_save(tf) # Must be overridden in subclasses - tf.close() - print(("Wrote calibration data to", filename)) - -def image_from_archive(archive, name): - """ - Load image PGM file from tar archive. - - Used for tarfile loading and unit test. - """ - member = archive.getmember(name) - imagefiledata = numpy.fromstring(archive.extractfile(member).read(), numpy.uint8) - imagefiledata.resize((1, imagefiledata.size)) - return cv2.imdecode(imagefiledata, cv2.IMREAD_COLOR) - -class ImageDrawable(object): - """ - Passed to CalibrationNode after image handled. Allows plotting of images - with detected corner points - """ - def __init__(self): - self.params = None - -class MonoDrawable(ImageDrawable): - def __init__(self): - ImageDrawable.__init__(self) - self.scrib = None - self.linear_error = -1.0 - - -class StereoDrawable(ImageDrawable): - def __init__(self): - ImageDrawable.__init__(self) - self.lscrib = None - self.rscrib = None - self.epierror = -1 - self.dim = -1 - - -class MonoCalibrator(Calibrator): - """ - Calibration class for monocular cameras:: - - images = [cv2.imread("mono%d.png") for i in range(8)] - mc = MonoCalibrator() - mc.cal(images) - print mc.as_message() - """ - - is_mono = True # TODO Could get rid of is_mono - - def __init__(self, *args, **kwargs): - if 'name' not in kwargs: - kwargs['name'] = 'fisheye' - super(MonoCalibrator, self).__init__(*args, **kwargs) - - def cal(self, images): - """ - Calibrate camera from given images - """ - goodcorners = self.collect_corners(images) - self.cal_fromcorners(goodcorners) - self.calibrated = True - - def collect_corners(self, images): - """ - :param images: source images containing chessboards - :type images: list of :class:`cvMat` - - Find chessboards in all images. - - Return [ (corners, ChessboardInfo) ] - """ - self.size = (images[0].shape[1], images[0].shape[0]) - corners = [self.get_corners(i) for i in images] - - goodcorners = [(co, b) for (ok, co, b) in corners if ok] - if not goodcorners: - raise CalibrationException("No corners found in images!") - return goodcorners - - def cal_fromcorners(self, good): - """ - :param good: Good corner positions and boards - :type good: [(corners, ChessboardInfo)] - - - """ - boards = [ b for (_, b) in good ] - - ipts = [ points for (points, _) in good ] - opts = self.mk_object_points(boards) - - self.intrinsics = numpy.zeros((3, 3), numpy.float64) - if self.calib_flags & cv2.CALIB_RATIONAL_MODEL: - self.distortion = numpy.zeros((8, 1), numpy.float64) # rational polynomial - else: - self.distortion = numpy.zeros((4, 1), numpy.float64) - # If FIX_ASPECT_RATIO flag set, enforce focal lengths have 1/1 ratio - self.intrinsics[0,0] = 1.0 - self.intrinsics[1,1] = 1.0 - - calibration_flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC +cv2.fisheye.CALIB_FIX_SKEW - # +cv2.fisheye.CALIB_CHECK_COND - rvecs = [numpy.zeros((1, 1, 3), dtype=numpy.float64) for i in range(len(opts))] - tvecs = [numpy.zeros((1, 1, 3), dtype=numpy.float64) for i in range(len(opts))] - - cv2.fisheye.calibrate( opts, ipts, self.size, self.intrinsics, self.distortion, - rvecs, tvecs, calibration_flags, (cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6)) - - # R is identity matrix for monocular calibration - self.R = numpy.eye(3, dtype=numpy.float64) - self.P = numpy.zeros((3, 4), dtype=numpy.float64) - - self.set_alpha(0.0) - - def set_alpha(self, a): - """ - Set the alpha value for the calibrated camera solution. The alpha - value is a zoom, and ranges from 0 (zoomed in, all pixels in - calibrated image are valid) to 1 (zoomed out, all pixels in - original image are in calibrated image). - """ - - # NOTE: see https://medium.com/@kennethjiang/calibrate-fisheye-lens-using-opencv-part-2-13990f1b157f - ncm = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(self.intrinsics, self.distortion, self.size, numpy.eye(3), balance=a) - for j in range(3): - for i in range(3): - #self.P[j,i] = ncm[j, i] - self.P[j,i] = self.intrinsics[j, i] - - #self.P = self.intrinsics - ##self.mapx, self.mapy = cv2.fisheye.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, ncm, self.size, cv2.CV_32FC1) # Use this to perform cropping - self.mapx, self.mapy = cv2.fisheye.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, self.intrinsics, self.size, cv2.CV_32FC1) ############################# - - def remap(self, src): - """ - :param src: source image - :type src: :class:`cvMat` - - Apply the post-calibration undistortion to the source image - """ - return cv2.remap(src, self.mapx, self.mapy, cv2.INTER_LANCZOS4) - - def undistort_points(self, src): - """ - :param src: N source pixel points (u,v) as an Nx2 matrix - :type src: :class:`cvMat` - - Apply the post-calibration undistortion to the source points - """ - - return cv2.fisheye.undistortPoints(src, self.intrinsics, self.distortion, R = self.R, P = self.P) - - def as_message(self): - """ Return the camera calibration as a CameraInfo message """ - return self.lrmsg(self.distortion, self.intrinsics, self.R, self.P) - - def from_message(self, msg, alpha = 0.0): - """ Initialize the camera calibration from a CameraInfo message """ - - self.size = (msg.width, msg.height) - self.intrinsics = numpy.array(msg.K, dtype=numpy.float64, copy=True).reshape((3, 3)) - self.distortion = numpy.array(msg.D, dtype=numpy.float64, copy=True).reshape((len(msg.D), 1)) - self.R = numpy.array(msg.R, dtype=numpy.float64, copy=True).reshape((3, 3)) - self.P = numpy.array(msg.P, dtype=numpy.float64, copy=True).reshape((3, 4)) - - self.set_alpha(0.0) - - def report(self): - self.lrreport(self.distortion, self.intrinsics, self.R, self.P) - - def ost(self): - return self.lrost(self.name, self.distortion, self.intrinsics, self.R, self.P) - - def yaml(self): - return self.lryaml(self.name, self.distortion, self.intrinsics, self.R, self.P) - - def linear_error_from_image(self, image): - """ - Detect the checkerboard and compute the linear error. - Mainly for use in tests. - """ - _, corners, _, board, _ = self.downsample_and_detect(image) - if corners is None: - return None - - undistorted = self.undistort_points(corners) - return self.linear_error(undistorted, board) - - @staticmethod - def linear_error(corners, b): - - """ - Returns the linear error for a set of corners detected in the unrectified image. - """ - - if corners is None: - return None - - def pt2line(x0, y0, x1, y1, x2, y2): - """ point is (x0, y0), line is (x1, y1, x2, y2) """ - return abs((x2 - x1) * (y1 - y0) - (x1 - x0) * (y2 - y1)) / math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) - - cc = b.n_cols - cr = b.n_rows - errors = [] - for r in range(cr): - (x1, y1) = corners[(cc * r) + 0, 0] - (x2, y2) = corners[(cc * r) + cc - 1, 0] - for i in range(1, cc - 1): - (x0, y0) = corners[(cc * r) + i, 0] - errors.append(pt2line(x0, y0, x1, y1, x2, y2)) - if errors: - return math.sqrt(sum([e**2 for e in errors]) / len(errors)) - else: - return None - - - def handle_msg(self, msg): - """ - Detects the calibration target and, if found and provides enough new information, - adds it to the sample database. - - Returns a MonoDrawable message with the display image and progress info. - """ - gray = self.mkgray(msg) - linear_error = -1 - - # Get display-image-to-be (scrib) and detection of the calibration target - scrib_mono, corners, downsampled_corners, board, (x_scale, y_scale) = self.downsample_and_detect(gray) - - if self.calibrated: - # Show rectified image - # TODO Pull out downsampling code into function - gray_remap = self.remap(gray) - gray_rect = gray_remap - if x_scale != 1.0 or y_scale != 1.0: - gray_rect = cv2.resize(gray_remap, (scrib_mono.shape[1], scrib_mono.shape[0])) - - scrib = cv2.cvtColor(gray_rect, cv2.COLOR_GRAY2BGR) - - if corners is not None: - # Report linear error - undistorted = self.undistort_points(corners) - linear_error = self.linear_error(undistorted, board) - - # Draw rectified corners - scrib_src = undistorted.copy() - scrib_src[:,:,0] /= x_scale - scrib_src[:,:,1] /= y_scale - cv2.drawChessboardCorners(scrib, (board.n_cols, board.n_rows), scrib_src, True) - - else: - scrib = cv2.cvtColor(scrib_mono, cv2.COLOR_GRAY2BGR) - if corners is not None: - # Draw (potentially downsampled) corners onto display image - cv2.drawChessboardCorners(scrib, (board.n_cols, board.n_rows), downsampled_corners, True) - - # Add sample to database only if it's sufficiently different from any previous sample. - params = self.get_parameters(corners, board, (gray.shape[1], gray.shape[0])) - if self.is_good_sample(params): - self.db.append((params, gray)) - self.good_corners.append((corners, board)) - print(("*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" % tuple([len(self.db)] + params))) - - rv = MonoDrawable() - rv.scrib = scrib - rv.params = self.compute_goodenough() - rv.linear_error = linear_error - return rv - - def do_calibration(self, dump = False): - if not self.good_corners: - print("**** Collecting corners for all images! ****") #DEBUG - images = [i for (p, i) in self.db] - self.good_corners = self.collect_corners(images) - # Dump should only occur if user wants it - if dump: - pickle.dump((self.is_mono, self.size, self.good_corners), - open("/tmp/camera_calibration_%08x.pickle" % random.getrandbits(32), "w")) - self.size = (self.db[0][1].shape[1], self.db[0][1].shape[0]) # TODO Needs to be set externally - self.cal_fromcorners(self.good_corners) - self.calibrated = True - # DEBUG - print((self.report())) - print((self.ost())) - - def do_tarfile_save(self, tf): - """ Write images and calibration solution to a tarfile object """ - - def taradd(name, buf): - if isinstance(buf, basestring): - s = StringIO(buf) - else: - s = BytesIO(buf) - ti = tarfile.TarInfo(name) - ti.size = len(s.getvalue()) - ti.uname = 'calibrator' - ti.mtime = int(time.time()) - tf.addfile(tarinfo=ti, fileobj=s) - - ims = [("left-%04d.png" % i, im) for i,(_, im) in enumerate(self.db)] - for (name, im) in ims: - taradd(name, cv2.imencode(".png", im)[1].tostring()) - taradd('ost.yaml', self.yaml()) - taradd('ost.txt', self.ost()) - - def do_tarfile_calibration(self, filename): - archive = tarfile.open(filename, 'r') - - limages = [ image_from_archive(archive, f) for f in archive.getnames() if (f.startswith('left') and (f.endswith('.pgm') or f.endswith('png'))) ] - - self.cal(limages) - -# TODO Replicate MonoCalibrator improvements in stereo -class StereoCalibrator(Calibrator): - """ - Calibration class for stereo cameras:: - - limages = [cv2.imread("left%d.png") for i in range(8)] - rimages = [cv2.imread("right%d.png") for i in range(8)] - sc = StereoCalibrator() - sc.cal(limages, rimages) - print sc.as_message() - """ - - is_mono = False - - def __init__(self, *args, **kwargs): - if 'name' not in kwargs: - kwargs['name'] = 'fisheye' - super(StereoCalibrator, self).__init__(*args, **kwargs) - self.l = MonoCalibrator(*args, **kwargs) - self.r = MonoCalibrator(*args, **kwargs) - # Collecting from two cameras in a horizontal stereo rig, can't get - # full X range in the left camera. - self.param_ranges[0] = 0.4 - - def cal(self, limages, rimages): - """ - :param limages: source left images containing chessboards - :type limages: list of :class:`cvMat` - :param rimages: source right images containing chessboards - :type rimages: list of :class:`cvMat` - - Find chessboards in images, and runs the OpenCV calibration solver. - """ - goodcorners = self.collect_corners(limages, rimages) - self.size = (limages[0].shape[1], limages[0].shape[0]) - self.l.size = self.size - self.r.size = self.size - self.cal_fromcorners(goodcorners) - self.calibrated = True - - def collect_corners(self, limages, rimages): - """ - For a sequence of left and right images, find pairs of images where both - left and right have a chessboard, and return their corners as a list of pairs. - """ - # Pick out (corners, board) tuples - lcorners = [ self.downsample_and_detect(i)[1:4:2] for i in limages] - rcorners = [ self.downsample_and_detect(i)[1:4:2] for i in rimages] - good = [(lco, rco, b) for ((lco, b), (rco, br)) in zip( lcorners, rcorners) - if (lco is not None and rco is not None)] - - if len(good) == 0: - raise CalibrationException("No corners found in images!") - return good - - def cal_fromcorners(self, good): - # Perform monocular calibrations - lcorners = [(l, b) for (l, r, b) in good] - rcorners = [(r, b) for (l, r, b) in good] - self.l.cal_fromcorners(lcorners) # Calibrate each as mono? - self.r.cal_fromcorners(rcorners) - - lipts = [ l for (l, _, _) in good ] - ripts = [ r for (_, r, _) in good ] - boards = [ b for (_, _, b) in good ] - - opts = self.mk_object_points(boards, True) - - flags = cv2.fisheye.CALIB_FIX_INTRINSIC - - # Sanity checks - opts = numpy.array(opts, dtype=numpy.float64) - lipts = numpy.array(lipts, dtype=numpy.float64) - ripts = numpy.array(ripts, dtype=numpy.float64) - - #print("opts.shape ",opts.shape,"lipts.shape " ,lipts.shape,"ripts.shape ",ripts.shape) - - #self.T = numpy.zeros((1,1,3), dtype=numpy.float64) - #self.R = numpy.zeros((1,1,3), dtype=numpy.float64) - - self.T = numpy.zeros((3, 1), dtype=numpy.float64) - self.R = numpy.eye(3, dtype=numpy.float64) - - if LooseVersion(cv2.__version__).version[0] == 2: - # cv2.stereoCalibrate(opts, lipts, ripts, self.size, - # self.l.intrinsics, self.l.distortion, - # self.r.intrinsics, self.r.distortion, - # self.R, # R - # self.T, # T - # criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), - # flags = flags) - print("You need OpenCV >3 to use fisheye camera model") - else: - cv2.fisheye.stereoCalibrate(opts, lipts, ripts, - self.l.intrinsics, self.l.distortion, - self.r.intrinsics, self.r.distortion, - self.size, - self.R, # R - self.T, # T - criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), # 30, 1e-6 - flags = flags) - - self.set_alpha(0.0) - - def set_alpha(self, a): - """ - Set the alpha value for the calibrated camera solution. The - alpha value is a zoom, and ranges from 0 (zoomed in, all pixels - in calibrated image are valid) to 1 (zoomed out, all pixels in - original image are in calibrated image). - """ - self.Q = numpy.zeros((4,4), dtype=numpy.float64) - - flags = cv2.CALIB_ZERO_DISPARITY # Operation flags that may be zero or CALIB_ZERO_DISPARITY . - # If the flag is set, the function makes the principal points of each camera have the same pixel coordinates in the rectified views. - # And if the flag is not set, the function may still shift the images in the horizontal or vertical direction - # (depending on the orientation of epipolar lines) to maximize the useful image area. - - cv2.fisheye.stereoRectify(self.l.intrinsics, self.l.distortion, - self.r.intrinsics, self.r.distortion, - self.size, - self.R, self.T, - flags, - self.l.R, self.r.R, - self.l.P, self.r.P, - self.Q, - self.size, - a, - 1.0 ) - # Put the P values of fx,fy,cx and cy to be K's: we keep the Camera matrix, as any rotation will be done by R and the translation (to the new camera frame) will be keept by the last column of P - for j in range(3): - for i in range(3): - self.l.P[j,i] = self.l.intrinsics[j, i] - self.r.P[j,i] = self.r.intrinsics[j, i] - - cv2.fisheye.initUndistortRectifyMap(self.l.intrinsics, self.l.distortion, self.l.R, self.l.intrinsics, self.size, cv2.CV_32FC1, - self.l.mapx, self.l.mapy) - cv2.fisheye.initUndistortRectifyMap(self.r.intrinsics, self.r.distortion, self.r.R, self.r.intrinsics, self.size, cv2.CV_32FC1, - self.r.mapx, self.r.mapy) - - def as_message(self): - """ - Return the camera calibration as a pair of CameraInfo messages, for left - and right cameras respectively. - """ - - return (self.lrmsg(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P), - self.lrmsg(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P)) - - def from_message(self, msgs, alpha = 0.0): - """ Initialize the camera calibration from a pair of CameraInfo messages. """ - self.size = (msgs[0].width, msgs[0].height) - - self.T = numpy.zeros((3, 1), dtype=numpy.float64) - self.R = numpy.eye(3, dtype=numpy.float64) - - self.l.from_message(msgs[0]) - self.r.from_message(msgs[1]) - # Need to compute self.T and self.R here, using the monocular parameters above - if False: - self.set_alpha(0.0) - - def report(self): - print("\nLeft:") - self.lrreport(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P) - print("\nRight:") - self.lrreport(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P) - print("self.T ", numpy.ravel(self.T).tolist()) - print("self.R ", numpy.ravel(self.R).tolist()) - - def ost(self): - return (self.lrost(self.name + "/left", self.l.distortion, self.l.intrinsics, self.l.R, self.l.P) + - self.lrost(self.name + "/right", self.r.distortion, self.r.intrinsics, self.r.R, self.r.P)) - - def yaml(self, suffix, info): - return self.lryaml(self.name + suffix, info.distortion, info.intrinsics, info.R, info.P) - - # TODO Get rid of "from_images" versions of these, instead have function to get undistorted corners - def epipolar_error_from_images(self, limage, rimage): - """ - Detect the checkerboard in both images and compute the epipolar error. - Mainly for use in tests. - """ - lcorners = self.downsample_and_detect(limage)[1] - rcorners = self.downsample_and_detect(rimage)[1] - if lcorners is None or rcorners is None: - return None - - lundistorted = self.l.undistort_points(lcorners) - rundistorted = self.r.undistort_points(rcorners) - - return self.epipolar_error(lundistorted, rundistorted) - - def epipolar_error(self, lcorners, rcorners): - """ - Compute the epipolar error from two sets of matching undistorted points - """ - d = lcorners[:,:,1] - rcorners[:,:,1] - return numpy.sqrt(numpy.square(d).sum() / d.size) - - def chessboard_size_from_images(self, limage, rimage): - _, lcorners, _, board, _ = self.downsample_and_detect(limage) - _, rcorners, _, board, _ = self.downsample_and_detect(rimage) - if lcorners is None or rcorners is None: - return None - - lundistorted = self.l.undistort_points(lcorners) - rundistorted = self.r.undistort_points(rcorners) - - return self.chessboard_size(lundistorted, rundistorted, board) - - def chessboard_size(self, lcorners, rcorners, board, msg = None): - """ - Compute the square edge length from two sets of matching undistorted points - given the current calibration. - :param msg: a tuple of (left_msg, right_msg) - """ - # Project the points to 3d - cam = image_geometry.StereoCameraModel() - if msg == None: - msg = self.as_message() - cam.fromCameraInfo(*msg) - disparities = lcorners[:,:,0] - rcorners[:,:,0] - pt3d = [cam.projectPixelTo3d((lcorners[i,0,0], lcorners[i,0,1]), disparities[i,0]) for i in range(lcorners.shape[0]) ] - def l2(p0, p1): - return math.sqrt(sum([(c0 - c1) ** 2 for (c0, c1) in zip(p0, p1)])) - - # Compute the length from each horizontal and vertical line, and return the mean - cc = board.n_cols - cr = board.n_rows - lengths = ( - [l2(pt3d[cc * r + 0], pt3d[cc * r + (cc - 1)]) / (cc - 1) for r in range(cr)] + - [l2(pt3d[c + 0], pt3d[c + (cc * (cr - 1))]) / (cr - 1) for c in range(cc)]) - return sum(lengths) / len(lengths) - - def handle_msg(self, msg): - # TODO Various asserts that images have same dimension, same board detected... - (lmsg, rmsg) = msg - lgray = self.mkgray(lmsg) - rgray = self.mkgray(rmsg) - epierror = -1 - - # Get display-images-to-be and detections of the calibration target - lscrib_mono, lcorners, ldownsampled_corners, lboard, (x_scale, y_scale) = self.downsample_and_detect(lgray) - rscrib_mono, rcorners, rdownsampled_corners, rboard, _ = self.downsample_and_detect(rgray) - - if self.calibrated: - # Show rectified images - lremap = self.l.remap(lgray) - rremap = self.r.remap(rgray) - lrect = lremap - rrect = rremap - if x_scale != 1.0 or y_scale != 1.0: - lrect = cv2.resize(lremap, (lscrib_mono.shape[1], lscrib_mono.shape[0])) - rrect = cv2.resize(rremap, (rscrib_mono.shape[1], rscrib_mono.shape[0])) - - lscrib = cv2.cvtColor(lrect, cv2.COLOR_GRAY2BGR) - rscrib = cv2.cvtColor(rrect, cv2.COLOR_GRAY2BGR) - - # Draw rectified corners - if lcorners is not None: - lundistorted = self.l.undistort_points(lcorners) - scrib_src = lundistorted.copy() - scrib_src[:,:,0] /= x_scale - scrib_src[:,:,1] /= y_scale - cv2.drawChessboardCorners(lscrib, (lboard.n_cols, lboard.n_rows), scrib_src, True) - - if rcorners is not None: - rundistorted = self.r.undistort_points(rcorners) - scrib_src = rundistorted.copy() - scrib_src[:,:,0] /= x_scale - scrib_src[:,:,1] /= y_scale - cv2.drawChessboardCorners(rscrib, (rboard.n_cols, rboard.n_rows), scrib_src, True) - - # Report epipolar error - if lcorners is not None and rcorners is not None and len(lcorners) == len(rcorners): - epierror = self.epipolar_error(lundistorted, rundistorted) - - else: - lscrib = cv2.cvtColor(lscrib_mono, cv2.COLOR_GRAY2BGR) - rscrib = cv2.cvtColor(rscrib_mono, cv2.COLOR_GRAY2BGR) - # Draw any detected chessboards onto display (downsampled) images - if lcorners is not None: - cv2.drawChessboardCorners(lscrib, (lboard.n_cols, lboard.n_rows), - ldownsampled_corners, True) - if rcorners is not None: - cv2.drawChessboardCorners(rscrib, (rboard.n_cols, rboard.n_rows), - rdownsampled_corners, True) - - # Add sample to database only if it's sufficiently different from any previous sample - if lcorners is not None and rcorners is not None and len(lcorners) == len(rcorners): - params = self.get_parameters(lcorners, lboard, (lgray.shape[1], lgray.shape[0])) - if self.is_good_sample(params): - self.db.append( (params, lgray, rgray) ) - self.good_corners.append( (lcorners, rcorners, lboard) ) - print(("*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" % tuple([len(self.db)] + params))) - - rv = StereoDrawable() - rv.lscrib = lscrib - rv.rscrib = rscrib - rv.params = self.compute_goodenough() - rv.epierror = epierror - return rv - - def do_calibration(self, dump = False): - # TODO MonoCalibrator collects corners if needed here - # Dump should only occur if user wants it - if dump: - pickle.dump((self.is_mono, self.size, self.good_corners), - open("/tmp/camera_calibration_%08x.pickle" % random.getrandbits(32), "w")) - self.size = (self.db[0][1].shape[1], self.db[0][1].shape[0]) # TODO Needs to be set externally - self.l.size = self.size - self.r.size = self.size - self.cal_fromcorners(self.good_corners) - self.calibrated = True - # DEBUG - print((self.report())) - print((self.ost())) - - def do_tarfile_save(self, tf): - """ Write images and calibration solution to a tarfile object """ - ims = ([("left-%04d.png" % i, im) for i,(_, im, _) in enumerate(self.db)] + - [("right-%04d.png" % i, im) for i,(_, _, im) in enumerate(self.db)]) - - def taradd(name, buf): - if isinstance(buf, basestring): - s = StringIO(buf) - else: - s = BytesIO(buf) - ti = tarfile.TarInfo(name) - ti.size = len(s.getvalue()) - ti.uname = 'calibrator' - ti.mtime = int(time.time()) - tf.addfile(tarinfo=ti, fileobj=s) - - for (name, im) in ims: - taradd(name, cv2.imencode(".png", im)[1].tostring()) - taradd('left.yaml', self.yaml("/left", self.l)) - taradd('right.yaml', self.yaml("/right", self.r)) - taradd('ost.txt', self.ost()) - - def do_tarfile_calibration(self, filename): - archive = tarfile.open(filename, 'r') - limages = [ image_from_archive(archive, f) for f in archive.getnames() if (f.startswith('left') and (f.endswith('pgm') or f.endswith('png'))) ] - rimages = [ image_from_archive(archive, f) for f in archive.getnames() if (f.startswith('right') and (f.endswith('pgm') or f.endswith('png'))) ] - - if not len(limages) == len(rimages): - raise CalibrationException("Left, right images don't match. %d left images, %d right" % (len(limages), len(rimages))) - - ##\todo Check that the filenames match and stuff - - self.cal(limages, rimages) diff --git a/camera_calibration_fisheye/src/camera_calibration/camera_calibrator.py b/camera_calibration_fisheye/src/camera_calibration/camera_calibrator.py deleted file mode 100644 index bb9af7c14..000000000 --- a/camera_calibration_fisheye/src/camera_calibration/camera_calibrator.py +++ /dev/null @@ -1,352 +0,0 @@ -#!/usr/bin/python -# -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the Willow Garage nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import cv2 -import message_filters -import numpy -import os -import rospy -import sensor_msgs.msg -import sensor_msgs.srv -import threading -import time -from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, ChessboardInfo, Patterns -from collections import deque -from message_filters import ApproximateTimeSynchronizer -from std_msgs.msg import String -from std_srvs.srv import Empty - - -class DisplayThread(threading.Thread): - """ - Thread that displays the current images - It is its own thread so that all display can be done - in one thread to overcome imshow limitations and - https://github.com/ros-perception/image_pipeline/issues/85 - """ - def __init__(self, queue, opencv_calibration_node): - threading.Thread.__init__(self) - self.queue = queue - self.opencv_calibration_node = opencv_calibration_node - - def run(self): - cv2.namedWindow("display", cv2.WINDOW_NORMAL) - cv2.setMouseCallback("display", self.opencv_calibration_node.on_mouse) - cv2.createTrackbar("scale", "display", 0, 100, self.opencv_calibration_node.on_scale) - while True: - # wait for an image (could happen at the very beginning when the queue is still empty) - while len(self.queue) == 0: - time.sleep(0.1) - im = self.queue[0] - cv2.imshow("display", im) - k = cv2.waitKey(6) & 0xFF - if k in [27, ord('q')]: - rospy.signal_shutdown('Quit') - elif k == ord('s'): - self.opencv_calibration_node.screendump(im) - -class ConsumerThread(threading.Thread): - def __init__(self, queue, function): - threading.Thread.__init__(self) - self.queue = queue - self.function = function - - def run(self): - while True: - # wait for an image (could happen at the very beginning when the queue is still empty) - while len(self.queue) == 0: - time.sleep(0.1) - self.function(self.queue[0]) - - -class CalibrationNode: - def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0, - pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0): - if service_check: - # assume any non-default service names have been set. Wait for the service to become ready - for svcname in ["camera", "left_camera", "right_camera"]: - remapped = rospy.remap_name(svcname) - if remapped != svcname: - fullservicename = "%s/set_camera_info" % remapped - print("Waiting for service", fullservicename, "...") - try: - rospy.wait_for_service(fullservicename, 5) - print("OK") - except rospy.ROSException: - print("Service not found") - rospy.signal_shutdown('Quit') - - self._boards = boards - self._calib_flags = flags - self._checkerboard_flags = checkerboard_flags - self._pattern = pattern - self._camera_name = camera_name - lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image) - rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image) - ts = synchronizer([lsub, rsub], 4) - ts.registerCallback(self.queue_stereo) - - msub = message_filters.Subscriber('image', sensor_msgs.msg.Image) - msub.registerCallback(self.queue_monocular) - - self.set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), - sensor_msgs.srv.SetCameraInfo) - self.set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), - sensor_msgs.srv.SetCameraInfo) - self.set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), - sensor_msgs.srv.SetCameraInfo) - - self.q_mono = deque([], 1) - self.q_stereo = deque([], 1) - - self.c = None - - mth = ConsumerThread(self.q_mono, self.handle_monocular) - mth.setDaemon(True) - mth.start() - - sth = ConsumerThread(self.q_stereo, self.handle_stereo) - sth.setDaemon(True) - sth.start() - - def redraw_stereo(self, *args): - pass - def redraw_monocular(self, *args): - pass - - def queue_monocular(self, msg): - self.q_mono.append(msg) - - def queue_stereo(self, lmsg, rmsg): - self.q_stereo.append((lmsg, rmsg)) - - def handle_monocular(self, msg): - if self.c == None: - if self._camera_name: - self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name, - checkerboard_flags=self._checkerboard_flags) - else: - self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, - checkerboard_flags=self.checkerboard_flags) - - # This should just call the MonoCalibrator - drawable = self.c.handle_msg(msg) - self.displaywidth = drawable.scrib.shape[1] - self.redraw_monocular(drawable) - - def handle_stereo(self, msg): - if self.c == None: - if self._camera_name: - self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name, - checkerboard_flags=self._checkerboard_flags) - else: - self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, - checkerboard_flags=self._checkerboard_flags) - - drawable = self.c.handle_msg(msg) - self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1] - self.redraw_stereo(drawable) - - - def check_set_camera_info(self, response): - if response.success: - return True - - for i in range(10): - print("!" * 80) - print() - print("Attempt to set camera info failed: " + response.status_message) - print() - for i in range(10): - print("!" * 80) - print() - rospy.logerr('Unable to set camera info for calibration. Failure message: %s' % response.status_message) - return False - - def do_upload(self): - self.c.report() - print(self.c.ost()) - info = self.c.as_message() - - rv = True - if self.c.is_mono: - response = self.set_camera_info_service(info) - rv = self.check_set_camera_info(response) - else: - response = self.set_left_camera_info_service(info[0]) - rv = rv and self.check_set_camera_info(response) - response = self.set_right_camera_info_service(info[1]) - rv = rv and self.check_set_camera_info(response) - return rv - - -class OpenCVCalibrationNode(CalibrationNode): - """ Calibration node with an OpenCV Gui """ - FONT_FACE = cv2.FONT_HERSHEY_SIMPLEX - FONT_SCALE = 0.6 - FONT_THICKNESS = 2 - - def __init__(self, *args, **kwargs): - - CalibrationNode.__init__(self, *args, **kwargs) - - self.queue_display = deque([], 1) - self.display_thread = DisplayThread(self.queue_display, self) - self.display_thread.setDaemon(True) - self.display_thread.start() - - @classmethod - def putText(cls, img, text, org, color = (0,0,0)): - cv2.putText(img, text, org, cls.FONT_FACE, cls.FONT_SCALE, color, thickness = cls.FONT_THICKNESS) - - @classmethod - def getTextSize(cls, text): - return cv2.getTextSize(text, cls.FONT_FACE, cls.FONT_SCALE, cls.FONT_THICKNESS)[0] - - def on_mouse(self, event, x, y, flags, param): - if event == cv2.EVENT_LBUTTONDOWN and self.displaywidth < x: - if self.c.goodenough: - if 180 <= y < 280: - self.c.do_calibration() - if self.c.calibrated: - if 280 <= y < 380: - self.c.do_save() - elif 380 <= y < 480: - # Only shut down if we set camera info correctly, #3993 - if self.do_upload(): - rospy.signal_shutdown('Quit') - - def on_scale(self, scalevalue): - if self.c.calibrated: - self.c.set_alpha(scalevalue / 100.0) - - def button(self, dst, label, enable): - dst.fill(255) - size = (dst.shape[1], dst.shape[0]) - if enable: - color = (155, 155, 80) - else: - color = (224, 224, 224) - cv2.circle(dst, (size[0] // 2, size[1] // 2), min(size) // 2, color, -1) - (w, h) = self.getTextSize(label) - self.putText(dst, label, ((size[0] - w) // 2, (size[1] + h) // 2), (255,255,255)) - - def buttons(self, display): - x = self.displaywidth - self.button(display[180:280,x:x+100], "CALIBRATE", self.c.goodenough) - self.button(display[280:380,x:x+100], "SAVE", self.c.calibrated) - self.button(display[380:480,x:x+100], "COMMIT", self.c.calibrated) - - def y(self, i): - """Set up right-size images""" - return 30 + 40 * i - - def screendump(self, im): - i = 0 - while os.access("/tmp/dump%d.png" % i, os.R_OK): - i += 1 - cv2.imwrite("/tmp/dump%d.png" % i, im) - - def redraw_monocular(self, drawable): - height = drawable.scrib.shape[0] - width = drawable.scrib.shape[1] - - display = numpy.zeros((max(480, height), width + 100, 3), dtype=numpy.uint8) - display[0:height, 0:width,:] = drawable.scrib - display[0:height, width:width+100,:].fill(255) - - - self.buttons(display) - if not self.c.calibrated: - if drawable.params: - for i, (label, lo, hi, progress) in enumerate(drawable.params): - (w,_) = self.getTextSize(label) - self.putText(display, label, (width + (100 - w) // 2, self.y(i))) - color = (0,255,0) - if progress < 1.0: - color = (0, int(progress*255.), 255) - cv2.line(display, - (int(width + lo * 100), self.y(i) + 20), - (int(width + hi * 100), self.y(i) + 20), - color, 4) - - else: - self.putText(display, "lin.", (width, self.y(0))) - linerror = drawable.linear_error - if linerror < 0: - msg = "?" - else: - msg = "%.2f" % linerror - #print "linear", linerror - self.putText(display, msg, (width, self.y(1))) - - self.queue_display.append(display) - - def redraw_stereo(self, drawable): - height = drawable.lscrib.shape[0] - width = drawable.lscrib.shape[1] - - display = numpy.zeros((max(480, height), 2 * width + 100, 3), dtype=numpy.uint8) - display[0:height, 0:width,:] = drawable.lscrib - display[0:height, width:2*width,:] = drawable.rscrib - display[0:height, 2*width:2*width+100,:].fill(255) - - self.buttons(display) - - if not self.c.calibrated: - if drawable.params: - for i, (label, lo, hi, progress) in enumerate(drawable.params): - (w,_) = self.getTextSize(label) - self.putText(display, label, (2 * width + (100 - w) // 2, self.y(i))) - color = (0,255,0) - if progress < 1.0: - color = (0, int(progress*255.), 255) - cv2.line(display, - (int(2 * width + lo * 100), self.y(i) + 20), - (int(2 * width + hi * 100), self.y(i) + 20), - color, 4) - - else: - self.putText(display, "epi.", (2 * width, self.y(0))) - if drawable.epierror == -1: - msg = "?" - else: - msg = "%.2f" % drawable.epierror - self.putText(display, msg, (2 * width, self.y(1))) - # TODO dim is never set anywhere. Supposed to be observed chessboard size? - if drawable.dim != -1: - self.putText(display, "dim", (2 * width, self.y(2))) - self.putText(display, "%.3f" % drawable.dim, (2 * width, self.y(3))) - - self.queue_display.append(display) diff --git a/camera_calibration_fisheye/src/camera_calibration/camera_checker.py b/camera_calibration_fisheye/src/camera_calibration/camera_checker.py deleted file mode 100644 index d27b893e0..000000000 --- a/camera_calibration_fisheye/src/camera_calibration/camera_checker.py +++ /dev/null @@ -1,200 +0,0 @@ -#!/usr/bin/python -# -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the Willow Garage nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import cv2 -import cv_bridge -import functools -import message_filters -import numpy -import rospy -import sensor_msgs.msg -import sensor_msgs.srv -import threading - -from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, ChessboardInfo -from message_filters import ApproximateTimeSynchronizer - -try: - from queue import Queue -except ImportError: - from Queue import Queue - - -def mean(seq): - return sum(seq) / len(seq) - -def lmin(seq1, seq2): - """ Pairwise minimum of two sequences """ - return [min(a, b) for (a, b) in zip(seq1, seq2)] - -def lmax(seq1, seq2): - """ Pairwise maximum of two sequences """ - return [max(a, b) for (a, b) in zip(seq1, seq2)] - -class ConsumerThread(threading.Thread): - def __init__(self, queue, function): - threading.Thread.__init__(self) - self.queue = queue - self.function = function - - def run(self): - while not rospy.is_shutdown(): - while not rospy.is_shutdown(): - m = self.queue.get() - if self.queue.empty(): - break - self.function(m) - -class CameraCheckerNode: - - def __init__(self, chess_size, dim, approximate=0): - self.board = ChessboardInfo() - self.board.n_cols = chess_size[0] - self.board.n_rows = chess_size[1] - self.board.dim = dim - - # make sure n_cols is not smaller than n_rows, otherwise error computation will be off - if self.board.n_cols < self.board.n_rows: - self.board.n_cols, self.board.n_rows = self.board.n_rows, self.board.n_cols - - image_topic = rospy.resolve_name("monocular") + "/image_rect" - camera_topic = rospy.resolve_name("monocular") + "/camera_info" - - tosync_mono = [ - (image_topic, sensor_msgs.msg.Image), - (camera_topic, sensor_msgs.msg.CameraInfo), - ] - - if approximate <= 0: - sync = message_filters.TimeSynchronizer - else: - sync = functools.partial(ApproximateTimeSynchronizer, slop=approximate) - - tsm = sync([message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10) - tsm.registerCallback(self.queue_monocular) - - left_topic = rospy.resolve_name("stereo") + "/left/image_rect" - left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info" - right_topic = rospy.resolve_name("stereo") + "/right/image_rect" - right_camera_topic = rospy.resolve_name("stereo") + "/right/camera_info" - - tosync_stereo = [ - (left_topic, sensor_msgs.msg.Image), - (left_camera_topic, sensor_msgs.msg.CameraInfo), - (right_topic, sensor_msgs.msg.Image), - (right_camera_topic, sensor_msgs.msg.CameraInfo) - ] - - tss = sync([message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo], 10) - tss.registerCallback(self.queue_stereo) - - self.br = cv_bridge.CvBridge() - - self.q_mono = Queue() - self.q_stereo = Queue() - - mth = ConsumerThread(self.q_mono, self.handle_monocular) - mth.setDaemon(True) - mth.start() - - sth = ConsumerThread(self.q_stereo, self.handle_stereo) - sth.setDaemon(True) - sth.start() - - self.mc = MonoCalibrator([self.board]) - self.sc = StereoCalibrator([self.board]) - - def queue_monocular(self, msg, cmsg): - self.q_mono.put((msg, cmsg)) - - def queue_stereo(self, lmsg, lcmsg, rmsg, rcmsg): - self.q_stereo.put((lmsg, lcmsg, rmsg, rcmsg)) - - def mkgray(self, msg): - return self.mc.mkgray(msg) - - def image_corners(self, im): - (ok, corners, b) = self.mc.get_corners(im) - if ok: - return corners - else: - return None - - def handle_monocular(self, msg): - - (image, camera) = msg - gray = self.mkgray(image) - C = self.image_corners(gray) - if C is not None: - linearity_rms = self.mc.linear_error(C, self.board) - - # Add in reprojection check - image_points = C - object_points = self.mc.mk_object_points([self.board], use_board_size=True)[0] - dist_coeffs = numpy.zeros((4, 1)) - camera_matrix = numpy.array( [ [ camera.P[0], camera.P[1], camera.P[2] ], - [ camera.P[4], camera.P[5], camera.P[6] ], - [ camera.P[8], camera.P[9], camera.P[10] ] ] ) - ok, rot, trans = cv2.solvePnP(object_points, image_points, camera_matrix, dist_coeffs) - # Convert rotation into a 3x3 Rotation Matrix - rot3x3, _ = cv2.Rodrigues(rot) - # Reproject model points into image - object_points_world = numpy.asmatrix(rot3x3) * numpy.asmatrix(object_points.squeeze().T) + numpy.asmatrix(trans) - reprojected_h = camera_matrix * object_points_world - reprojected = (reprojected_h[0:2, :] / reprojected_h[2, :]) - reprojection_errors = image_points.squeeze().T - reprojected - - reprojection_rms = numpy.sqrt(numpy.sum(numpy.array(reprojection_errors) ** 2) / numpy.product(reprojection_errors.shape)) - - # Print the results - print("Linearity RMS Error: %.3f Pixels Reprojection RMS Error: %.3f Pixels" % (linearity_rms, reprojection_rms)) - else: - print('no chessboard') - - def handle_stereo(self, msg): - - (lmsg, lcmsg, rmsg, rcmsg) = msg - lgray = self.mkgray(lmsg) - rgray = self.mkgray(rmsg) - - L = self.image_corners(lgray) - R = self.image_corners(rgray) - if L is not None and R is not None: - epipolar = self.sc.epipolar_error(L, R) - - dimension = self.sc.chessboard_size(L, R, self.board, msg=(lcmsg, rcmsg)) - - print("epipolar error: %f pixels dimension: %f m" % (epipolar, dimension)) - else: - print("no chessboard") diff --git a/camera_calibration_fisheye/test/directed.py b/camera_calibration_fisheye/test/directed.py deleted file mode 100644 index 43a7aaa4e..000000000 --- a/camera_calibration_fisheye/test/directed.py +++ /dev/null @@ -1,274 +0,0 @@ -#!/usr/bin/env python -# -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the Willow Garage nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import roslib -import rosunit -import rospy -import cv2 - -import collections -import copy -import numpy -import os -import sys -import tarfile -import unittest - -from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, \ - Patterns, CalibrationException, ChessboardInfo, image_from_archive - -board = ChessboardInfo() -board.n_cols = 8 -board.n_rows = 6 -board.dim = 0.108 - -class TestDirected(unittest.TestCase): - def setUp(self): - tar_path = roslib.packages.find_resource('camera_calibration', 'camera_calibration.tar.gz')[0] - self.tar = tarfile.open(tar_path, 'r') - self.limages = [image_from_archive(self.tar, "wide/left%04d.pgm" % i) for i in range(3, 15)] - self.rimages = [image_from_archive(self.tar, "wide/right%04d.pgm" % i) for i in range(3, 15)] - self.l = {} - self.r = {} - self.sizes = [(320,240), (640,480), (800,600), (1024,768)] - for dim in self.sizes: - self.l[dim] = [] - self.r[dim] = [] - for li,ri in zip(self.limages, self.rimages): - rli = cv2.resize(li, (dim[0], dim[1])) - rri = cv2.resize(ri, (dim[0], dim[1])) - self.l[dim].append(rli) - self.r[dim].append(rri) - - def assert_good_mono(self, c, dim, max_err): - #c.report() - self.assert_(len(c.ost()) > 0) - lin_err = 0 - n = 0 - for img in self.l[dim]: - lin_err_local = c.linear_error_from_image(img) - if lin_err_local: - lin_err += lin_err_local - n += 1 - if n > 0: - lin_err /= n - self.assert_(0.0 < lin_err, 'lin_err is %f' % lin_err) - self.assert_(lin_err < max_err, 'lin_err is %f' % lin_err) - - flat = c.remap(img) - self.assertEqual(img.shape, flat.shape) - - def test_monocular(self): - # Run the calibrator, produce a calibration, check it - mc = MonoCalibrator([ board ], cv2.CALIB_FIX_K3) - max_errs = [0.1, 0.2, 0.4, 0.7] - for i, dim in enumerate(self.sizes): - mc.cal(self.l[dim]) - self.assert_good_mono(mc, dim, max_errs[i]) - - # Make another calibration, import previous calibration as a message, - # and assert that the new one is good. - - mc2 = MonoCalibrator([board]) - mc2.from_message(mc.as_message()) - self.assert_good_mono(mc2, dim, max_errs[i]) - - def test_stereo(self): - epierrors = [0.1, 0.2, 0.45, 1.0] - for i, dim in enumerate(self.sizes): - print("Dim =", dim) - sc = StereoCalibrator([board], cv2.CALIB_FIX_K3) - sc.cal(self.l[dim], self.r[dim]) - - sc.report() - #print sc.ost() - - # NOTE: epipolar error currently increases with resolution. - # At highest res expect error ~0.75 - epierror = 0 - n = 0 - for l_img, r_img in zip(self.l[dim], self.r[dim]): - epierror_local = sc.epipolar_error_from_images(l_img, r_img) - if epierror_local: - epierror += epierror_local - n += 1 - epierror /= n - self.assert_(epierror < epierrors[i], - 'Epipolar error is %f for resolution i = %d' % (epierror, i)) - - self.assertAlmostEqual(sc.chessboard_size_from_images(self.l[dim][0], self.r[dim][0]), .108, 2) - - #print sc.as_message() - - img = self.l[dim][0] - flat = sc.l.remap(img) - self.assertEqual(img.shape, flat.shape) - flat = sc.r.remap(img) - self.assertEqual(img.shape, flat.shape) - - sc2 = StereoCalibrator([board]) - sc2.from_message(sc.as_message()) - # sc2.set_alpha(1.0) - #sc2.report() - self.assert_(len(sc2.ost()) > 0) - - def test_nochecker(self): - # Run with same images, but looking for an incorrect chessboard size (8, 7). - # Should raise an exception because of lack of input points. - new_board = copy.deepcopy(board) - new_board.n_cols = 8 - new_board.n_rows = 7 - - sc = StereoCalibrator([new_board]) - self.assertRaises(CalibrationException, lambda: sc.cal(self.limages, self.rimages)) - mc = MonoCalibrator([new_board]) - self.assertRaises(CalibrationException, lambda: mc.cal(self.limages)) - - - -class TestArtificial(unittest.TestCase): - Setup = collections.namedtuple('Setup', ['pattern', 'cols', 'rows', 'lin_err', 'K_err']) - - def setUp(self): - # Define some image transforms that will simulate a camera position - M = [] - cv2.getPerspectiveTransform - self.K = numpy.array([[500, 0, 250], [0, 500, 250], [0, 0, 1]], numpy.float32) - self.D = numpy.array([]) - # physical size of the board - self.board_width_dim = 1 - - # Generate data for different grid types. For each grid type, define the different sizes of - # grid that are recognized (n row, n col) - # Patterns.Circles, Patterns.ACircles - self.setups = [ self.Setup(pattern=Patterns.Chessboard, cols=7, rows=8, lin_err=0.2, K_err=8.2), - self.Setup(pattern=Patterns.Circles, cols=7, rows=8, lin_err=0.1, K_err=4), - self.Setup(pattern=Patterns.ACircles, cols=3, rows=5, lin_err=0.1, K_err=8) ] - self.limages = [] - self.rimages = [] - for setup in self.setups: - self.limages.append([]) - self.rimages.append([]) - - # Create the pattern - if setup.pattern == Patterns.Chessboard: - pattern = numpy.zeros((50*(setup.rows+3), 50*(setup.cols+3), 1), numpy.uint8) - pattern.fill(255) - for j in range(1, setup.rows+2): - for i in range(1+(j%2), setup.cols+2, 2): - pattern[50*j:50*(j+1), 50*i:50*(i+1)].fill(0) - elif setup.pattern == Patterns.Circles: - pattern = numpy.zeros((50*(setup.rows+2), 50*(setup.cols+2), 1), numpy.uint8) - pattern.fill(255) - for j in range(1, setup.rows+1): - for i in range(1, setup.cols+1): - cv2.circle(pattern, (50*i + 25, 50*j + 25), 15, (0,0,0), -1 ) - elif setup.pattern == Patterns.ACircles: - x = 60 - pattern = numpy.zeros((x*(setup.rows+2), x*(setup.cols+5), 1), numpy.uint8) - pattern.fill(255) - for j in range(1, setup.rows+1): - for i in range(0, setup.cols): - cv2.circle(pattern, (x*(1 + 2*i + (j%2)) + x/2, x*j + x/2), x/3, (0,0,0), -1) - - rows, cols, _ = pattern.shape - object_points_2d = numpy.array([[0, 0], [0, cols-1], [rows-1, cols-1], [rows-1, 0]], numpy.float32) - object_points_3d = numpy.array([[0, 0, 0], [0, cols-1, 0], [rows-1, cols-1, 0], [rows-1, 0, 0]], numpy.float32) - object_points_3d *= self.board_width_dim/float(cols) - - # create the artificial view points - rvec = [ [0, 0, 0], [0, 0, 0.4], [0, 0.4, 0], [0.4, 0, 0], [0.4, 0.4, 0], [0.4, 0, 0.4], [0, 0.4, 0.4], [0.4, 0.4, 0.4] ] - tvec = [ [-0.5, -0.5, 3], [-0.5, -0.5, 3], [-0.5, -0.1, 3], [-0.1, -0.5, 3], [-0.1, -0.1, 3], [-0.1, -0.5, 3], [-0.5, -0.1, 3], [-0.1, 0.1, 3] ] - - dsize = (480, 640) - for i in range(len(rvec)): - R = numpy.array(rvec[i], numpy.float32) - T = numpy.array(tvec[i], numpy.float32) - - image_points, _ = cv2.projectPoints(object_points_3d, R, T, self.K, self.D) - - # deduce the perspective transform - M.append(cv2.getPerspectiveTransform(object_points_2d, image_points)) - - # project the pattern according to the different cameras - pattern_warped = cv2.warpPerspective(pattern, M[i], dsize) - self.limages[-1].append(pattern_warped) - - def assert_good_mono(self, c, images, max_err): - #c.report() - self.assert_(len(c.ost()) > 0) - lin_err = 0 - n = 0 - for img in images: - lin_err_local = c.linear_error_from_image(img) - if lin_err_local: - lin_err += lin_err_local - n += 1 - if n > 0: - lin_err /= n - print("linear error is %f" % lin_err) - self.assert_(0.0 < lin_err, 'lin_err is %f' % lin_err) - self.assert_(lin_err < max_err, 'lin_err is %f' % lin_err) - - flat = c.remap(img) - self.assertEqual(img.shape, flat.shape) - - def test_monocular(self): - # Run the calibrator, produce a calibration, check it - for i, setup in enumerate(self.setups): - board = ChessboardInfo() - board.n_cols = setup.cols - board.n_rows = setup.rows - board.dim = self.board_width_dim - - mc = MonoCalibrator([ board ], flags=cv2.CALIB_FIX_K3, pattern=setup.pattern) - - if 0: - # display the patterns viewed by the camera - for pattern_warped in self.limages[i]: - cv2.imshow("toto", pattern_warped) - cv2.waitKey(0) - - mc.cal(self.limages[i]) - self.assert_good_mono(mc, self.limages[i], setup.lin_err) - - # Make sure the intrinsics are similar - err_intrinsics = numpy.linalg.norm(mc.intrinsics - self.K, ord=numpy.inf) - self.assert_(err_intrinsics < setup.K_err, - 'intrinsics error is %f for resolution i = %d' % (err_intrinsics, i)) - print('intrinsics error is %f' % numpy.linalg.norm(mc.intrinsics - self.K, ord=numpy.inf)) - -if __name__ == '__main__': - #rosunit.unitrun('camera_calibration', 'directed', TestDirected) - rosunit.unitrun('camera_calibration', 'artificial', TestArtificial) diff --git a/camera_calibration_fisheye/test/multiple_boards.py b/camera_calibration_fisheye/test/multiple_boards.py deleted file mode 100644 index a726a703a..000000000 --- a/camera_calibration_fisheye/test/multiple_boards.py +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env python -# -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the Willow Garage nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import roslib -import rostest -import rospy -import unittest -import tarfile -import copy -import os, sys - -from camera_calibration.calibrator import StereoCalibrator, ChessboardInfo, image_from_archive - -# Large board used for PR2 calibration -board = ChessboardInfo() -board.n_cols = 7 -board.n_rows = 6 -board.dim = 0.108 - -class TestMultipleBoards(unittest.TestCase): - def test_multiple_boards(self): - small_board = ChessboardInfo() - small_board.n_cols = 5 - small_board.n_rows = 4 - small_board.dim = 0.025 - - stereo_cal = StereoCalibrator([board, small_board]) - - my_archive_name = roslib.packages.find_resource('camera_calibration', 'multi_board_calibration.tar.gz')[0] - stereo_cal.do_tarfile_calibration(my_archive_name) - - stereo_cal.report() - stereo_cal.ost() - - # Check error for big image - archive = tarfile.open(my_archive_name) - l1_big = image_from_archive(archive, "left-0000.png") - r1_big = image_from_archive(archive, "right-0000.png") - epi_big = stereo_cal.epipolar_error_from_images(l1_big, r1_big) - self.assert_(epi_big < 1.0, "Epipolar error for large checkerboard > 1.0. Error: %.2f" % epi_big) - - # Small checkerboard has larger error threshold for now - l1_sm = image_from_archive(archive, "left-0012-sm.png") - r1_sm = image_from_archive(archive, "right-0012-sm.png") - epi_sm = stereo_cal.epipolar_error_from_images(l1_sm, r1_sm) - self.assert_(epi_sm < 2.0, "Epipolar error for small checkerboard > 2.0. Error: %.2f" % epi_sm) - - - -if __name__ == '__main__': - if 1: - rostest.unitrun('camera_calibration', 'test_multi_board_cal', TestMultipleBoards) - else: - suite = unittest.TestSuite() - suite.addTest(TestMultipleBoards('test_multi_board_cal')) - unittest.TextTestRunner(verbosity=2).run(suite) From 0c8f8cec1c541d258eca46c93485bdeae64bbdd4 Mon Sep 17 00:00:00 2001 From: DavidTorresOcana Date: Mon, 28 Oct 2019 01:08:43 +0100 Subject: [PATCH 03/10] Correct typo --- camera_calibration/src/camera_calibration/calibrator.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index b2fc01364..defe527b6 100755 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -1025,7 +1025,7 @@ def set_alpha(self, a): self.T, self.l.R, self.r.R, self.l.P, self.r.P, alpha = a) - + cv2.initUndistortRectifyMap(self.l.intrinsics, self.l.distortion, self.l.R, self.l.P, self.size, cv2.CV_32FC1, self.l.mapx, self.l.mapy) cv2.initUndistortRectifyMap(self.r.intrinsics, self.r.distortion, self.r.R, self.r.P, self.size, cv2.CV_32FC1, @@ -1066,7 +1066,7 @@ def as_message(self): """ return (self.lrmsg(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P, self.size,self.l.distortion_model), - self.lrmsg(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P, self.size,seld.r.distortion_model)) + self.lrmsg(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P, self.size,self.r.distortion_model)) def from_message(self, msgs, alpha = 0.0): """ Initialize the camera calibration from a pair of CameraInfo messages. """ From e47b48c088fbe96a31bc98136b545ed21b62279d Mon Sep 17 00:00:00 2001 From: DavidTorresOcana Date: Mon, 28 Oct 2019 21:37:34 +0100 Subject: [PATCH 04/10] Restore camera_calib files permisions --- camera_calibration/CHANGELOG.rst | 0 camera_calibration/CMakeLists.txt | 0 camera_calibration/button.jpg | Bin camera_calibration/doc/conf.py | 0 camera_calibration/doc/index.rst | 0 camera_calibration/mainpage.dox | 0 camera_calibration/package.xml | 0 camera_calibration/rosdoc.yaml | 0 camera_calibration/setup.py | 0 .../src/camera_calibration/__init__.py | 0 .../src/camera_calibration/calibrator.py | 0 camera_calibration/test/directed.py | 0 camera_calibration/test/multiple_boards.py | 0 13 files changed, 0 insertions(+), 0 deletions(-) mode change 100755 => 100644 camera_calibration/CHANGELOG.rst mode change 100755 => 100644 camera_calibration/CMakeLists.txt mode change 100755 => 100644 camera_calibration/button.jpg mode change 100755 => 100644 camera_calibration/doc/conf.py mode change 100755 => 100644 camera_calibration/doc/index.rst mode change 100755 => 100644 camera_calibration/mainpage.dox mode change 100755 => 100644 camera_calibration/package.xml mode change 100755 => 100644 camera_calibration/rosdoc.yaml mode change 100755 => 100644 camera_calibration/setup.py mode change 100755 => 100644 camera_calibration/src/camera_calibration/__init__.py mode change 100755 => 100644 camera_calibration/src/camera_calibration/calibrator.py mode change 100755 => 100644 camera_calibration/test/directed.py mode change 100755 => 100644 camera_calibration/test/multiple_boards.py diff --git a/camera_calibration/CHANGELOG.rst b/camera_calibration/CHANGELOG.rst old mode 100755 new mode 100644 diff --git a/camera_calibration/CMakeLists.txt b/camera_calibration/CMakeLists.txt old mode 100755 new mode 100644 diff --git a/camera_calibration/button.jpg b/camera_calibration/button.jpg old mode 100755 new mode 100644 diff --git a/camera_calibration/doc/conf.py b/camera_calibration/doc/conf.py old mode 100755 new mode 100644 diff --git a/camera_calibration/doc/index.rst b/camera_calibration/doc/index.rst old mode 100755 new mode 100644 diff --git a/camera_calibration/mainpage.dox b/camera_calibration/mainpage.dox old mode 100755 new mode 100644 diff --git a/camera_calibration/package.xml b/camera_calibration/package.xml old mode 100755 new mode 100644 diff --git a/camera_calibration/rosdoc.yaml b/camera_calibration/rosdoc.yaml old mode 100755 new mode 100644 diff --git a/camera_calibration/setup.py b/camera_calibration/setup.py old mode 100755 new mode 100644 diff --git a/camera_calibration/src/camera_calibration/__init__.py b/camera_calibration/src/camera_calibration/__init__.py old mode 100755 new mode 100644 diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py old mode 100755 new mode 100644 diff --git a/camera_calibration/test/directed.py b/camera_calibration/test/directed.py old mode 100755 new mode 100644 diff --git a/camera_calibration/test/multiple_boards.py b/camera_calibration/test/multiple_boards.py old mode 100755 new mode 100644 From 80bf0836e88446ec4766a2a5a0c05dc9f512dd7f Mon Sep 17 00:00:00 2001 From: DavidTorresOcana Date: Tue, 29 Oct 2019 00:31:50 +0100 Subject: [PATCH 05/10] Upgrades to calibrator tool for multi model calibration --- .../src/camera_calibration/calibrator.py | 83 ++++++++++--------- .../camera_calibration/camera_calibrator.py | 6 +- 2 files changed, 46 insertions(+), 43 deletions(-) diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index defe527b6..70bcc3a0e 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -49,7 +49,13 @@ import tarfile import time from distutils.version import LooseVersion +import sys +from enum import Enum +# Supported camera models +class CAMERA_MODEL(Enum): + PINHOLE = 0 + FISHEYE = 1 # Supported calibration patterns class Patterns: @@ -213,6 +219,18 @@ def _get_circles(img, board, pattern): return (ok, corners) +def _get_dist_model(dist_params, cam_model): + # Select dist model + if CAMERA_MODEL.PINHOLE == cam_model: + if dist_params.size > 5: + dist_model = "rational_polynomial" + else: + dist_model = "plumb_bob" + elif CAMERA_MODEL.FISHEYE == cam_model: + dist_model = "fisheye" + else: + dist_model = "unknown" + return dist_model # TODO self.size needs to come from CameraInfo, full resolution class Calibrator(object): @@ -238,7 +256,7 @@ def __init__(self, boards, flags=0, pattern=Patterns.Chessboard, name='', self.checkerboard_flags = checkerboard_flags self.pattern = pattern self.br = cv_bridge.CvBridge() - self.distortion_model = "pinhole" + self.camera_model = CAMERA_MODEL.PINHOLE # self.db is list of (parameters, image) samples for use in calibration. parameters has form # (X, Y, size, skew) all normalized to [0,1], to keep track of what sort of samples we've taken # and ensure enough variety. @@ -292,8 +310,8 @@ def get_parameters(self, corners, board, size): skew = _get_skew(corners, board) params = [p_x, p_y, p_size, skew] return params - def set_distmodel(self,modelname): - self.distortion_model = modelname + def set_cammodel(self, modeltype): + self.camera_model = modeltype def is_slow_moving(self, corners, last_frame_corners): """ @@ -454,19 +472,12 @@ def downsample_and_detect(self, img): return (scrib, corners, downsampled_corners, board, (x_scale, y_scale)) - @staticmethod - def lrmsg(d, k, r, p, size, distortion_model): + def lrmsg(d, k, r, p, size, camera_model): """ Used by :meth:`as_message`. Return a CameraInfo message for the given calibration matrices """ msg = sensor_msgs.msg.CameraInfo() msg.width, msg.height = size - if "pinhole" in distortion_model: - if d.size > 5: - msg.distortion_model = "rational_polynomial" - else: - msg.distortion_model = "plumb_bob" - elif "fisheye" in distortion_model: - msg.distortion_model = "fisheye" + msg.camera_model = _get_dist_model(d, camera_model) msg.D = numpy.ravel(d).copy().tolist() msg.K = numpy.ravel(k).copy().tolist() @@ -523,21 +534,14 @@ def lrost(name, d, k, r, p, size): return calmessage @staticmethod - def lryaml(name, d, k, r, p, size,dist_model): + def lryaml(name, d, k, r, p, size, cam_model): def format_mat(x, precision): return ("[%s]" % ( numpy.array2string(x, precision=precision, suppress_small=True, separator=", ") .replace("[", "").replace("]", "").replace("\n", "\n ") )) - # Select dist model - if "pinhole" in dist_model: - if d.size > 5: - distortion_model = "rational_polynomial" - else: - distortion_model = "plumb_bob" - elif "fisheye" in dist_model: - distortion_model = "fisheye" + dist_model = _get_dist_model(d, cam_model) assert k.shape == (3, 3) assert r.shape == (3, 3) @@ -550,7 +554,7 @@ def format_mat(x, precision): " rows: 3", " cols: 3", " data: " + format_mat(k, 5), - "distortion_model: " + distortion_model, + "camera_model: " + dist_model, "distortion_coefficients:", " rows: 1", " cols: %d" % d.size, @@ -664,7 +668,7 @@ def cal_fromcorners(self, good): # If FIX_ASPECT_RATIO flag set, enforce focal lengths have 1/1 ratio intrinsics_in = numpy.eye(3, dtype=numpy.float64) - if "pinhole" in self.distortion_model: + if self.camera_model == CAMERA_MODEL.PINHOLE: reproj_err, self.intrinsics, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera( opts, ipts, self.size, @@ -674,13 +678,11 @@ def cal_fromcorners(self, good): # OpenCV returns more than 8 coefficients (the additional ones all zeros) when CALIB_RATIONAL_MODEL is set. # The extra ones include e.g. thin prism coefficients, which we are not interested in. self.distortion = dist_coeffs.flat[:8].reshape(-1, 1) - elif "fisheye" in self.distortion_model: + elif self.camera_model == CAMERA_MODEL.FISHEYE: calibration_flags = cv2.fisheye.CALIB_FIX_SKEW + self.calib_flags # Add user flags reproj_err, self.intrinsics, self.distortion, rvecs, tvecs = cv2.fisheye.calibrate( - opts, ipts, - self.size, - intrinsics_in, None, flags = calibration_flags, - criteria = (cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6)) + opts, ipts, self.size, + intrinsics_in, None, flags = calibration_flags) else: print("Something went wrong when selecting a model") # R is identity matrix for monocular calibration @@ -697,7 +699,7 @@ def set_alpha(self, a): original image are in calibrated image). """ - if "pinhole" in self.distortion_model: + if self.camera_model == CAMERA_MODEL.PINHOLE: # NOTE: Prior to Electric, this code was broken such that we never actually saved the new # camera matrix. In effect, this enforced P = [K|0] for monocular cameras. # TODO: Verify that OpenCV #1199 gets applied (improved GetOptimalNewCameraMatrix) @@ -706,7 +708,7 @@ def set_alpha(self, a): for i in range(3): self.P[j,i] = ncm[j, i] self.mapx, self.mapy = cv2.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, ncm, self.size, cv2.CV_32FC1) - elif "fisheye" in self.distortion_model: + elif self.camera_model == CAMERA_MODEL.FISHEYE: # TODO: Implement re-computation of P given users balance value # ncm = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(self.intrinsics, self.distortion, self.size, self.R, balance=a) for j in range(3): @@ -740,7 +742,7 @@ def undistort_points(self, src): def as_message(self): """ Return the camera calibration as a CameraInfo message """ - return self.lrmsg(self.distortion, self.intrinsics, self.R, self.P, self.size,self.distortion_model) + return self.lrmsg(self.distortion, self.intrinsics, self.R, self.P, self.size, self.camera_model) def from_message(self, msg, alpha = 0.0): """ Initialize the camera calibration from a CameraInfo message """ @@ -760,7 +762,7 @@ def ost(self): return self.lrost(self.name, self.distortion, self.intrinsics, self.R, self.P, self.size) def yaml(self): - return self.lryaml(self.name, self.distortion, self.intrinsics, self.R, self.P, self.size,self.distortion_model) + return self.lryaml(self.name, self.distortion, self.intrinsics, self.R, self.P, self.size, self.camera_model) def linear_error_from_image(self, image): """ @@ -973,7 +975,7 @@ def cal_fromcorners(self, good): self.T = numpy.zeros((3, 1), dtype=numpy.float64) self.R = numpy.eye(3, dtype=numpy.float64) - if "pinhole" in self.distortion_model: + if self.camera_model == CAMERA_MODEL.PINHOLE: if LooseVersion(cv2.__version__).version[0] == 2: cv2.stereoCalibrate(opts, lipts, ripts, self.size, self.l.intrinsics, self.l.distortion, @@ -991,9 +993,10 @@ def cal_fromcorners(self, good): self.T, # T criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), flags = flags) - elif "fisheye" in self.distortion_model: + elif self.camera_model == CAMERA_MODEL.FISHEYE: if LooseVersion(cv2.__version__).version[0] == 2: - print("You need OpenCV >3 to use fisheye camera model") + print("ERROR: You need OpenCV >3 to use fisheye camera model") + sys.exit() else: cv2.fisheye.stereoCalibrate(opts, lipts, ripts, self.l.intrinsics, self.l.distortion, @@ -1015,7 +1018,7 @@ def set_alpha(self, a): in calibrated image are valid) to 1 (zoomed out, all pixels in original image are in calibrated image). """ - if "pinhole" in self.distortion_model: + if self.camera_model == CAMERA_MODEL.PINHOLE: cv2.stereoRectify(self.l.intrinsics, self.l.distortion, self.r.intrinsics, @@ -1031,7 +1034,7 @@ def set_alpha(self, a): cv2.initUndistortRectifyMap(self.r.intrinsics, self.r.distortion, self.r.R, self.r.P, self.size, cv2.CV_32FC1, self.r.mapx, self.r.mapy) - elif "fisheye" in self.distortion_model: + elif self.camera_model == CAMERA_MODEL.FISHEYE: self.Q = numpy.zeros((4,4), dtype=numpy.float64) flags = cv2.CALIB_ZERO_DISPARITY # Operation flags that may be zero or CALIB_ZERO_DISPARITY . @@ -1065,8 +1068,8 @@ def as_message(self): and right cameras respectively. """ - return (self.lrmsg(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P, self.size,self.l.distortion_model), - self.lrmsg(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P, self.size,self.r.distortion_model)) + return (self.lrmsg(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P, self.size, self.l.camera_model), + self.lrmsg(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P, self.size, self.r.camera_model)) def from_message(self, msgs, alpha = 0.0): """ Initialize the camera calibration from a pair of CameraInfo messages. """ @@ -1094,7 +1097,7 @@ def ost(self): self.lrost(self.name + "/right", self.r.distortion, self.r.intrinsics, self.r.R, self.r.P, self.size)) def yaml(self, suffix, info): - return self.lryaml(self.name + suffix, info.distortion, info.intrinsics, info.R, info.P, self.size,self.distortion_model) + return self.lryaml(self.name + suffix, info.distortion, info.intrinsics, info.R, info.P, self.size,self.camera_model) # TODO Get rid of "from_images" versions of these, instead have function to get undistorted corners def epipolar_error_from_images(self, limage, rimage): diff --git a/camera_calibration/src/camera_calibration/camera_calibrator.py b/camera_calibration/src/camera_calibration/camera_calibrator.py index 9e0102891..bf8931daa 100755 --- a/camera_calibration/src/camera_calibration/camera_calibrator.py +++ b/camera_calibration/src/camera_calibration/camera_calibrator.py @@ -47,7 +47,7 @@ from queue import Queue except ImportError: from Queue import Queue - +from calibrator import CAMERA_MODEL class BufferQueue(Queue): """Slight modification of the standard Queue that discards the oldest item @@ -275,8 +275,8 @@ def on_mouse(self, event, x, y, flags, param): # Only shut down if we set camera info correctly, #3993 if self.do_upload(): rospy.signal_shutdown('Quit') - def on_model_change(self,model_select_val): - self.c.set_distmodel( "pinhole" if model_select_val < 0.5 else "fisheye") + def on_model_change(self, model_select_val): + self.c.set_cammodel( CAMERA_MODEL.PINHOLE if model_select_val < 0.5 else CAMERA_MODEL.FISHEYE) def on_scale(self, scalevalue): if self.c.calibrated: From eb285f3b131975ed7465cc75e86d5d59a6880081 Mon Sep 17 00:00:00 2001 From: DavidTorresOcana Date: Tue, 29 Oct 2019 00:57:52 +0100 Subject: [PATCH 06/10] Solve fisheye balance selection TODO: For some reason estimateNewCameraMatrixForUndistortRectify is not producing the expected result, hence a workaround was implemented --- .../src/camera_calibration/calibrator.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index 70bcc3a0e..9f6234f61 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -477,7 +477,7 @@ def lrmsg(d, k, r, p, size, camera_model): """ Used by :meth:`as_message`. Return a CameraInfo message for the given calibration matrices """ msg = sensor_msgs.msg.CameraInfo() msg.width, msg.height = size - msg.camera_model = _get_dist_model(d, camera_model) + msg.distortion_model = _get_dist_model(d, camera_model) msg.D = numpy.ravel(d).copy().tolist() msg.K = numpy.ravel(k).copy().tolist() @@ -709,13 +709,12 @@ def set_alpha(self, a): self.P[j,i] = ncm[j, i] self.mapx, self.mapy = cv2.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, ncm, self.size, cv2.CV_32FC1) elif self.camera_model == CAMERA_MODEL.FISHEYE: - # TODO: Implement re-computation of P given users balance value # ncm = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(self.intrinsics, self.distortion, self.size, self.R, balance=a) - for j in range(3): - for i in range(3): - self.P[j,i] = self.intrinsics[j, i] - # self.P[j,i] = ncm[j, i] - + # self.P[:3,:3] = ncm[:3,:3] + # NOTE: estimateNewCameraMatrixForUndistortRectify not producing proper results, using a naive approach instead: + self.P[:3,:3] = self.intrinsics[:3,:3] + self.P[0,0] /= (1. + a) + self.P[1,1] /= (1. + a) self.mapx, self.mapy = cv2.fisheye.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, self.P, self.size, cv2.CV_32FC1) else: print("Something went wrong when selecting a model") From bc4f2e3fd4bf1820035803bfe47277be5906519b Mon Sep 17 00:00:00 2001 From: DavidTorresOcana Date: Tue, 29 Oct 2019 10:12:42 +0100 Subject: [PATCH 07/10] Add fisheye calibration flags as user arguments --- camera_calibration/nodes/cameracalibrator.py | 67 +++++++++++++++---- .../src/camera_calibration/calibrator.py | 8 +-- .../camera_calibration/camera_calibrator.py | 13 ++-- 3 files changed, 65 insertions(+), 23 deletions(-) diff --git a/camera_calibration/nodes/cameracalibrator.py b/camera_calibration/nodes/cameracalibrator.py index 2a6cb5e49..d05051cc8 100755 --- a/camera_calibration/nodes/cameracalibrator.py +++ b/camera_calibration/nodes/cameracalibrator.py @@ -73,24 +73,44 @@ def main(): help="image queue size (default %default, set to 0 for unlimited)") parser.add_option_group(group) group = OptionGroup(parser, "Calibration Optimizer Options") - group.add_option("--fix-principal-point", + group.add_option("--pinhole-fix-principal-point", action="store_true", default=False, - help="fix the principal point at the image center") - group.add_option("--fix-aspect-ratio", + help="for pinhole, fix the principal point at the image center") + group.add_option("--pinhole-fix-aspect-ratio", action="store_true", default=False, - help="enforce focal lengths (fx, fy) are equal") - group.add_option("--zero-tangent-dist", + help="for pinhole, enforce focal lengths (fx, fy) are equal") + group.add_option("--pinhole-zero-tangent-dist", action="store_true", default=False, - help="set tangential distortion coefficients (p1, p2) to zero") - group.add_option("-k", "--k-coefficients", + help="for pinhole, set tangential distortion coefficients (p1, p2) to zero") + group.add_option("--pinhole-k-coefficients", type="int", default=2, metavar="NUM_COEFFS", - help="number of radial distortion coefficients to use (up to 6, default %default)") + help="for pinhole, number of radial distortion coefficients to use (up to 6, default %default)") + + group.add_option("--fisheye-recompute-extrinsicsts", + action="store_true", default=False, + help="for fisheye, extrinsic will be recomputed after each iteration of intrinsic optimization") + group.add_option("--fisheye-fix-skew", + action="store_true", default=False, + help="for fisheye, skew coefficient (alpha) is set to zero and stay zero") + group.add_option("--fisheye-fix-principal-point", + action="store_true", default=False, + help="for fisheye,fix the principal point at the image center") + group.add_option("--fisheye-k-coefficients", + type="int", default=4, metavar="NUM_COEFFS", + help="for fisheye, number of radial distortion coefficients to use fixing to zero the rest (up to 4, default %default)") + + group.add_option("--fisheye-check-conditions", + action="store_true", default=False, + help="for fisheye, the functions will check validity of condition number") + group.add_option("--disable_calib_cb_fast_check", action='store_true', default=False, help="uses the CALIB_CB_FAST_CHECK flag for findChessboardCorners") group.add_option("--max-chessboard-speed", type="float", default=-1.0, help="Do not use samples where the calibration pattern is moving faster \ than this speed in px/frame. Set to eg. 0.5 for rolling shutter cameras.") + parser.add_option_group(group) + options, args = parser.parse_args() if len(options.size) != len(options.square): @@ -110,14 +130,15 @@ def main(): else: sync = functools.partial(ApproximateTimeSynchronizer, slop=options.approximate) - num_ks = options.k_coefficients + # Pinhole opencv calibration options parsing + num_ks = options.pinhole_k_coefficients calib_flags = 0 - if options.fix_principal_point: + if options.pinhole_fix_principal_point: calib_flags |= cv2.CALIB_FIX_PRINCIPAL_POINT - if options.fix_aspect_ratio: + if options.pinhole_fix_aspect_ratio: calib_flags |= cv2.CALIB_FIX_ASPECT_RATIO - if options.zero_tangent_dist: + if options.pinhole_zero_tangent_dist: calib_flags |= cv2.CALIB_ZERO_TANGENT_DIST if (num_ks > 3): calib_flags |= cv2.CALIB_RATIONAL_MODEL @@ -134,6 +155,26 @@ def main(): if (num_ks < 1): calib_flags |= cv2.CALIB_FIX_K1 + # Opencv calibration flags parsing: + num_ks = options.fisheye_k_coefficients + fisheye_calib_flags = 0 + if options.fisheye_fix_principal_point: + fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_PRINCIPAL_POINT + if options.fisheye_fix_skew: + fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_SKEW + if options.fisheye_recompute_extrinsicsts: + fisheye_calib_flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + if options.fisheye_check_conditions: + fisheye_calib_flags |= cv2.fisheye.CALIB_CHECK_COND + if (num_ks < 4): + fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_K4 + if (num_ks < 3): + fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_K3 + if (num_ks < 2): + fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_K2 + if (num_ks < 1): + fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_K1 + pattern = Patterns.Chessboard if options.pattern == 'circles': pattern = Patterns.Circles @@ -148,7 +189,7 @@ def main(): checkerboard_flags = cv2.CALIB_CB_FAST_CHECK rospy.init_node('cameracalibrator') - node = OpenCVCalibrationNode(boards, options.service_check, sync, calib_flags, pattern, options.camera_name, + node = OpenCVCalibrationNode(boards, options.service_check, sync, calib_flags, fisheye_calib_flags, pattern, options.camera_name, checkerboard_flags=checkerboard_flags, max_chessboard_speed=options.max_chessboard_speed, queue_size=options.queue_size) rospy.spin() diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index 9f6234f61..0210f9f12 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -237,7 +237,7 @@ class Calibrator(object): """ Base class for calibration system """ - def __init__(self, boards, flags=0, pattern=Patterns.Chessboard, name='', + def __init__(self, boards, flags=0, fisheye_flags = 0, pattern=Patterns.Chessboard, name='', checkerboard_flags=cv2.CALIB_CB_FAST_CHECK, max_chessboard_speed = -1.0): # Ordering the dimensions for the different detectors is actually a minefield... if pattern == Patterns.Chessboard: @@ -253,6 +253,7 @@ def __init__(self, boards, flags=0, pattern=Patterns.Chessboard, name='', # Set to true after we perform calibration self.calibrated = False self.calib_flags = flags + self.fisheye_calib_flags = fisheye_flags self.checkerboard_flags = checkerboard_flags self.pattern = pattern self.br = cv_bridge.CvBridge() @@ -679,10 +680,9 @@ def cal_fromcorners(self, good): # The extra ones include e.g. thin prism coefficients, which we are not interested in. self.distortion = dist_coeffs.flat[:8].reshape(-1, 1) elif self.camera_model == CAMERA_MODEL.FISHEYE: - calibration_flags = cv2.fisheye.CALIB_FIX_SKEW + self.calib_flags # Add user flags reproj_err, self.intrinsics, self.distortion, rvecs, tvecs = cv2.fisheye.calibrate( opts, ipts, self.size, - intrinsics_in, None, flags = calibration_flags) + intrinsics_in, None, flags = self.fisheye_calib_flags) else: print("Something went wrong when selecting a model") # R is identity matrix for monocular calibration @@ -1096,7 +1096,7 @@ def ost(self): self.lrost(self.name + "/right", self.r.distortion, self.r.intrinsics, self.r.R, self.r.P, self.size)) def yaml(self, suffix, info): - return self.lryaml(self.name + suffix, info.distortion, info.intrinsics, info.R, info.P, self.size,self.camera_model) + return self.lryaml(self.name + suffix, info.distortion, info.intrinsics, info.R, info.P, self.size, self.camera_model) # TODO Get rid of "from_images" versions of these, instead have function to get undistorted corners def epipolar_error_from_images(self, limage, rimage): diff --git a/camera_calibration/src/camera_calibration/camera_calibrator.py b/camera_calibration/src/camera_calibration/camera_calibrator.py index bf8931daa..5065cb831 100755 --- a/camera_calibration/src/camera_calibration/camera_calibrator.py +++ b/camera_calibration/src/camera_calibration/camera_calibrator.py @@ -110,8 +110,8 @@ def run(self): class CalibrationNode: def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0, - pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0, max_chessboard_speed = -1, - queue_size = 1): + fisheye_flags = 0, pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0, + max_chessboard_speed = -1, queue_size = 1): if service_check: # assume any non-default service names have been set. Wait for the service to become ready for svcname in ["camera", "left_camera", "right_camera"]: @@ -128,6 +128,7 @@ def __init__(self, boards, service_check = True, synchronizer = message_filters. self._boards = boards self._calib_flags = flags + self._fisheye_calib_flags = fisheye_flags self._checkerboard_flags = checkerboard_flags self._pattern = pattern self._camera_name = camera_name @@ -176,11 +177,11 @@ def queue_stereo(self, lmsg, rmsg): def handle_monocular(self, msg): if self.c == None: if self._camera_name: - self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name, + self.c = MonoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern, name=self._camera_name, checkerboard_flags=self._checkerboard_flags, max_chessboard_speed = self._max_chessboard_speed) else: - self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, + self.c = MonoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern, checkerboard_flags=self.checkerboard_flags, max_chessboard_speed = self._max_chessboard_speed) @@ -192,11 +193,11 @@ def handle_monocular(self, msg): def handle_stereo(self, msg): if self.c == None: if self._camera_name: - self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name, + self.c = StereoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern, name=self._camera_name, checkerboard_flags=self._checkerboard_flags, max_chessboard_speed = self._max_chessboard_speed) else: - self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, + self.c = StereoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern, checkerboard_flags=self._checkerboard_flags, max_chessboard_speed = self._max_chessboard_speed) From 79c005cbd104b8e8108045d95d487c1ab4b5e695 Mon Sep 17 00:00:00 2001 From: DavidTorresOcana Date: Tue, 29 Oct 2019 10:17:41 +0100 Subject: [PATCH 08/10] Add undistortion of points for fisheye --- camera_calibration/src/camera_calibration/calibrator.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index 0210f9f12..957dd91ec 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -736,8 +736,12 @@ def undistort_points(self, src): Apply the post-calibration undistortion to the source points """ - - return cv2.undistortPoints(src, self.intrinsics, self.distortion, R = self.R, P = self.P) + if self.camera_model == CAMERA_MODEL.PINHOLE: + return cv2.undistortPoints(src, self.intrinsics, self.distortion, R = self.R, P = self.P) + elif self.camera_model == CAMERA_MODEL.FISHEYE: + return cv2.fisheye.undistortPoints(src, self.intrinsics, self.distortion, R = self.R, P = self.P) + else: + print("Something went wrong when selecting a model") def as_message(self): """ Return the camera calibration as a CameraInfo message """ From 6c50c6b574dc51cffa3ceb9d923fba6329c7f4da Mon Sep 17 00:00:00 2001 From: DavidTorresOcana Date: Thu, 31 Oct 2019 13:30:02 +0100 Subject: [PATCH 09/10] cam_calib: rolling back flags Rolling back changes to previous commit on camera calibrator flags to enable backwards compatibility --- camera_calibration/nodes/cameracalibrator.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/camera_calibration/nodes/cameracalibrator.py b/camera_calibration/nodes/cameracalibrator.py index d05051cc8..379cf3ffe 100755 --- a/camera_calibration/nodes/cameracalibrator.py +++ b/camera_calibration/nodes/cameracalibrator.py @@ -73,16 +73,16 @@ def main(): help="image queue size (default %default, set to 0 for unlimited)") parser.add_option_group(group) group = OptionGroup(parser, "Calibration Optimizer Options") - group.add_option("--pinhole-fix-principal-point", + group.add_option("--fix-principal-point", action="store_true", default=False, help="for pinhole, fix the principal point at the image center") - group.add_option("--pinhole-fix-aspect-ratio", + group.add_option("--fix-aspect-ratio", action="store_true", default=False, help="for pinhole, enforce focal lengths (fx, fy) are equal") - group.add_option("--pinhole-zero-tangent-dist", + group.add_option("--zero-tangent-dist", action="store_true", default=False, help="for pinhole, set tangential distortion coefficients (p1, p2) to zero") - group.add_option("--pinhole-k-coefficients", + group.add_option("-k", "--k-coefficients", type="int", default=2, metavar="NUM_COEFFS", help="for pinhole, number of radial distortion coefficients to use (up to 6, default %default)") @@ -131,14 +131,14 @@ def main(): sync = functools.partial(ApproximateTimeSynchronizer, slop=options.approximate) # Pinhole opencv calibration options parsing - num_ks = options.pinhole_k_coefficients + num_ks = options.k_coefficients calib_flags = 0 - if options.pinhole_fix_principal_point: + if options.fix_principal_point: calib_flags |= cv2.CALIB_FIX_PRINCIPAL_POINT - if options.pinhole_fix_aspect_ratio: + if options.fix_aspect_ratio: calib_flags |= cv2.CALIB_FIX_ASPECT_RATIO - if options.pinhole_zero_tangent_dist: + if options.zero_tangent_dist: calib_flags |= cv2.CALIB_ZERO_TANGENT_DIST if (num_ks > 3): calib_flags |= cv2.CALIB_RATIONAL_MODEL From 10e5c279085632a383e28b148c25deb6e6c4cbdd Mon Sep 17 00:00:00 2001 From: DavidTorresOcana Date: Tue, 5 Nov 2019 21:27:00 +0100 Subject: [PATCH 10/10] cam_calib: Style formating --- .../src/camera_calibration/calibrator.py | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index 957dd91ec..0fa3d508d 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -683,8 +683,7 @@ def cal_fromcorners(self, good): reproj_err, self.intrinsics, self.distortion, rvecs, tvecs = cv2.fisheye.calibrate( opts, ipts, self.size, intrinsics_in, None, flags = self.fisheye_calib_flags) - else: - print("Something went wrong when selecting a model") + # R is identity matrix for monocular calibration self.R = numpy.eye(3, dtype=numpy.float64) self.P = numpy.zeros((3, 4), dtype=numpy.float64) @@ -709,15 +708,11 @@ def set_alpha(self, a): self.P[j,i] = ncm[j, i] self.mapx, self.mapy = cv2.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, ncm, self.size, cv2.CV_32FC1) elif self.camera_model == CAMERA_MODEL.FISHEYE: - # ncm = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(self.intrinsics, self.distortion, self.size, self.R, balance=a) - # self.P[:3,:3] = ncm[:3,:3] # NOTE: estimateNewCameraMatrixForUndistortRectify not producing proper results, using a naive approach instead: self.P[:3,:3] = self.intrinsics[:3,:3] self.P[0,0] /= (1. + a) self.P[1,1] /= (1. + a) self.mapx, self.mapy = cv2.fisheye.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, self.P, self.size, cv2.CV_32FC1) - else: - print("Something went wrong when selecting a model") def remap(self, src): @@ -740,8 +735,6 @@ def undistort_points(self, src): return cv2.undistortPoints(src, self.intrinsics, self.distortion, R = self.R, P = self.P) elif self.camera_model == CAMERA_MODEL.FISHEYE: return cv2.fisheye.undistortPoints(src, self.intrinsics, self.distortion, R = self.R, P = self.P) - else: - print("Something went wrong when selecting a model") def as_message(self): """ Return the camera calibration as a CameraInfo message """ @@ -1009,8 +1002,6 @@ def cal_fromcorners(self, good): self.T, # T criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), # 30, 1e-6 flags = flags) - else: - print("Something went wrong when selecting a model") self.set_alpha(0.0) @@ -1062,8 +1053,6 @@ def set_alpha(self, a): self.l.mapx, self.l.mapy) cv2.fisheye.initUndistortRectifyMap(self.r.intrinsics, self.r.distortion, self.r.R, self.r.intrinsics, self.size, cv2.CV_32FC1, self.r.mapx, self.r.mapy) - else: - print("Something went wrong when selecting a model") def as_message(self): """