## 2 Single-Camera Trilateration Algorithm  
### Notation and Assumptions  
The proposed single-camera trilateration algorithm is derived on the basis of the classical pinhole camera model (Figure 1). Here, \(X_{\mathrm{g}}Y_{\mathrm{g}}Z_{\mathrm{g}}\) denotes the global frame of reference attached to the environment in which the camera is moving, and a vector defined in \(X_{\mathrm{g}}Y_{\mathrm{g}}Z_{\mathrm{g}}\) is labeled with a superscript \"\(g\)\", e.g., \({}^{\mathrm{s}}\mathbf{v}\); \(X_{\mathrm{c}}Y_{\mathrm{c}}Z_{\mathrm{c}}\) denotes the camera frame with the origin at the optical center \(\mathbf{p}_{0}\) and \(Z_{\mathrm{c}}\) axis pointing along the optical axis from \(\mathbf{p}_{0}\) towards the image center \(\mathbf{q}_{0}\), and a vector defined in \(X_{\mathrm{c}}Y_{\mathrm{c}}Z_{\mathrm{c}}\) is labeled with a superscript \"\(c\)\", e.g., \({}^{\mathrm{c}}\mathbf{v}\); UV denotes the image frame with the origin at the upper left corner of the image, and a vector defined in UV is labeled with a superscript \"m\", e.g., \({}^{m}\mathbf{v}\); UVW is the corresponding 3D extension of UV. The vectors in \(X_{\mathrm{g}}Y_{\mathrm{g}}Z_{\mathrm{g}}\), \(X_{\mathrm{c}}Y_{\mathrm{c}}Z_{\mathrm{c}}\) and UVW are 3-dimensional, while those in UV are 2-dimensional. A position defined in \(X_{\mathrm{g}}Y_{\mathrm{g}}Z_{\mathrm{g}}\) or \(X_{\mathrm{c}}Y_{\mathrm{c}}Z_{\mathrm{c}}\) is in the unit of millimeters, while that defined in UV or UVW is in the unit of pixels. In particular, \(\mathbf{p}_{0}\) denotes the optical center of the camera, and its positions defined in \(X_{\mathrm{g}}Y_{\mathrm{g}}Z_{\mathrm{g}}\) and \(X_{\mathrm{c}}Y_{\mathrm{c}}Z_{\mathrm{c}}\) are denoted by \({}^{\mathrm{g}}\mathbf{p}_{0}\) and \({}^{\mathrm{c}}\mathbf{p}_{0}\), respectively. \(\mathbf{q}_{0}\) denotes the image center (also known as the principal point)--The intersection between the optical axis and image plane (which are perpendicular to each other), and its position defined in UV is denoted by \({}^{m}\mathbf{q}_{0}\). The distance between \(\mathbf{p}_{0}\) and \(\mathbf{q}_{0}\) is known as the focal length f. Assuming that there are N landmarks captured by the camera (where N\({}^{\prime}\geq\) 3 for 3D trilateration according to the principle of trilateration), we denote the reference point of the \(i\)th landmark as \(\mathbf{p}_{i}\), where \(i\)\(\in\)\(\{1,2,\ldots,N\}\), and its positions defined in \(X_{\mathrm{g}}Y_{\mathrm{g}}Z_{\mathrm{g}}\) and \(X_{\mathrm{c}}Y_{\mathrm{c}}Z_{\mathrm{c}}\) are denoted by \({}^{\mathrm{g}}\mathbf{p}_{i}\) and \({}^{\mathrm{c}}\mathbf{p}_{i}\), respectively. Moreover, \(\mathbf{q}_{i}\) denotes the projection of \(\mathbf{p}_{i}\) on the image plane, and its positions defined in \(X_{\mathrm{c}}Y_{\mathrm{c}}Z_{\mathrm{c}}\) and UV are denoted by \({}^{\mathrm{c}}\mathbf{q}_{i}\) and \({}^{m}\mathbf{q}_{i}\), respectively. As a necessary input to the proposed single-camera trilateration algorithm, the set of intrinsic parameters of the involved camera can be retrieved from an off-line camera calibration process, a classic topic which has been addressed extensively in computer vision literature such as [59; 60; 61]. A well-adopted camera calibration toolbox can also be found at [62]. Using the calibration toolbox, we can determine the set of intrinsic camera parameters, including the focal length \({}^{m}\mathbf{f}=[t_{u}f_{v}]^{T}\), image center \({}^{m}\mathbf{q}_{0}=[t_{0},v_{0}]^{T}\), and distortion coefficients, as well as their estimation uncertainty. In particular, \(f_{u}=fD_{u}s_{u},f_{v}=fD_{v}\), where f is the physical focal length (mm), \(D_{u}\) the pixel size in the U direction (pixel/mm), \(D_{v}\) the pixel size in the V direction, and \(s_{u}\) the scale factor. It is known that f, \(D_{u}\), \(D_{v}\) and \(s_{u}\) are in general impossible to calibrate separately [60; 61; 62]. Therefore, instead of using f, \(D_{u}\), \(D_{v}\) and \(s_{u}\) separately, we use \(f_{u}\) and \(f_{v}\) in our algorithm.