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

@@ -11,6 +11,7 @@ Depth = 3
 AprilTags.AprilTagDetector
 AprilTags.freeDetector!
 homography_to_pose
+homographytopose
 drawTagBox!
 drawTagAxes!
 getAprilTagImage

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

src/AprilTags.jl

@@ -17,6 +17,7 @@ AprilTagDetector,
 freeDetector!,
 getTagDetections,
 homography_to_pose,
+homographytopose,
 threadcalldetect,
 getAprilTagImage,
 

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

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

test/runtests.jl

@@ -195,6 +195,8 @@ using Test
 
     end
 
+    include("homography.jl")
+
     @testset "Errors" begin
         #testing freed detectors errors
         detector = AprilTagDetector()