# Non-Cooperative Spacecraft Pose Measurement with Binocular Camera and TOF Camera Collaboration
## 3 Detailed Description of the Proposed Method
### Binocular Initial Pose Acquisition
#### 3.2.2 Relative Motion Calculation  
As shown in Figure 7, the binocular initial pose estimation can be divided into the following steps.  
Figure 6: Schematic diagram of the feature extraction and tracking.  
Figure 7: Flow chart of the initial pose acquisition phase.  
1. For the first frame, the feature point extraction is performed in the left and right images. A set of feature point pairs is obtained through brute force matching and the epipolar constraint criterion between the left and right cameras. Its three-dimensional coordinates in the left camera coordinate system are calculated through the principle of triangulation. 2. For a non-first frame, the number of feature points will decrease if the optical flow tracking time is too long. If the number of feature points tracked in the current frame is less than threshold \(T_{N}\) (since perspective-n-point problems require at least 3 sets of points, the threshold should be greater than 3, which was set as 6 in this manuscript), then this frame adopts the method of the first frame to add new feature points. After obtaining the 3D set in the left camera coordinate system of the previous frame and the 2D feature point set of the left image in this frame, the rotation matrix and translation vector are solved quickly based on the random sample consensus (RANSAC) [27] method. 3. Since the surface of a non-cooperative spacecraft has multilayer reflective materials, the texture information is relatively lacking, which leads to the appearance of outliers in the binocular measurement process. A Kalman filter model [28] was introduced to eliminate the outliers and ensure the stability of the system in this paper. Assuming that the acceleration of the tracking spacecraft relative to the target spacecraft is constant, the geometric kinematics between the two spacecrafts is modeled. The state and observation of the system can be represented as: \[\mathbf{X}_{k}=\mathbf{A}(\mathbf{X}_{k-1})+\mathbf{W}_{k-1}\] (13) \[\mathbf{Z}_{k}=\mathbf{H}(\mathbf{X}_{k})+\mathbf{V}_{k}\] (14) The state vector and observation vector are defined as \(\mathbf{X}_{k}=[x,y,z,\theta_{x},\theta_{y},\theta_{z},\dot{x},\dot{y},\dot{z}, \dot{\theta}_{x},\dot{\theta}_{y},\dot{\theta}_{z}]^{\mathrm{T}}\) and \(\mathbf{Z}_{k}=[\dot{x},\dot{y},\dot{z},\dot{\theta}_{x},\dot{\theta}_{y},\dot{ \theta}_{z}]^{\mathrm{T}}\). \(\mathbf{W}_{k-1}\) and \(\mathbf{V}_{k}\) are the noise vector of the system and the noise vector in the observation process, respectively. The state transition matrix and observation matrix of the system can be defined as: \[\mathbf{A}_{k}=\begin{bmatrix}\mathbf{I}_{3\times 3}&\mathbf{O}_{3\times 3}&\Delta t \cdot\mathbf{I}_{3\times 3}&\mathbf{O}_{3\times 3}\\ \mathbf{O}_{3\times 3}&\mathbf{I}_{3\times 3}&\mathbf{O}_{3\times 3}&\Delta t\cdot\mathbf{I}_{3 \times 3}\\ \mathbf{O}_{3\times 3}&\mathbf{O}_{3\times 3}&\mathbf{I}_{3\times 3}&\mathbf{O}_{3\times 3}\\ \mathbf{O}_{3\times 3}&\mathbf{O}_{3\times 3}&\mathbf{O}_{3\times 3}&\mathbf{I}_{3\times 3} \\ \end{bmatrix},\mathbf{H}_{k}=\begin{bmatrix}\mathbf{I}_{3\times 3}&\mathbf{O}_{3\times 3}&\mathbf{O}_{3 \times 3}&\mathbf{O}_{3\times 3}&\mathbf{O}_{3\times 3}\\ \mathbf{O}_{3\times 3}&\mathbf{I}_{3\times 3}&\mathbf{O}_{3\times 3}&\mathbf{O}_{3\times 3} \\ \end{bmatrix}\] (15) The predicted value of the filter is employed as the rotation and translation vector of the current frame to ensure that the target can still be tracked stably during the initial visual pose acquisition when abnormal values appear in the measurement process.