Experimentation to try to improve the Dementhon planar pose estimation

[s-trinh]

Code for experimentation.

---

Original issue:

• when using Dementhon to compute the planar pose for AprilTag detection, in some configurations it is easy to observe "pose flipping" issue

When adding VVS after each estimated pose inside the Dementhon pipeline, it greatly improves the pose estimation:

• no more "pose flipping" has been observed from the quick tests
• to test, the easiest thing is to add in "tutorial/detection/tag/tutorial-apriltag-detector-live.cpp" a second vpDetectorAprilTag detector2(opt_tag_family); and to duplicate everything to have side-by-side the two displays (e.g. Dementhon vs Lagrange or Dementhon vs Homography VVS, etc.)

For Lagrange:

• "pose flipping" is less frequent compared to Dementhon
• but I have managed to trigger it when aligning the tag frame such as having the tag x-axis pointing downward, the y-axis pointing to the right and to rotate around the x-axis
• and when the tag is small and at the border of the image, e.g. bottom right
• but I think it is more how I grasp the tag with my hand (which impacts the corner locations extraction) rather then a specific configuration compared to the Dementhon issue which is easy to trigger with my tests

---

Currently, distortion is not handled when using the AprilTag pose estimation methods.
But I think the code modification to support it is quite easy, such as in the PR code.

---

visp/modules/detection/src/tag/vpDetectorAprilTag.cpp

Lines 1170 to 1179 in f339e92

	if (fallback_2nd_solution) {
	if (cMo2) {
	// Fallback: set default cMo2 to identity and set error to an invalid value
	cMo2->eye();
	if (projErrors2) {
	*projErrors2 = -1.0;
	}
	vpTRACE("Second solution is only computed for HOMOGRAPHY_ORTHOGONAL_ITERATION");
	}
	}

Currently, the second pose is set to -1 when it is not possible to compute it, but maybe it should be set to INF/HUGE_VAL.
I don't know if it is possible to have a second reprojection error inferior to the first one. But this would avoid issue if the poses are sorted and consistent with the AprilTag library (the second pose cannot always be computed for instance):

visp/3rdparty/apriltag/apriltag_pose.c

Lines 509 to 517 in f339e92

	estimate_pose_for_tag_homography(info, solution1);
	*err1 = orthogonal_iteration(v, p, &solution1->t, &solution1->R, 4, nIters);
	solution2->R = fix_pose_ambiguities(v, p, solution1->t, solution1->R, 4);
	if (solution2->R) {
	solution2->t = matd_create(3, 1);
	*err2 = orthogonal_iteration(v, p, &solution2->t, &solution2->R, 4, nIters);
	} else {
	*err2 = HUGE_VAL;
	}

[codecov]

Codecov Report

❌ Patch coverage is 84.84848% with 5 lines in your changes missing coverage. Please review.
✅ Project coverage is 48.76%. Comparing base (10ba152) to head (bddbb1f).
⚠️ Report is 26 commits behind head on master.

Files with missing lines	Patch %	Lines
modules/detection/src/tag/vpDetectorAprilTag.cpp	66.66%	4 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1940      +/-   ##
==========================================
+ Coverage   48.68%   48.76%   +0.07%     
==========================================
  Files         532      532              
  Lines       69311    69342      +31     
  Branches    32414    32438      +24     
==========================================
+ Hits        33746    33812      +66     
+ Misses      24974    24965       -9     
+ Partials    10591    10565      -26     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.