# Multi-Tag Fusion Localization Method Based on Geometric Constraints
## 2 Visual Tag Process
### Coordinate Transformation  
To ascertain the global coordinates of a target within a scene, it is imperative to perform transformations across multiple coordinate systems. This process engages several defined coordinate systems: the world coordinate system (W), tag coordinate system (T), camera coordinate system (C), and the self or robot coordinate system (M), all adhering to the right-hand rule for orientation. As illustrated in Figure 3, the X-axis is represented in red, the Y-axis in green, and the Z-axis in blue.  
Firstly, the relative pose between the tag and the camera, denoted as \(P_{\text{C}}\), is transformed into the relative pose between the tag and the target, denoted as \(P_{\text{M}}\):  
\[P_{\text{M}}=R_{\text{C}}^{\text{M}}P_{\text{C}}+T_{\text{C}}^{\text{M}} \tag{1}\]  
Figure 3: Diagram of the coordinate system.  
Figure 2: AprilTag3 detection process.  
In the equation, \(R_{\text{C}}^{\text{M}}\) represents the rotation matrix from the self-coordinate system to the camera coordinate system, and \(T_{\text{C}}^{\text{M}}\) is the installation position of the camera. Subsequently, the coordinates of the target P in the tag coordinate system are obtained as \(P_{\text{T}}\):  
\[P_{\text{T}}=R_{\text{C}}^{\text{T}}P_{\text{M}}+T_{\text{C}}^{\text{T}} \tag{2}\]  
\(R_{\text{C}}^{\text{T}}\) is the rotation matrix between the tag coordinate system and the camera coordinate system. During the initial deployment of the tags, each tag is assigned a unique identifier, and the world coordinates of the center of the tag are represented by the matrix \(T(id)\):  
\[T(id)=[x,y,z]^{\text{T}} \tag{3}\]  
Finally, the coordinates of the target P in the world coordinate system are calculated as \(P_{\text{W}}\):  
\[P_{\text{W}}=R_{\text{T}}^{\text{W}}P_{\text{T}}+T(\text{id}) \tag{4}\]  
In this formula, \(R_{\text{T}}^{\text{W}}\) is the rotation matrix from the tag coordinate system to the world coordinate system. The aforementioned rotation matrices \(R_{\text{C}}^{\text{M}}\), \(R_{\text{C}}^{\text{T}}\), \(R_{\text{T}}^{\text{W}}\) are derived from the Equation (5):  
\[R=R_{x}R_{y}R_{z} \tag{5}\]  
\(R_{x}\), \(R_{y}\), and \(R_{z}\), are the rotation matrices calculated from the corresponding angles, \(\phi\), \(\theta\) and \(\psi\) representing the rotation around the X-axis, Y-axis, and Z-axis, respectively.  
\[R_{x}=\begin{bmatrix}1&0&0\\ 0&\cos\phi&\sin\phi\\ 0&-\sin\phi&\cos\phi\end{bmatrix} \tag{6}\]  
\[R_{y}=\begin{bmatrix}\cos\theta&0&-\sin\theta\\ 0&1&0\\ \sin\theta&0&\cos\theta\end{bmatrix} \tag{7}\]  
\[R_{z}=\begin{bmatrix}\cos\psi&\sin\psi&0\\ -\sin\psi&\cos\psi&0\\ 0&0&1\end{bmatrix} \tag{8}\]  
An image containing N tags, once processed through tag detection, yields N distinct relative poses. After undergoing coordinate transformation, these relative poses result in N global poses of the target in the world coordinate system at the same moment. These global poses are then utilized as observation values for subsequent fusion steps. The observation values include the two-dimensional coordinates in the world coordinate system and the orientation angle.