## 4 Robot Self-Localization  
The image projection of the simulated lines are matched to the lines in the real world, and applied to derive the robot motion in the virtual environment. It is assumed that the surrounding environment is fixed, and the robot captures the images in a moving trajectory. In this work, we use the perspective-n-point (PnP) algorithm to compute the robot motion. The algorithm estimates the camera position in 3D with respect to a series of 3D known feature such as points and lines. It is efficient to use linear equations to solve the PnP problem, but this has to result in a larger position estimation error [26]. A better solving strategy is to adopt nonlinear iterative equations to achieve high accuracy [27]. Our method is based on the efficient perspective-n-point (EPnP) algorithm proposed by [PERSON] et al. [28] to estimate the virtual camera position during the robot motion.  
In the EPnP algorithm, we first select four non-coplanar points as reference points. Their coordinates in the camera reference system are derived according to the 2D image points and the relationships between 3D points and the reference points. Let \(\mathbf{p}_{i}^{w}\), \(i=1,\cdots,n\) be \(n\) points in the 3D space, and \(\mathbf{c}_{j}^{w}\) and \(\mathbf{c}_{j}^{c}\) for \(j=1,\cdots,4\) be the four reference points in the world and camera coordinate systems, respectively. Then the transformation between \(\mathbf{p}_{i}^{w}\) and \(\mathbf{c}_{j}^{w}\) is given by  
\[\mathbf{p}_{i}^{w}=\sum_{j=1}^{4}\alpha_{ij}c_{j}^{w},i=1,\cdots,n \tag{6}\]  
where \((\alpha_{i1},\alpha_{i2},\alpha_{i3},\alpha_{i4})^{\top}\) are the coordinates of \(\mathbf{p}_{i}^{w}\) taking the reference points as basis. Similarly, we have  
\[\mathbf{p}_{i}^{c}=\sum_{j=1}^{4}\alpha_{ij}c_{j}^{c},i=1,\cdots,n \tag{7}\]  
Thus,  
\[w_{i}\left(\begin{array}{c}u_{i}\\ v_{i}\\ 1\end{array}\right)=\mathbf{a}\mathbf{p}_{i}^{c}=\Lambda\sum_{j=1}^{4}\alpha_{ ij}c_{j}^{c},\quad\forall i \tag{8}\]  
where \(w_{i}\) represents the projection of \(\mathbf{p}_{i}^{2}\) and \(\Lambda\) is the intrinsic camera parameter matrix.  
Let \(\mathbf{x}=(c_{1}^{c},c_{2}^{c},c_{3}^{c},c_{4}^{c})^{\top}\) and for given \(N\) points. The right-hand side of Equation (8) can be written as \(\mathtt{Mx}\) with  
\[\mathtt{M}=(\mathtt{M}_{1}\mathtt{M}_{2}\mathtt{M}_{3}\mathtt{M}_{4}) \tag{9}\]  
Figure 7: The line matching between the virtual room and the real room.  
where  
\[\mathtt{M}_{i}=\left[\begin{array}{ccc}\alpha_{1i}&0&-\alpha_{1i}u_{1}\\ 0&\alpha_{1i}&-\alpha_{1i}v_{1}\\ \vdots&\vdots&\vdots\\ \alpha_{Ni}&0&-\alpha_{Ni}u_{N}\\ 0&\alpha_{Ni}&-\alpha_{Ni}v_{N}\end{array}\right] \tag{10}\]  
Thus, the above equation can be solved by singular value decomposition. Given the world coordinates \(\mathbf{x}_{i}^{w}\) and the camera coordinates \(\mathbf{x}_{i}^{c}\), the relationship between \(\mathbf{p}_{i}^{w}\) and \(\mathbf{p}_{i}^{c}\) is written as  
\[\mathtt{Rp}_{i}^{c}+\mathbf{T}=\mathbf{p}_{i}^{w},\quad i=1,\cdots,n \tag{11}\]  
by Equations (6) and (7). The extrinsic camera parameter matrices R and T can then be derived with the closed-form solution [29].  
To apply the EPnP algorithm to estimate the camera's position in the virtual environment, we define a set of 3D points and the corresponding 2D points detected in the acquired omnidirectional images. The second set of 3D and 2D point correspondences is then obtained in the omnidirectional image captured after the robot's motion. As illustrated in Figure 8, the EPnP algorithm is used to compute the transformation between the two sets of 3D and 2D point correspondence. This provides the rotation matrix and translation vector of the robot movement.