## 3D Target Tracking
### Spatial-Positioning Method Based on Geometric Constraints  
For a point \(P\) in three-dimensional space, it is expressed by \(P_{w}=\left[\mathrm{X}_{w},\mathrm{Y}_{w},\mathrm{Z}_{w}\right]^{T}\) in the world coordinate system \(W\). For the pixel coordinate system \(UV\), it is expressed by \(P_{w}=\left[\,u_{x}\,\,v_{y}\,\,1\,\right]^{T}\). For the camera coordinate system \(C\), it is expressed by \(P_{c}=\left[\,\mathrm{X}_{c}\,\,\mathrm{Y}_{c}\,\,\mathrm{Z}_{c}\,\right]^{T}\). The input of the depth-estimation module is a typical pinhole projection model and the pixel coordinates of the extracted feature points, and the output is a depth-estimation equation set with the depth information of the feature points as unknowns. In this study, the targets were abstracted as geometric images such as parallelograms or circles. As shown in Figure 8, the coordinates of the target in the world coordinate system were defined as \(Pi=\left[\mathrm{X}_{wi},\,\mathrm{Y}_{wi},\,\,Z_{wi}\right]^{T}\), \(i=1,2,3,4\ldots\) The coordinates in the monocular camera coordinate system were defined as \(P_{w}i=\left[\,\,p_{wxi}\,\,p_{wyi}\,\,p_{wzi}\right]^{T}\), \(i=1,2,3,4\ldots\) The coordinates of the target in the two-dimensional image coordinate system were defined as \(P_{w}i=\left[u_{i}\,v_{i}\,\,1\,\right]^{T}\), \(i=1,2,3,4\ldots\) The normalized projection points of the normalized plane \(Z=1\) were defined as \(P_{c}i=\left[\,\,p_{cxi}\,\,1\,\right]^{T}\), \(i=1,2,3,4\ldots\) The relationship between the image coordinate system and the camera coordinate system can be obtained with the internal reference model of the pinhole camera:  
\[\left[ \begin{array}{c}u\\ v\\ 1\end{array} \right]=CM\times\left[ \begin{array}{c}x_{c}\\ y_{c}\\ 1\end{array} \right]=\left[ \begin{array}{ccc}f_{x}&0&c_{x}\\ 0&f_{x}&c_{y}\\ 0&0&1\end{array} \right]\times\left[ \begin{array}{c}x_{c}\\ y_{c}\\ 1\end{array} \right], \tag{5}\]  
where \(CM\) is the internal parameter matrix. The camera can identify target feature points and obtain a set of 2D-image point positions through real-time image processing. We can obtain the coordinates of the projection with distortion \(P_{c}i=\left[\,\,p_{cxi}\,\,p_{cyi}\,\,1\,\right]^{T}\) in case of \(Z=1\) plane by using the internal reference l relationship of the pinhole projection mode. \[P_{c}i=CM^{-1}\times P_{w}i, \tag{6}\]  
\[P_{w}i=\left[ \begin{array}{c}u_{xi}\\ v_{yi}\\ 1\end{array} \right]=CM\times\left[\begin{array}{c}p_{wxi}&\,\,p_{yxi}\\ \,\,p_{wzi}\,\,1\,\right]^{T}, \tag{7}\]  
\[P_{c}i=\left[ \begin{array}{c}p_{cxi}\\ p_{cyi}\\ p_{czi}\end{array} \right]=\left[\begin{array}{c}p_{wxi}&\,\,p_{wyi}\\ \,\,p_{wzi}\,\,1\,\right]^{T}, \tag{8}\]  
\[ \begin{cases}p_{wxi}=p_{cxi}\times p_{wzi}\\ p_{wyi}=p_{cyi}\times p_{wzi}\end{cases} , \tag{9}\]  
Figure 8: Diagram of 3D to 2D projection. Since \(P_{c}i\) was obtained from (8), we obtained the mathematical constraint relationship of \(p_{wxi},p_{wyi},\ p_{wzi}\) in the feature point of \(P_{w}i=[\ p_{wxi}\ p_{wyi}\ p_{wzi}]^{T}\):  
The solution \(p_{wzi}\) was to solve the scale \(\lambda\) uncertainty problem in monocular-vision positioning. So far, through Formula (9), we can express \(P_{w}i\) as a three-dimensional vector with only one unknown quantity, \(p_{wzi}\), that is, the depth value mentioned in Sections 3 and 4\(D_{0}\):  
\[P_{w}i=\left[ \begin{array}{c}p_{cxi}\times p_{wzi}\\ p_{cyi}\times p_{wzi}\\ p_{wzi}\end{array} \right], \tag{10}\]  
If the target is abstracted as a parallelogram, then a reliable method to construct the constraint equations is presented. As seen in Figure 9, four vertices \(P_{wzi},\ i=1,2,3,4\) are given for the parallelogram feature, and six distance constraints and two groups of parallel constraints can be given, among which the distance constraint is:  
\[