## 3D Target Tracking
### Spatial-Positioning Method Based on Geometric Constraints
\begin{split} d_{ij}^{2}=(p_{wxi}-p_{wyi})^{2}+(p_{wyi}-p_{wyi})^{ 2}+(p_{wzi}-p_{wzi})^{2}\\ =(p_{cxi}\times p_{wzi}-p_{cxy}\times p_{wzi})^{2}+(p_{cyi}\times p _{wzi}-p_{cyi}\times p_{wzi})^{2}+(p_{wzi}-p_{wzi})^{2}\\ i,j=1,2,3,4\text{ and }i\
eq j\end{split}  \tag{11}\]  
The parallel vector constraint is shown in Figure 9, and the expansion of the mathematical expression of \(\overset{\rightarrow}{V_{12}}\left|\right|\overset{\rightarrow}{V_{34}}\) and \(\overset{\rightarrow}{V_{13}}\left|\right|\overset{\rightarrow}{V_{24}}\) in Cartesian coordinates can be expressed as six equations with the unknown parameter \(p_{wzi}\). Considering that there are four unknown depth-information values of four feature points, we needed to reasonably select and use at least four of the 12 geometric constraint equations, so as to construct the multiple quadratic well-posed or overdetermined equation set \(F\) about \(p_{wzi}\), which can be obtained by solving \(F\). Note that \(p_{wzi}\) must be positive, and all non-positive solutions in the space of \(F\) solution are pseudo-solutions. From the author's previous related work [24], we introduced the Improved Newton Iteration Method, which is an improved Newton-Raphson method with the fifth-order convergence adapted, whose iterative equation is as follows:  
\[ \begin{split}\mathrm{Y}^{n}=\mathrm{X}^{n}-\frac{F(\mathrm{X}^{n })}{F^{\prime}(\mathrm{X}^{n})},\end{split}  \tag{12}\]  
\[ \begin{split} Z^{n}=\mathrm{X}^{n}-\frac{2F(\mathrm{X}^{n})}{F^{ \prime}(\mathrm{X}^{n})+F^{\prime}(\mathrm{Y}^{n})},\end{split}  \tag{13}\]  
Figure 9: Geometric constraints. \[\mathrm{X}^{n+1}=Z^{n}-\frac{F(Z^{n})}{F^{\prime}(Y^{n})}, \tag{14}\]  
where \(\mathrm{X}^{n}\) is the vector to be solved composed of depth value \(p_{wzi}\), and \(\mathrm{Y}^{n}\) and \(\mathrm{Z}^{n}\) are the intermediate variables of the iteration. By substituting the solved \(p_{wzi}\) into Equation (10), the coordinates \(P_{w}i\) of the spatial-feature points of the target in the camera coordinate system can be obtained. \(R\) and \(T\) of the posture were obtained with the monocular VSLAM method:  
\[R\times Pi+T=P_{wi}\ \ \Rightarrow\ \ Pi=R^{-1}\times P_{wi}-T, \tag{15}\]  
where \(R\) is the rotation matrix of the camera relative to the world coordinate system, and \(T\) is the displacement vector of the camera relative to the world coordinate system. The coordinates \(Pi\) of the target feature point in the world coordinate system can be obtained by solving the Equation (15). In this study, the method was extended to the solution of circular targets. The properties of the circle projection are that the projection of the circle is a circle or an ellipse, and the projection of the circle is still the center of the projection. The center of the projection-surface image can be obtained with image algorithms. Following the projection properties, the line segment was obtained through the intersection of any two rays at the center, and the graph is radius \(R\), as shown in Figure 10. According to the symmetry of the circle, the shape formed by the four intersections must be a parallelogram. Therefore, solving the depth problem essentially returns to the method of solving the parallelogram above. In actual engineering, the side length scale of the quadrilateral obtained by this method is difficult to obtain. The diagonal distance is the radius \(R_{c}\), which is easier to calculate:  
\[ \begin{split}|\mathrm{R}c|=&\Big{|}d_{ij}^{2}|=(p_{ wxi}-p_{wxj})^{2}+(p_{wyi}-p_{wwyj})^{2}+(p_{wzi}-p_{wzj})^{2}\\ =&(p_{cxi}\times p_{wzi}-p_{cxy}\times p_{wzj})^{2}+( p_{cyi}\times p_{wzi}-p_{cyj}\times p_{wzj})^{2}+(p_{wzi}-p_{wzj})^{2}\\ &\ \ \ \ for\ i=1,j=3\ or\ i=2,j=4.\end{split}  \tag{16}\]  
Generally, the combination of the diagonal distance \(R_{c}\) and the quadrilateral parallel condition equation is performed to obtain the solution depth of the equation group \(F\) and thus the spatial position of the circular target.