Add get_second_solution() to handle tag pose ambiguity

Extract second pose candidate resolution 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.

Author: fspindle

README.md

@@ -193,6 +193,21 @@ Note: The tag size should not be measured from the outside of the tag. The tag s
 ### Coordinate System
 The coordinate system has the origin at the camera center. The z-axis points from the camera center out the camera lens. The x-axis is to the right in the image taken by the camera, and y is down. The tag's coordinate frame is centered at the center of the tag. From the viewer's perspective, the x-axis is to the right, y-axis down, and z-axis is into the tag.
 
+### Handling Pose Ambiguity
+Planar targets often result in two possible pose solutions with similar errors (the "ambiguity" problem). To retrieve the alternative solution, you can use:
+
+```c
+apriltag_pose_t pose1, pose2;
+double err1 = estimate_tag_pose(&info, &pose1);
+double err2;
+int nIters = 50;
+
+// v and p are the object and image points used during the iteration
+get_second_solution(v, p, &pose1, &pose2, nIters, &err2);
+```
+
+This is particularly useful when temporal filtering or additional constraints are used to disambiguate the tag's orientation.
+
 Utility Functions
 =================
 AprilTag 3 now includes helper functions for deep-copying structures, which is essential for multi-threaded applications or when you need to store detections beyond the detector's lifecycle.

apriltag_pose.c

@@ -545,3 +545,15 @@ double estimate_tag_pose(apriltag_detection_info_t* info, apriltag_pose_t* pose)
         return err2;
     }
 }
+
+void get_second_solution(matd_t* v[4], matd_t* p[4], apriltag_pose_t* solution1, apriltag_pose_t* solution2, int nIters, double* err2)
+{
+    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;
+    }
+}

apriltag_pose.h

@@ -74,6 +74,40 @@ void estimate_tag_pose_orthogonal_iteration(
  */
 double estimate_tag_pose(apriltag_detection_info_t* info, apriltag_pose_t* pose);
 
+/**
+ * Computes the second pose solution from a known first solution.
+ *
+ * Pose estimation from a planar target (such as an AprilTag) suffers from an
+ * inherent ambiguity: two geometrically distinct poses can produce the same
+ * image projection. This function attempts to recover this second candidate
+ * solution using fix_pose_ambiguities(), then refines it via orthogonal iteration.
+ *
+ * If no second solution exists (unambiguous pose), solution2->R is left as NULL
+ * and err2 is set to HUGE_VAL.
+ *
+ * @param[in]  v         Array of 4 normalized image points (3x1 column vectors),
+ *                       expressed in normalized camera coordinates:
+ *                       [(u - cx)/fx, (v - cy)/fy, 1]'.
+ * @param[in]  p         Array of 4 object points in the tag frame (3x1 column vectors).
+ * @param[in]  solution1 First pose solution (R and t already estimated and refined).
+ *                       Used as a starting point to search for the second solution.
+ * @param[out] solution2 Second candidate pose solution. If a second solution is found,
+ *                       solution2->R and solution2->t are allocated by this function
+ *                       and must be freed by the caller via matd_destroy().
+ *                       Otherwise, solution2->R is NULL and solution2->t is undefined.
+ * @param[in]  nIters    Number of iterations for refinement via orthogonal_iteration().
+ * @param[out] err2      Object-space error associated with solution2 after refinement.
+ *                       Set to HUGE_VAL if no second solution was found.
+ *
+ * @note The caller is responsible for freeing solution2->R and solution2->t
+ *       (only if solution2->R != NULL) via matd_destroy().
+ *
+ * @see fix_pose_ambiguities()
+ * @see orthogonal_iteration()
+ * @see estimate_tag_pose_orthogonal_iteration()
+ */
+void get_second_solution(matd_t* v[4], matd_t* p[4], apriltag_pose_t* solution1, apriltag_pose_t* solution2, int nIters, double* err2);
+
 #ifdef __cplusplus
 }
 #endif