## 2 Single-Camera Trilateration Algorithm
### Camera Position Estimation  
Assuming that \(N\geq 3\) landmarks are chosen for localizing the camera, we have a system of N independent trilateration equations in \(\mathrm{X_{g}Y_{g}Z_{g}}\) in the form of:  
\[({}^{s}\mathbf{P}_{i}-{}^{s}\mathbf{P}_{0})^{T}({}^{s}\mathbf{P}_{i}-{}^{s} \mathbf{P}_{0})=r_{i}^{2},Vi\in\{1,\ldots,N\}, \tag{5}\]  
which is in fact Equation (1) defined in \(\mathrm{X_{g}Y_{g}Z_{g}}\). In principle, given \({}^{s}\mathbf{p}_{i}\) and \(r_{i}\), one can estimate \({}^{s}\mathbf{p}_{0}\) by solving Equation (5). Representing the standard ranging-based trilateration problem, Equation (5) can be solved using one of the existing closed-form or numerical algorithms in the literature [2; 4; 5; 6; 19; 16]. Here, \({}^{s}\mathbf{p}_{i}\) are known as the pre-mapped landmark positions, but \(r_{i}\) are yet to be determined. In order to determine \(r_{i}\), we can write down a system of totally \(N\) (\(N-1\))/2 equations as:  
\[r_{i}^{2}+r_{j}^{2}-2r_{i}r_{j}\cos\theta_{ij}=r_{ij}^{2},\forall i,j\in\{1, \ldots,N\},i\
eq j, \tag{6}\]  
each of which is defined in a triangle \(\Delta\mathbf{p}_{0}\mathbf{p}_{i}\mathbf{p}_{j}\) in \(\mathrm{X_{g}Y_{g}Z_{g}}\) according to the law of cosine. Here, \(\theta_{ij}\) denotes the vertex angle associated with \(\mathbf{p}_{0}\) in \(\Delta\mathbf{p}_{0}\mathbf{p}_{i}\mathbf{p}_{j}\) (which is also known as the visual angle that \(\mathbf{p}_{i}\mathbf{p}_{j}\) subtends at \(\mathbf{p}_{0}\)), and \(r_{ij}\) the distance between \(\mathbf{p}_{i}\) and \(\mathbf{p}_{j}\). In principle, given \(r_{ij}\) and \(\mathrm{cos}\theta_{ij}\), one can estimate \(\mathrm{r}_{i}\) by solving Equation (6). At least two general schemes can be adopted to obtain \(r_{i}\):  
1. Solving \(N\) equations for \(N\) variables: first, choose a subset of \(N\) independent equations from Equation (6) which contain \(N\) variables \(\{r_{i}|i=1,\ldots,N\}\); next, by combining the \(N\) equations and eliminating \(N-1\) variables \(\{r_{i}|i=2,\ldots,N\}\), obtain a \(2^{N-1}\)-degree polynomial equation of \(r_{1}{}^{2}\), i.e., \[\sum_{k=0}^{2^{N-1}}c_{k}(r_{1}^{2})^{k}=0;\] (7)  
then, solve Equation (7) for \(\mathrm{r}_{1}\), and substitute \(\mathrm{r}_{1}\) into the chosen N equations to solve for \(\{r_{i}|i=2,\ldots,N\}\) iteratively. When \(N=3,\) (7) is a 4-degree polynomial equation which can be solved in the closed form. When \(N>3,\) (7) is a polynomial equation of degree 8 or greater. Although it has been proven (by [PERSON] in 1824) that there cannot be any closed-form formula for such a polynomial equation, a number of numerical algorithms are available, such as [PERSON]'s method [65], the [PERSON] method [66], the [PERSON] method [67], [PERSON] method [68],splitting circle method [69] and the Dandelin-Graffe method [70]. The roots of a polynomial can also be found as the eigenvalues of its companion matrix [71; 72; 73]. 2. Solving an associated optimization problem: A nonlinear least-squares problem can be formulated to estimate the optimal \(r_{i}\) that minimizes the residue of Equation (6), i.e., \[\mathbf{r}=\underset{\mathbf{r}}{\text{argmin}}\quad\sum_{ \begin{array}{c}i, j=1\\ i\
eq j\end{array} }^{N}\quad(r_{i}^{2}+r_{j}^{2}-2r_{i}r_{j}\cos\theta_{ij}-r_{ij}^{ 2})^{2},\] (8) where \(\mathbf{r}\) denotes the vector of \(\{r_{i}|i=1,\,\ldots,N\}\). Search-based optimization algorithms are commonly used to solve this category of problems, including both local optimization algorithms, e.g., the steepest descent method and the Newton-Raphson method (also known as [PERSON]'s method), and global optimization algorithms, e.g., the simulated annealing and the genetic algorithm [74; 75; 76; 77; 78]. To solve Equations (6)-(8), we need to know \(r_{ij}\) and \(\text{cos}\theta_{\bar{y}}\). Here, \(r_{ij}=\sqrt{\left(\
icefrac{{g}}{{g}}-\
icefrac{{g}}{{g}}\right)^{T}\left( \
icefrac{{g}}{{g}}-\
icefrac{{g}}{{g}}\right)}\), but \(\text{cos}\theta_{\bar{y}}\) are yet to be determined.