## 2 Proposed VO System
### Solution Implementation
#### 2.3.1 Movement Estimation  
The movement between two frames, \(l_{k}\) and \(l_{k+1}\), can be represented as a transformation matrix, \(T_{k+1,k}\in\mathbb{R}^{3\times 3}\), that takes the following form:  
\[T_{k+1,k}= \begin{bmatrix}R_{k+1,k}&t_{k+1,k}\\ 0&1\end{bmatrix}  \tag{1}\]  
where \(R_{k+1,k}\) represents the rotation matrix, and \(t_{k+1,k}\) is the translation matrix that describes the movement. On the other hand, the position of the robot at time \(K\) can be represented as a vector, \(C_{k}=[c_{x},c_{y},1]^{T}\), where \(c_{x}\) and \(c_{y}\) represent the pose on the \(x\) and \(y\) axes, respectively. The sequence of positions of the robot can be described as \(C_{0,k}=\left\{C_{0},C_{1},...,C_{k}\right\}\). These positions refer to both the camera coordinates and the robot coordinates since it can be assumed that they are the same for the VO algorithm. In this way, the new pose of the robot, \(C_{k+1}\), at time \(k+1\) can be obtained from the previous one, \(C_{k}\), and the calculated movement between the two frames, \(T_{k+1,k}\), can be calculated as follows:  
\[C_{k+1}=T_{k+1,k}\cdot C_{k} \tag{2}\]  
To obtain the transformation matrix \(T_{k+1,k}\) between the frames \(k\) and \(k+1\), the set of features of each frame and their movement obtained in the flow analysis process are used. First, the translation vector, \(t_{k+1,k}\), is calculated as the difference of the centers of mass of both sets of key points, \(t_{k+1,k}=G_{k+1}-G_{k}\), where \(G_{i}\) represents the center of mass  
Figure 1: Flow diagram of the algorithm. of the set of key points in frame \(i\). For the calculation of the rotation matrix, \(R_{k+1,k}\), the reprojection error between the set of key points of both frames is minimized. Describing as \(\hat{P_{k}}\) the set of features from frame \(k\), displaced by the translation vector \(t_{k+1,k}\) and as \(P_{k+1}\), the set of key points from the frame \(k+1\), a rotation matrix, \(R_{k+1,k}\), can be obtained that fulfills the following:  
\[\hat{P_{k}}=R_{k+1,k}\cdot P_{k+1} \tag{3}\]  
This matrix is obtained by minimizing the expression of the quadratic rotation error, and it declares the problem as minimizing the following statement:  
\[\frac{1}{N}\cdot\sum_{i=1}^{N}(\hat{P_{k}}-R_{k+1,k}\cdot P_{k+1})^{T}\cdot( \hat{P_{k}}-R_{k+1,k}\cdot P_{k+1}) \tag{4}\]  
By mathematically developing this equation, the problem can be transformed into maximizing the following expression:  
\[\frac{1}{N}\cdot\sum_{i=1}^{N}(\hat{P_{k}}^{T}\cdot R_{k+1,k}\cdot P _{k+1}) \tag{5}\] \[=tr(R_{k+1,k}^{T}\cdot\frac{1}{N}\cdot\sum_{i=1}^{N}(\hat{P_{k}} ^{T}\cdot P_{k+1}))\]  
where \(tr(A)\) represents the trace of the matrix \(A\). The term \(\frac{1}{N}\cdot\sum_{i=1}^{N}(\hat{P_{k}}^{T}\cdot P_{k+1})\) can be represented as a matrix, \(c\), also called a cross-dispersion or correlation matrix. This matrix can be decomposed into singular values as \(c=u\cdot w\cdot v^{T}\), where \(u\) and \(v\) are orthogonal matrices, and \(w\) has all singular values of \(c\). Therefore, Equation (5) can be written as follows:  
\[tr\Big{(}R_{k+1,k}^{T}\cdot c\Big{)}=tr\Big{(}R_{k+1,k}^{T}\cdot u\cdot w\cdot v ^{T}\Big{)} \tag{6}\]  
Since \(tr(a\cdot b)\) equals \(tr(b\cdot a)\), the terms in (6) can be moved, leaving \(w\) at the end and leaving the equation as follows:  
\[tr\Big{(}v^{T}\cdot R_{k+1,k}^{T}\cdot u\cdot w\Big{)}=tr\big{(}c^{\prime} \cdot w\big{)} \tag{7}\]  
where \(c^{\prime}\) is defined as \(c^{\prime}=v^{T}\cdot R_{k+1,k}\cdot u\). Since \(w\) is a diagonal matrix, only the diagonal elements of \(c^{\prime}\) add to the trace. Furthermore, since all three matrices composing \(c^{\prime}\) are orthogonal, it must also be orthogonal. As a consequence, \(tr(c^{\prime}\cdot w)\) is maximized when \(c^{\prime}\) is equal to \(\mathbb{I}\). This finally allows for the determination of the rotation matrix as demonstrated below:  
\[R_{k+1,k}=u\cdot v^{T} \tag{8}\]  
These calculations finally allow the transformation matrix in Equation (1) to be built.