## Appendix B
The equations are summarized in the below:  
\[\begin{array}{c}p_{x}=d_{x}-L_{t}a_{x}\\ p_{y}=d_{y}-L_{t}a_{y}\\ p_{z}=d_{z}-L_{t}a_{z}\\ \\ q_{1}=arctan(\frac{p_{y}}{p_{x}})\\ \\ q_{2}=\frac{p^{i}}{2}-\arccos(\frac{{L_{2}}^{2}+{L_{3}}^{2}+{L_{4}}^{2}-\left( \sqrt{{p_{x}}^{2}+{p_{y}}^{2}}-{a_{1}}\right)^{2}+\left({p_{z}}-{L_{1}}\right)^ {2}-{L_{3}}^{2}-{L_{4}}^{2}}{2{L_{2}}\sqrt{{L_{3}}^{2}+{L_{4}}^{2}}})})\\ \qquad\qquad\qquad\qquad\qquad-arctan(\frac{{p_{z}}-{L_{1}}}{\sqrt{{p_{x}}^{2}+ {p_{y}}^{2}-{a_{1}}}})\\ \\ q_{3}=\pi-\arccos(\frac{{L_{2}}^{2}+{L_{3}}^{2}+{L_{4}}^{2}-\left(\sqrt{{p_{x} }^{2}+{p_{y}}^{2}}-{a_{1}}\right)^{2}-\left({p_{z}}-{L_{1}}\right)^{2}}{2{L_{2} }\sqrt{{L_{3}}^{2}+{L_{4}}^{2}}})-\arctan(\frac{{L_{4}}}{{L_{3}}})\\ \\ q_{5}=\arccos({c_{1}}{c_{23}}{a_{x}}+{s_{1}}{c_{23}}{a_{y}}-{s_{23}}{a_{z}}) \\ \\ q_{4}=\arctan(\frac{{s_{1}}{a_{x}}-{c_{1}}{a_{y}}}{{c_{1}}{s_{23}}{a_{x}}+{s _{1}}{s_{23}}{a_{y}}+{c  
Using Equations (9) and (10), the relationships between 3D coordinates and image coordinates in a rectified stereo setup are:  
\[Z^{C}\cdot u_{l}=F\cdot\left(X^{C}-b/2\right)\] (A16)  
\[Z^{C}\cdot u_{r}=F\cdot\left(X^{C}+b/2\right)\] (A17)  
\[Z^{C}\cdot v=F\cdot Y^{C}\] (A18)  
Solving Equations (A16)-(A18) yields the inverse stereo mapping:  
\[X^{C}=\frac{b(u_{r}+u_{l})}{2(u_{r}-u_{l})}\] (A19)  
\[Y^{C}=\frac{b\cdot v}{(u_{r}-u_{l})}\] (A20)  
\[Z^{C}=\frac{F\cdot b}{u_{r}-u_{l}}\] (A21)  
Equations (A19)-(A21) implement the inverse mapping \(M^{-1}\) in Equation (A15), allowing us to convert each marker's observed image coordinates into 3D positions in the camera frame: \(P_{R_{1}}^{C}\), \(P_{R_{2}}^{C}\) and \(P_{R_{3}}^{C}\). We now define a marker coordinate frame, illustrated in Figure A3. The origin is placed at the center of one of the markers. The Z axis is defined to point perpendicular to the marker plane, facing toward the camera. The X axis points along the vector from the origin to another marker's center, and the Y axis is determined using the right-hand rule to complete the orthonormal frame. In this frame, the 3D positions of the marker centers are: \(P_{R_{1}}^{M}\), \(P_{R_{2}}^{M}\) and \(P_{R_{3}}^{M}\). Let \(P_{R}^{M}=\left[P_{R_{1}}^{M},P_{R_{2}}^{M},P_{R_{3}}^{M}\right]\), and \(P_{R}^{C}=\left[P_{R_{1}}^{C},P_{R_{2}}^{C},P_{R_{3}}^{C}\right]\). The transformation \(T_{M}^{C}\in SE(3)\) maps marker coordinates to camera coordinates:  
\[P_{R}^{C}=R\cdot P_{R}^{M}+t\] (A22)  
\[T_{M}^{C}= \begin{bmatrix}R&t\\ 0&1\end{bmatrix} \] (A23)  
Here, \(R\in\ \mathbb{R}^{3\times 3}\) is the rotation matrix and \(t\in\ \mathbb{R}^{3}\) is a translation vector. We begin by computing the centroids of the point sets:  
\[\overline{P_{R}^{M}}=\frac{1}{3}\sum_{i=1}^{3}P_{R_{i}}^{M}\] (A24)  
Figure A3: Marker’s Coordinates frame. \[\overline{P_{R}^{C}}=\frac{1}{3}\sum_{i=1}^{3}P_{R_{i}}^{C} \tag{111}\]  
Then, we center the coordinates:  
\[P_{R_{i}}^{M^{\prime}}=P_{R_{i}}^{M}-\overline{P_{R}^{M}},\qquad i=1,2,3 \tag{112}\]  
\[P_{R_{i}}^{C^{\prime}}=P_{R_{i}}^{C}-\overline{P_{R}^{C}},\qquad i=1,2,3 \tag{113}\]  
Compute the cross-covariance matrix:  
\[H\in\ \mathbb{R}^{3\times 3}=\sum_{i=1}^{3}P_{R_{i}}^{M^{\prime}}\cdot\left(P_{R _{i}}^{C^{\prime}}\right)^{T} \tag{114}\]  
Apply Singular Value Decomposition (SVD):  
\[H=U\Sigma V \tag{115}\]  
Here \(U\in\mathbb{R}^{3\times 3}\) is the left orthogonal matrix, \(\Sigma\ \epsilon\ \mathbb{R}^{3\times 3}\) is the diagonal matrix with singular values, and \(V\in\mathbb{R}^{3\times 3}\) is the right orthogonal matrix. The optimal rotation is:  
\[R=VUl^{T} \tag{116}\]  
And from Equation (111), the optimal translation is:  
\[t=\overline{P_{R}^{C}}-R\cdot\overline{P_{R}^{M}} \tag{117}\]  
Finally, the full transformation from marker to camera frame is:  
\[T_{M}^{C}= \begin{bmatrix}VUl^{T}&\overline{P_{R}^{C}}-VUl^{T}\cdot\overline{ P_{R}^{M}}\\ 0&1\end{bmatrix}  \tag{118}\]