goodrobots / maverick

UAV Autonomous Systems Management
https://goodrobots.github.io/maverick/
MIT License
173 stars 61 forks source link

Work out how to do camera calibration and automate #241

Closed fnoop closed 7 years ago

fnoop commented 7 years ago

http://answers.opencv.org/question/121137/calibration-images/ https://april.eecs.umich.edu/wiki/Camera_suite

fnoop commented 7 years ago

http://docs.opencv.org/3.2.0/d7/d21/tutorial_interactive_calibration.html http://answers.opencv.org/question/121137/calibration-images/?comment=121148#comment-121148

fnoop commented 7 years ago

Aprilcal is legacy unmaintained software by the look of it, has little documentation or instructions. Java based so needs jdk and several other dependencies installed/compiled. apt install openjdk-8-jdk Install lcm from http://lcm-proj.github.io/ Looks like jogl has moved to: http://jogamp.org/jogl/www/ Remove -sse2 build flag from src/common/Makefile, not supported on ARM Reference compiled lcm.jar in build.xml, within build-java javac tag:

      <classpath>
        <pathelement path="/usr/local/share/java/lcm.jar"/>
      </classpath>

Disambiguate Base64 in src/april/config/Config.java:655:

        return april.util.Base64.decode(lines);

Assume that the included Base64 class should be used instead of generic class.

Install libdc1394-22-dev and libusb-dev as dependency for jni, also change java/jni/jcam/Makefile to:

DC1394_INCLUDES = -I/usr/local/include/ -I/usr/lib/jvm/java-8-openjdk-armhf/include/linux/

In jserial and jgl Makefile, change to:

%.o: %.c
    $(CC) $(CCFLAGS) $(JNI_INCLUDES) -I/usr/lib/jvm/java-8-openjdk-armhf/include/linux/ $<

change libusb-1.0/libusb.h reference to usb.h in image_source_pgusb.c, actually remove all usb references from jcam/Makefile and image_source.c. This removes support for pointgrey cameras so better fix should be made at some point.

ant build successful, run like: java -cp april.jar:/usr/local/share/java/lcm.jar -Djava.library.path=/usr/share/java:./jni/jgl:./jni/jcam:./jni/jserial april.camera.calibrator.AprilCal -u "v4l2:///dev/video4"

Run fails due to missing jgl runtime. Install libjogl2-java.

fnoop commented 7 years ago

Opencv >=3.1.0 has a new interactive calibrator. It works in vaguely the same way as aprilcal so if we can get it working we might not need aprilcal. opencv interactive calibrator from calib3d module, but also depends on aruco module from opencv_contrib.

fnoop commented 7 years ago

./create_charuco -w=8 -h=5 --ml=50 --sl=80 -d=14 ~/charuco-pattern.png opencv_interactive-calibration -ci=4 -h=5 -w=8 --sz=65 --pf=defaultConfig.xml --of=cameraParameters.xml -t=charuco opencv_interactive-calibration -ci=0 -h=5 -w=8 --sz=55 --pf=defaultConfig.xml --of=cameraParameters.xml -t=charuco opencv_interactive-calibration -ci=1 -h=5 -w=8 --sz=70 --pf=defaultConfig.xml --of=cameraParameters.xml -t=charuco

http://answers.opencv.org/question/121137/calibration-images/?comment=121148#comment-121148 http://answers.opencv.org/question/121327/interactive-calibration-frame-rejected/

fnoop commented 7 years ago

http://stackoverflow.com/questions/5987285/what-is-an-acceptable-return-value-from-cvcalibratecamera http://stackoverflow.com/questions/11918315/does-a-smaller-reprojection-error-always-means-better-calibration

fnoop commented 7 years ago

Good doc on using calibration matrices to do pose estimation: http://docs.opencv.org/trunk/d5/dae/tutorial_aruco_detection.html

fnoop commented 7 years ago

Ok used aruco_calibration from 'official' aruco software: https://www.uco.es/investiga/grupos/ava/node/26

This produces a yml calibration that aruco_tracker can use. Although so far pose estimation doesn't work, get 0 coords back.

1=(224.602,113.886) (265.759,190.696) (209.869,271.163) (153.3,195.428) Txyz=0 0 0 Rxyz=0 0 0 
1=(224.557,113.652) (265.63,190.89) (209.655,271.1) (153.224,195.417) Txyz=0 0 0 Rxyz=0 0 0 
1=(224.469,114.38) (265.457,191.572) (209.469,272.036) (153.095,196.148) Txyz=0 0 0 Rxyz=0 0 0 
1=(224.134,114.872) (264.952,192.322) (208.929,272.381) (152.587,196.677) Txyz=0 0 0 Rxyz=0 0 0 

Edit: Important to resize camera parameters to size of capture image, therefore also to set capture geometry so the process is deterministic. Now we're getting accurate reprojection and and pose estimation:

1=(256.988,57.8294) (412.854,41.1703) (418.418,196.257) (249.176,207.818) Txyz=0.00487261 -0.146473 0.543734 Rxyz=2.95224 -0.0122558 -0.360109 
1=(257.026,57.7732) (412.914,41.1136) (418.575,196.272) (249.138,207.878) Txyz=0.00491773 -0.146379 0.543308 Rxyz=2.95169 -0.012372 -0.360012 
1=(256.986,57.8762) (412.879,41.1592) (418.457,196.266) (249.172,207.89) Txyz=0.00487857 -0.146402 0.543557 Rxyz=2.95168 -0.0124483 -0.360846 
1=(257.01,57.8329) (412.91,41.1714) (418.584,196.342) (249.173,207.921) Txyz=0.00492784 -0.146345 0.543406 Rxyz=2.95207 -0.0125466 -0.359523 
1=(257.01,57.8298) (412.836,41.1952) (418.578,196.249) (249.149,207.9) Txyz=0.00490928 -0.146399 0.543505 Rxyz=2.95146 -0.0128555 -0.358812 
1=(256.988,57.8374) (412.839,41.1896) (418.472,196.277) (249.173,207.878) Txyz=0.00488265 -0.14643 0.543665 Rxyz=2.9522 -0.0125406 -0.359661 

Distance is in translation vector Z value