Merge pull request #62 from JuliaRobotics/feat/4Q20/gridcalib

Add April Cal Grid Cailbration

Project.toml

@@ -1,12 +1,13 @@
 name = "AprilTags"
 uuid = "f0fec3d5-a81e-5a6a-8c28-d2b34f3659de"
-keywords = ["AprilTags", "fiducials", "markers"]
+keywords = ["AprilTags", "fiducials", "markers", "camera", "calibration"]
 desc = "Visual fiducial marking system"
-version = "0.7.5"
+version = "0.8.0"
 
 [deps]
 ColorTypes = "3da002f7-5984-5a60-b8a6-cbb66c0b333f"
 Colors = "5ae59095-9a9b-59fe-a467-6f913c188581"
+DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
 FixedPointNumbers = "53c48c17-4a7d-5ca2-90c5-79b7896eea93"
 ImageDraw = "4381153b-2b60-58ae-a1ba-fd683676385f"
 Libdl = "8f399da3-3557-5675-b5ff-fb832c97cbdb"
@@ -17,17 +18,19 @@ Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2"
 [compat]
 ColorTypes = "0.10"
 Colors = "0.8, 0.9, 0.10, 0.11, 0.12"
+DocStringExtensions = "0.8"
 FixedPointNumbers = "0.7, 0.8"
 ImageDraw = "0.1, 0.2"
 Requires = "1.0"
-julia = "0.7, 1"
+julia = "1"
 
 [extras]
 FileIO = "5789e2e9-d7fb-5bc7-8068-2c6fae9b9549"
 FreeTypeAbstraction = "663a7486-cb36-511b-a19d-713bb74d65c9"
 ImageCore = "a09fc81d-aa75-5fe9-8630-4744c3626534"
 ImageMagick = "6218d12a-5da1-5696-b52f-db25d2ecc6d1"
+Optim = "429524aa-4258-5aef-a3af-852621145aeb"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
-test = ["Test", "FileIO", "ImageCore", "ImageMagick"]
+test = ["Test", "FileIO", "ImageCore", "ImageMagick", "Optim"]

docs/src/index.md

@@ -117,6 +117,21 @@ taglength = 0.172
 freeDetector!(detector)
 ```
 
+## Camera Calibration
+
+Using a AprilTag grid, it is possible to take a series of photographs for estimating the camera intrinsic calibration parameters:
+```@raw html
+<p align="center">
+<img src="https://user-images.githubusercontent.com/6412556/101559167-930a5800-398e-11eb-934d-e880c014c873.png" width="600" border="0" />
+</p>
+```
+
+See the [Calibration example](https://github.com/JuliaRobotics/AprilTags.jl/blob/master/examples/AprilTagsGridCalibration.jl) file for more details, as well as function documentation:
+
+```@docs
+calcCalibResidualAprilTags!
+```
+
 ## Manual Outline
 ```@contents
 Pages = [

examples/AprilTagsGridCalibration.jl

@@ -0,0 +1,172 @@
+
+##
+
+using FreeTypeAbstraction
+using AprilTags
+using Images, ImageView
+using FileIO
+using Optim
+
+using ImageDraw, Colors, FixedPointNumbers
+
+
+
+##
+
+calibfiles = [
+  ENV["HOME"]*"/data/cameracalib/CommonData/IMG_2909.jpg";
+  ENV["HOME"]*"/data/cameracalib/CommonData/IMG_2910.jpg";
+  ENV["HOME"]*"/data/cameracalib/CommonData/IMG_2911.jpg";
+  ENV["HOME"]*"/data/cameracalib/CommonData/IMG_2913.jpg";
+  ENV["HOME"]*"/data/cameracalib/CommonData/IMG_2914.jpg";
+  ENV["HOME"]*"/data/cameracalib/CommonData/IMG_2916.jpg";
+  ENV["HOME"]*"/data/cameracalib/CommonData/IMG_2920.jpg";
+]
+
+
+##
+
+
+INITTEST = load(calibfiles[1])
+
+
+f_width  = size(INITTEST,1) + 0.0
+f_height = f_width  + 0.0
+c_width  = size(INITTEST,2) / 2 + 0.0
+c_height = size(INITTEST,1) / 2 + 0.0
+
+s = 0.0
+
+taglength = 0.03
+
+
+##
+
+
+arr = Vector{typeof(INITTEST)}()
+
+for i in 1:length(calibfiles)
+  push!(arr, load(calibfiles[i]))
+end
+
+
+## quick test
+
+
+detector = AprilTagDetector()
+tags = detector.(arr)
+
+
+#
+
+ARR = []
+
+for tag in tags
+  push!(ARR, [(;id=tg.id,p=tg.p, H=tg.H) for tg in tag])
+end
+
+
+##
+
+resid = calcCalibResidualAprilTags!( arr, ARR, f_height=f_height, taglength=taglength )
+
+
+##
+
+
+obj = (f_w, f_h, c_w, c_h) -> calcCalibResidualAprilTags!(arr, ARR,
+                                                          # tagList=40:40;
+                                                          f_width=f_w, 
+                                                          f_height=f_h, 
+                                                          c_width=c_w, 
+                                                          c_height=c_h, 
+                                                          taglength=taglength )
+
+obj_ = (fc_wh) -> obj(fc_wh...)
+
+
+
+
+## current best guess
+
+f_width_  = f_width
+f_height_ = f_height
+c_width_  = c_width
+c_height_ = c_height
+
+
+##
+
+# check that it works
+obj_([f_width_, f_height_, c_width_, c_height_])
+
+
+##
+
+result = optimize(obj_, [f_width_; f_height_; c_width_; c_height_ ], BFGS(), Optim.Options(iterations = 100, show_trace=true, x_tol=1e-8))
+
+##
+
+
+f_width_  = result.minimizer[1]
+f_height_ = result.minimizer[2]
+c_width_  = result.minimizer[3]
+c_height_ = result.minimizer[4]
+
+
+##
+
+# iPhone 8 rear camera (coarse calibration)
+# f_height_ = 3346.1894
+# f_width_ = 3346.1894
+# c_height_ = 2021.11068
+# c_width_ = 1471.0241
+
+# f_height_ = 3371.2553294118493
+# f_width_ = 3353.696574041437
+# c_height_ = 2007.7796750349364
+# c_width_ = 1496.4523912712611
+
+# f_height_ = 3370.4878918701756
+# f_width_  = 3352.8348099534364
+# c_height_ = 2005.641610450976
+# c_width_  = 1494.8282013012076
+
+# f_height_ = 3431.554669353193
+# f_width_  = 3411.8526620805414
+# c_height_ = 2016.7496065830883
+# c_width_  = 1501.0475003688337
+
+
+minim = obj(f_width_, f_height_, c_width_, c_height_)
+
+
+## draw what is going on
+
+SEL = 4
+
+
+cimg_ = deepcopy(arr[SEL])
+calcCornerProjectionsAprilTags!(cimg_, ARR[SEL],
+                                # tagList=18:18,
+                                taglength=0.0315,
+                                f_width=f_width_,
+                                f_height=f_height_,
+                                c_width=c_width_,
+                                c_height=c_height_,
+                                dodraw=true )
+#
+
+
+##
+
+ImageView.imshow(cimg_)
+
+
+##
+
+
+
+freeDetector!(detector)
+
+##