## 2 Single-Camera Trilateration Algorithm
### Camera Orientation Estimation  
Knowing \(\
icefrac{{g}}{{\mathbf{p}}}_{0}\) and the set of \(\
icefrac{{g}}{{\mathbf{p}}}_{i}\), we obtain from Equation (2):  
\[\
icefrac{{g}}{{\Delta\mathbf{P}}}=\
icefrac{{g}}{{\mathbf{R}_{c}}}^{c} \mathbf{P} \tag{11}\]where \({}^{\delta}\Delta\mathbf{P}\) is a \(3\times\) N matrix with the \(i\)th column as \({}^{\delta}\Delta\mathbf{p}_{i}={}^{\delta}\mathbf{p}_{i}-{}^{\delta}\mathbf{p} _{0}\), and \({}^{c}\mathbf{P}\) is a \(3\times\) N matrix with the \(i\)th column as \({}^{c}\mathbf{p}_{i}\). Here, \({}^{c}\mathbf{p}_{i}\) can be determined by substituting Equation (4) into Equation (3):  
\[{}^{c}\mathbf{p}_{i}={}^{c}z_{i}\Big{[} \begin{array}{cc}\frac{u_{i}-u_{0}}{ f_{u}}&\frac{v_{i}-v_{0}}{f_{v}}&1\end{array} \Big{]}^{T}, \tag{12}\]  
where \({}^{c}z_{i}\) can be calculated as  
\[{}^{c}z_{i}=\frac{fr_{i}}{\rho_{i}}=\frac{r_{i}}{\sqrt{\big{(}\frac{u_{i}-u_{0 }}{f_{u}}\big{)}^{2}+\big{(}\frac{v_{i}-v_{0}}{f_{v}}\big{)}^{2}+1}}. \tag{13}\]  
Given \({}^{\delta}\Delta\mathbf{P}\) and \({}^{c}\mathbf{P}\), \({}^{\delta}\mathbf{R}_{c}\) can in principle be calculated from Equation (11) as  
\[{}^{\delta}\mathbf{R}_{c}={}^{\delta}\Delta\mathbf{P}^{\delta}\Delta\mathbf{P }^{T}({}^{c}\mathbf{P}^{\delta}\mathbf{P}^{T})^{-1}. \tag{14}\]  
However, due to input errors, such as those in reference positioning, camera calibration and image segmentation, the resulting \({}^{\delta}\mathbf{R}_{c}\) may not strictly be a rotation matrix. In particular, the orthonormality of the matrix may not be guaranteed. Thus, instead of using Equation (14), we go through the following process to obtain a valid rotation matrix. We define an optimal approximation of \({}^{\delta}\mathbf{R}_{c}\) as the one that minimizes the residue of Equation (11), i.e.,  
\[{}^{\delta}\mathbf{R}_{c,opt}=\underset{{}^{\delta}\mathbf{R}_{c}}{\text{ argmin}}\sum_{i=1}^{N}\big{(}{}^{\delta}\mathbf{R}_{c}{}^{c}\mathbf{p}_{i}-{}^{ \delta}\Delta\mathbf{p}_{i}\big{)}^{T}({}^{\delta}\mathbf{R}_{c}{}^{c} \mathbf{p}_{i}-{}^{\delta}\Delta\mathbf{p}_{i}\big{)} \tag{15}\] \[=\underset{{}^{\delta}\mathbf{R}_{c}}{\text{argmin}}Tr[({}^{ \delta}\mathbf{R}_{c}{}^{c}\mathbf{P}-{}^{\delta}\Delta\mathbf{P})^{T}({}^{ \delta}\mathbf{R}_{c}{}^{c}\mathbf{P}-{}^{\delta}\Delta\mathbf{P})]\] \[=\underset{{}^{\delta}\mathbf{R}_{c}}{\text{argmin}}Tr({}^{c} \mathbf{P}^{T}\mathbf{P}+{}^{\delta}\Delta\mathbf{P}^{T}{}^{\delta}\Delta \mathbf{P}-2{}^{\delta}\Delta\mathbf{P}^{T}{}^{\delta}\mathbf{R}_{c}{}^{c} \mathbf{P})\] \[=\underset{{}^{\delta}\mathbf{R}_{c}}{\text{argmax}}Tr({}^{ \delta}\Delta\mathbf{P}^{T}{}^{\delta}\mathbf{R}_{c}{}^{c}\mathbf{P})\]  
where \(Tr\) (\(\mathbf{M}\)) denotes the trace of the matrix \(\mathbf{M}\). Since \(Tr\) (\(\mathbf{M}_{1}\mathbf{M}_{2}\)) = \(Tr\) (\(\mathbf{M}_{2}\mathbf{M}_{1}\)) for two matrices \(\mathbf{M}_{1}\) and \(\mathbf{M}_{2}\), we have:  
\[{}^{\delta}\mathbf{R}_{c,opt}=\underset{{}^{\delta}\mathbf{R}_{c}}{\text{ argmax}}Tr({}^{c}\mathbf{P}{}^{\delta}\Delta\mathbf{P}^{T}{}^{\delta}\mathbf{R}_{c}) \tag{16}\] \[=\underset{{}^{\delta}\mathbf{R}_{c}}{\text{argmax}}Tr({}^{c} \mathbf{A}\mathbf{D}{}^{T}{}^{\delta}\mathbf{R}_{c}\mathbf{A})\]  
where \(\mathbf{A}\mathbf{D}\mathbf{B}^{T}\) is the singular value decomposition of the \(3\times 3\) matrix \({}^{c}\mathbf{P}{}^{\delta}\Delta\mathbf{P}^{T}\) in which \(\mathbf{A}\) and \(\mathbf{B}\) are orthogonal matrices and \(\mathbf{D}\) is an diagonal matrix with non-negative real numbers on the diagonal.