From ae48bf34937818068db574c2a2868f444590df4a Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Wed, 29 Aug 2018 12:50:32 +0200 Subject: [PATCH] update homography (#29) * added new homographytopose It uses a camera looking in the positive z direction with x to the right and y down * added taglengths param --- docs/src/func_ref.md | 1 + examples/detect_in_image.jl | 2 +- src/AprilTags.jl | 1 + src/helpers.jl | 85 ++++++++++++++++++++++++++-- test/homography.jl | 110 ++++++++++++++++++++++++++++++++++++ test/runtests.jl | 2 + 6 files changed, 194 insertions(+), 7 deletions(-) create mode 100644 test/homography.jl diff --git a/docs/src/func_ref.md b/docs/src/func_ref.md index 654d934..5679af0 100644 --- a/docs/src/func_ref.md +++ b/docs/src/func_ref.md @@ -11,6 +11,7 @@ Depth = 3 AprilTags.AprilTagDetector AprilTags.freeDetector! homography_to_pose +homographytopose drawTagBox! drawTagAxes! getAprilTagImage diff --git a/examples/detect_in_image.jl b/examples/detect_in_image.jl index 901f973..8207683 100644 --- a/examples/detect_in_image.jl +++ b/examples/detect_in_image.jl @@ -22,7 +22,7 @@ fx = 524.040 fy = 524.040 cy = 319.254 cx = 251.227 -K = [fx 0 cx; +K = [-fx 0 cx; 0 fy cy] detector = nothing diff --git a/src/AprilTags.jl b/src/AprilTags.jl index 587538b..16d6bd6 100644 --- a/src/AprilTags.jl +++ b/src/AprilTags.jl @@ -17,6 +17,7 @@ AprilTagDetector, freeDetector!, getTagDetections, homography_to_pose, +homographytopose, threadcalldetect, getAprilTagImage, diff --git a/src/helpers.jl b/src/helpers.jl index 8a519f0..fd09604 100644 --- a/src/helpers.jl +++ b/src/helpers.jl @@ -391,11 +391,15 @@ end """ - homography_to_pose(H, fx, fy, cx, cy) + homography_to_pose(H, fx, fy, cx, cy, [taglength = 2.0]) Given a 3x3 homography matrix and the camera model (focal length and centre), compute the pose of the tag. -The focal lengths should be given in pixels +The focal lengths should be given in pixels. +The returned units are those of the tag size, +therefore the translational components should be scaled with the tag size. +Note: the tag coordinates are from (-1,-1) to (1,1), i.e. the tag size has lenght of 2 units. +Optionally, the tag length (in metre) can be passed to return a scaled value. """ -function homography_to_pose(H::Matrix{Float64}, fx::Float64, fy::Float64, cx::Float64, cy::Float64)::Matrix{Float64} +function homography_to_pose(H::Matrix{Float64}, fx::Float64, fy::Float64, cx::Float64, cy::Float64; taglength::Float64 = 2.0)::Matrix{Float64} # Note that every variable that we compute is proportional to the scale factor of H. R31 = H[3, 1] R32 = H[3, 2] @@ -447,8 +451,77 @@ function homography_to_pose(H::Matrix{Float64}, fx::Float64, fy::Float64, cx::Fl R = U * V' end - return [R [TX; - TY; - TZ]; + return [R [TX*taglength/2.0; + TY*taglength/2.0; + TZ*taglength/2.0]; + [0 0 0 1.0]] +end + + +""" + homographytopose(H, fx, fy, cx, cy, [taglength = 2.0]) +Given a 3x3 homography matrix and the camera model (focal length and centre), compute the pose of the tag. +The focal lengths should be given in pixels. +The returned units are those of the tag size, +therefore the translational components should be scaled with the tag size. +Note: the tag coordinates are from (-1,-1) to (1,1), i.e. the tag size has lenght of 2 units. +Optionally, the tag length (in metre) can be passed to return a scaled value. +The camara coordinate system: camera looking in positive Z axis with x to the right and y down. +""" +function homographytopose(H::Matrix{Float64}, fx::Float64, fy::Float64, cx::Float64, cy::Float64; taglength::Float64 = 2.0)::Matrix{Float64} + # Note that every variable that we compute is proportional to the scale factor of H. + R31 = H[3, 1] + R32 = H[3, 2] + TZ = H[3, 3] + R11 = (H[1, 1] - cx*R31) / fx + R12 = (H[1, 2] - cx*R32) / fx + TX = (H[1, 3] - cx*TZ) / fx + R21 = (H[2, 1] - cy*R31) / fy + R22 = (H[2, 2] - cy*R32) / fy + TY = (H[2, 3] - cy*TZ) / fy + + # compute the scale by requiring that the rotation columns are unit length + # (Use geometric average of the two length vectors we have) + length1 = sqrt(R11*R11 + R21*R21 + R31*R31) + length2 = sqrt(R12*R12 + R22*R22 + R32*R32) + s = 1.0 / sqrt(length1 * length2) + + # get sign of S by requiring the tag to be in front the camera + # we assume camera looks in the +Z direction. + if (TZ < 0) + s *= -1.0 + end + + R31 *= s + R32 *= s + TZ *= s + R11 *= s + R12 *= s + TX *= s + R21 *= s + R22 *= s + TY *= s + + # now recover [R13 R23 R3] by noting that it is the cross product of the other two columns. + R13 = R21*R32 - R31*R22 + R23 = R31*R12 - R11*R32 + R33 = R11*R22 - R21*R12 + + # Improve rotation matrix by applying polar decomposition. + if (true) + # do polar decomposition. This makes the rotation matrix + # "proper", but probably increases the reprojection error. An + # iterative alignment step would be superior. + R = [R11 R12 R13; + R21 R22 R23; + R31 R32 R33] + + U, S, V = svd(R) + R = U * V' + end + + return [R [TX*taglength/2.0; + TY*taglength/2.0; + TZ*taglength/2.0]; [0 0 0 1.0]] end diff --git a/test/homography.jl b/test/homography.jl new file mode 100644 index 0000000..4ed75ff --- /dev/null +++ b/test/homography.jl @@ -0,0 +1,110 @@ +struct PixPos + pix::UInt8 + pos::NamedTuple{(:x, :y, :z),Tuple{Float64, Float64, Float64}} +end +PixPos(pix::UInt8, pos::Vector{Float64}) = PixPos(pix, (x=pos[1], y=pos[2], z=pos[3])) +PixPos(pix::UInt8, pos::Array{Float64,1}) = PixPos(pix, (x=pos[1], y=pos[2], z=pos[3])) + +@testset "Homography to Pose" begin + + fx = 1000. + fy = 1000. + cx = 320. + cy = 240. + + K = [fx 0 cx; + 0 fy cy] + + C = [K; 0 0 1] + + detector = AprilTagDetector() + + #_____________________________________________________________________ + + + #test offset only + test_cTw = [[1.0 0.0 0.0 -100.0; + 0.0 1.0 0.0 -100.0; + 0.0 0.0 1.0 1500.0; + 0.0 0.0 0.0 1.0]] + + #test offset only + #FIXME bigger offsets like this one fails test + #= + push!(test_cTw, [1.0 0.0 0.0 320.0; + 0.0 1.0 0.0 -200.0; + 0.0 0.0 1.0 1500.0; + 0.0 0.0 0.0 1.0]) + =# + + #test horizontal angle + push!(test_cTw, [ 0.707107 0.0 0.707107 0.0; + 0.0 1.0 0.0 0.0; + -0.707107 0.0 0.707107 1500.0; + 0.0 0.0 0.0 1.0]) + + #test offset and horizontal angle + push!(test_cTw, [ 0.707107 0.0 0.707107 100.0; + 0.0 1.0 0.0 100.0; + -0.707107 0.0 0.707107 1000.0; + 0.0 0.0 0.0 1.0]) + + #test vertical angle + push!(test_cTw, [ 1.0 0.0 0.0 0.0; + 0.0 0.707107 0.707107 0.0; + 0.0 -0.707107 0.707107 1500.0; + 0.0 0.0 0.0 1.0]) + + #test 2 angles + push!(test_cTw, [0.707107 0.0 -0.707107 0.0; + 0.5 0.707107 0.5 -60.6602; + 0.5 -0.707107 0.5 2060.66; + 0.0 0.0 0.0 1.0]) + + for cTw = test_cTw + back = ones(UInt8, 301,301)*0x40 + back[11:290,11:290] = ones(UInt8, 280,280)*0x80 + + #tag 0 draw in camera frame + #_____________________________________________________________________ + tag0 = kron(reinterpret(UInt8,getAprilTagImage(0)), ones(UInt8, 20,20)) + back[51:250,51:250] = tag0' + + x = collect(-150.:150) + y = collect(-150.:150) + z = 0. + + tag0cloud = Array{PixPos,2}(undef, 301,301) + for i = 1:301, j = 1:301 + tag0cloud[i,j] = PixPos(back[i,j],[x[i]; y[j]; z]) + end + + #Projection + #_____________________________________________________________________ + + projImg = zeros(Float32, 480,640); + cnt = zeros(Int, 480,640); + + for i = 1:301, j = 1:301 + tc = tag0cloud[i,j] + pos = C*cTw[1:3,1:4]*[tc.pos.x; tc.pos.y; tc.pos.z; 1] + pos /= pos[3] + u = round(Int,pos[2]) + v = round(Int,pos[1]) + if 1 < u < 480 && 1 < v < 640 + cnt[u,v] += 1f0 + n = cnt[u,v] + projImg[u,v] = projImg[u,v]* (n-1)/n + tc.pix/255f0/n + end + end + + tags = detector(Gray{N0f8}.(projImg)) + + pose = homographytopose(tags[1].H, fx, fy, cx, cy, taglength = 160.) + display(pose) + + @test pose[1:3,1:3] ≈ cTw[1:3,1:3] atol = 0.05 + @test pose[1:3,4] ≈ cTw[1:3,4] atol = 10. + end + +end diff --git a/test/runtests.jl b/test/runtests.jl index c101424..f4736cb 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -195,6 +195,8 @@ using Test end + include("homography.jl") + @testset "Errors" begin #testing freed detectors errors detector = AprilTagDetector()