Add get_second_solution() to handle tag pose ambiguity

[fspindle]

Pose Ambiguity Handling

Planar markers often have two mathematically plausible pose solutions. This PR exposes the second solution from the orthogonal iteration algorithm.

• Added get_second_solution() to apriltag_pose.h. It allows to extract second pose candidate into a dedicated function. Uses fix_pose_ambiguities() to find the alternative solution and refines it via orthogonal iteration. Returns HUGE_VAL in err2 if no second solution exists.
• This allows developers to use temporal filtering or external constraints to choose the correct orientation when the error difference is small.

[christian-rauch]

@mkrogius Can you have a look at this?

[mkrogius]

We already have estimate_tag_pose_orthogonal_iteration which returns both solutions. Although the name could be clearer, this seems more straightforward to call than get_second_solution since the user does not need to calculate the object and image points

[christian-rauch]

@fspindle Can you reuse estimate_tag_pose_orthogonal_iteration:

apriltag/apriltag_pose.c

Lines 490 to 523 in 3057666

	void estimate_tag_pose_orthogonal_iteration(
	apriltag_detection_info_t* info,
	double* err1,
	apriltag_pose_t* solution1,
	double* err2,
	apriltag_pose_t* solution2,
	int nIters) {
	double scale = info->tagsize/2.0;
	matd_t* p[4] = {
	matd_create_data(3, 1, (double[]) {-scale, scale, 0}),
	matd_create_data(3, 1, (double[]) {scale, scale, 0}),
	matd_create_data(3, 1, (double[]) {scale, -scale, 0}),
	matd_create_data(3, 1, (double[]) {-scale, -scale, 0})};
	matd_t* v[4];
	for (int i = 0; i < 4; i++) {
	v[i] = matd_create_data(3, 1, (double[]) {
	(info->det->p[i][0] - info->cx)/info->fx, (info->det->p[i][1] - info->cy)/info->fy, 1});
	}
	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;
	}
	for (int i = 0; i < 4; i++) {
	matd_destroy(p[i]);
	matd_destroy(v[i]);
	}
	}

instead of the custom functions proposed here?

[fspindle]

@christian-rauch I will check end of the week. Thanks.

[fspindle]

@christian-rauch @mkrogius I agree that the function I propose to introduce is not optimal. This PR can be closed.

I found that the ubuntu package doesn't expose symbols of estimate_pose_for_tag_homography() and estimate_tag_pose_orthogonal_iteration(). Their visibility seems hidden. Could be nice to change that.

[christian-rauch]

As far as I can see, we do not control the visibility of symbols.

Which Ubuntu package are you talking about? Looking at the apriltag package manifest in Debian, they only expose estimate_tag_pose@Base 3.1.7. This is out of our control, and you have to open an issue there to get teh symbols added to the list.

[christian-rauch]

@fspindle I am closing this at it appears that the functionality already exists.