Merge pull request #70 from JuliaRobotics/fix/drawaxis

Affie · web-flow · commit 87ed08678738 · 2021-01-18T12:28:24.000+02:00
fix drawTagAxis! and swop cx,cy to close #66
diff --git a/src/tagdraw.jl b/src/tagdraw.jl
@@ -102,7 +102,7 @@ function drawTagAxes!(image::AbstractArray{<:AbstractRGB,2},
                       K::AbstractArray{<:Real,2} )
     #
     Kp = [K [0;0]; 0.0 0.0 1.0 0.0]
-    pose = AprilTags.homography_to_pose(tag.H, K[1,1], K[2,2], K[1,3], K[2,3])
+    pose = AprilTags.homographytopose(tag.H, Kp[1,1], Kp[2,2], Kp[1,3], Kp[2,3])
 
     # calculate and project
     p0 = Kp*pose[:,4]
diff --git a/test/basictests.jl b/test/basictests.jl
@@ -125,8 +125,8 @@ using Test
         #test drawing functions
         fx = 524.040
         fy = 524.040
-        cy = 319.254
-        cx = 251.227
+        cx = 319.254
+        cy = 251.227
         K = [fx 0  cx;
               0 fy cy]
         imCol = RGB.(image)
@@ -208,14 +208,14 @@ using Test
         detector.quad_decimate = 1.0 #NOTE see line 84
         fx = 524.040
         fy = 524.040
-        cx = 251.227
-        cy = 319.254
+        cx = 319.254
+        cy = 251.227
         taglength = 0.172
         (tags, poses) = detectAndPose(detector, image, fx, fy, cx, cy, taglength)
-        # TODO test here
-        @test all(isapprox.(poses[1], [ 0.657276  -0.43653   0.614354  -0.236778;
-                                        0.180276   0.882573  0.434242   0.268374;
-                                       -0.731771  -0.174663  0.65879    1.65107],
+        # TODO test here, this is just result used as test.
+        @test all(isapprox.(poses[1], [ 0.630532  -0.391638  0.670111  -0.461887;
+                                        0.118082   0.901717  0.415889   0.494238;
+                                       -0.767128  -0.183103  0.614807   1.69053],
                                       atol = 0.01))
         freeDetector!(detector)
 

Original file line number	Diff line number	Diff line change
`@@ -102,7 +102,7 @@ function drawTagAxes!(image::AbstractArray{<:AbstractRGB,2},`
`102`	`102`	`K::AbstractArray{<:Real,2} )`
`103`	`103`	`#`
`104`	`104`	`Kp = [K [0;0]; 0.0 0.0 1.0 0.0]`
`105`		`- pose = AprilTags.homography_to_pose(tag.H, K[1,1], K[2,2], K[1,3], K[2,3])`
	`105`	`+ pose = AprilTags.homographytopose(tag.H, Kp[1,1], Kp[2,2], Kp[1,3], Kp[2,3])`
`106`	`106`
`107`	`107`	`# calculate and project`
`108`	`108`	`p0 = Kp*pose[:,4]`