diff --git a/src/tagdraw.jl b/src/tagdraw.jl index d33c86b..4ec26fb 100644 --- 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 index e40328f..16ee3f3 100644 --- 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)