propose another method for estimating tag position.

[freedomcondor]

Hello,
My name is Weixu Zhu. I'm a PhD student in IRIDIA lab, Université libre de Bruxelles.
When using estimate_tag_pose, I got "Error, more than one new minima found" pretty often. When this happens, I got the pose wrong, shown in attached file

. I had a look at code apriltag_pose.c, this seems to be caused by a first bad orthogonal_iteration estimating, and then fix_pose_ambiguities got confused and got n_minima > 1.
(Also, near line 424 - 437 in apriltag_pose.c, I also observed that (n_minima == 0) happens more often than (n_minima > 1). The code doesn't do anything about it. I'm not sure if it is a problem or not. I guess this is fine, since when (n_minima == 0) happens, orthogonal_iteration seems to produce a good result.)
Anyway, I propose an alternative method, I call it solve_square method. I change apriltag_pose.c a little bit, so that when "Error, more than one new minima found" happens, it switches to solve_square method to get the estimation result. It works pretty fine as I test it, shown in the file

The detail of the solve_square method is here:
solve_square_description.pdf
You can also clone my testbench https://github.com/freedomcondor/tagTestBench.git, which contains a picture I save which causes "Error, more than one new minima found", to see the results yourself.
I would appreciate it if you could evaluate my solution to see if it is a good one.
Thank you very much. Have a nice day.
Weixu Zhu

[mkrogius]

Hi Weixu,

I would like to apologize for not responding sooner to your pull request. I have looked at it a couple times over the years, but each time, other, more pressing work interrupted me before I could even really start evaluating your solve square method. The problem is that I have very little time to devote to AprilTag support, so it mostly gets consumed by fixing day-to-day bugs. The other problem is that I currently do not have a good system to test the pose estimation calculations. I do not think this situation will change in the future, but I will leave this PR open in the hopes that eventually it can be evaluated.

Best,
Max

[jrepp]

I have been able to reproduce the results in the PR using the author's original test bed.

I used a branch of apriltag that has diagnostics tracing code embedded which allowed this capture

I modified the submissions code to start in PAUSE=1 and only process one iteration at a time.

The orthogonal iteration is done first, there are around 44k lines emitted from math for this operation and it results in the output described above. As it is implemented directly the second method 'solve squares' results in only a dozen or so lines, in effect only to build the rotational matrix for the pose output.

Here are my observations:

• For the sample input the solve squared method produces a visually 'true' result.
• April tags repro does not have any valid test code for estimating poses, this is the first simple example I have seen
• Solve squares as a direct implementation (e.g. not implemented through matd_op) appears to be more efficient but needs detailed profiling
• I currently lack additional inputs that would be needed to compare both methods.

[freedomcondor]

I would like to thank both of you very much for looking at this PR.
I hope the problem of "Error, more than one new minima found" can be fixed eventually with or without my proposition.
I'd be very glad to provide any more information if you need.

[jrepp]

Do you have more test inputs and verifiable outputs?
Would you be open to adapting your testbed into a new test harness?

From what I've seen running your test framework I think it's worthwhile to spend additional time upstreaming both the solve squares method (even if it's a run-time or compile-time option) and the test framework.

[s-trinh]

The detail of the solve_square method is here:
solve_square_description.pdf

I think there is also SOLVEPNP_IPPE_SQUARE (from @tobycollins) in OpenCV which could be an alternative or another approach to solve the problem?

https://encov.ip.uca.fr/publications/pubfiles/2014_Collins_etal_IJCV_plane.pdf