# Multi-Layered Interactive Target Guidance with Visual Safety in Convex-Shaped Obstacle Environments
## 2 Problem Formulation
### Sensor Model and Field of View Maintenance  
To enable the robot to pursue the target object, it is necessary to obtain the relative pose \(g_{co}\). Here, a method is adopted to estimate \(g_{co}\) from the feature points of the target object obtained by a camera, such as an RGB camera or an RGB-D camera, mounted on the robot. The target has \(n_{f}\) features, each of which can be treated as a point. The feature points as viewed from \(\Sigma_{o}\) are presented as \(p_{oi}\in\mathbb{R}^{3}\), and the configuration of the feature points is assumed to be known. The feature points as viewed from \(\Sigma_{c}\) are presented as \(p_{ci}\in\mathbb{R}^{3}\), which satisfies \(p_{ci}=e^{\hat{g}_{co}}p_{oi}+p_{co}\). In the case of an RGB camera, as illustrated in Figure 1, the projected \(p_{ci}\) on the image plane is extracted to estimate \(g_{co}\). For an RGB-D camera, the feature point \(p_{ci}\) is directly extracted and used for pose estimation. In unstructured environments with obstacles, such as the current scenario, it is known that using only RGB data reduces the accuracy of pose estimation [29]. Therefore, in the scenario of this paper, the use of an RGB-D camera, which can also utilize depth data, is more appropriate. In order to pursue a target, it is necessary to keep the target within the field of view of the visual sensor and obtain the relative pose \(g_{co}\). We call the property that the camera robot achieves as field of view maintenance. **Definition 1** (Field of view maintenance). : _The camera robot is said to achieve field of view maintenance when all feature points \(p_{ci}\) of the target object are located within the field of view._  
Next, a sufficient condition for field of view maintenance is presented. Consider the case where the camera is positioned at the origin of the camera-fixed frame \(\Sigma_{c}\) with the optical axis aligned along the z-axis. Then, the condition under which the field of view maintenance is achieved is equivalent to feature points being included in the cone-shaped area illustrated in Figure 1. This can be formulated as  
\[p_{ci}^{T}Tp_{ci}\geq 0, \tag{8}\]  
where \(T\coloneqq\text{diag}\big{(}-1/\tan^{2}\phi,-1/\tan^{2}\phi,1\big{)}\), and \(\phi\in\mathbb{R}\) represents the sensing range. The distance of the furthest feature point \(p_{oi}\) from the origin of \(\Sigma_{o}\) is denoted as \(r_{M}\coloneqq\max_{i\in 1,\dots,n_{f}}\|p_{oi}\|\). If the sphere with the radius of \(r_{M}\) centered on the origin of \(\Sigma_{o}\) is included in the cone, then all feature points can be measured. Next, we consider a condition  
\[p_{co}^{T}T_{M}p_{co}-\Delta_{fov}\geq 0, \tag{9}\]  
Figure 1: The field of view of the camera. The point \(p_{oi}\) denotes the coordinate of a feature point with respect to the target-fixed frame \(\Sigma_{o}\). where \(\Delta_{fov}>0\), and  
\[T_{M}\coloneqq\text{diag}\bigg{(}-\frac{1}{\tan^{2}\phi_{M}},-\frac{1}{\tan^{2} \phi_{M}},1\bigg{)},\ \ \phi>\phi_{M}>0. \tag{10}\]  
The parameter \(\phi_{M}\) is a design parameter, and a condition to be met will be provided later. For the condition (9), the following proposition holds. **Proposition 1**: _Suppose that the parameters \(\phi_{M}\) and \(\Delta_{fov}\) are chosen such that_  
\[\sqrt{\Delta_{fov}}\frac{\sin(\phi-\phi_{M})}{\cos\phi_{M}}\geq r_{M}, \tag{11}\]  
_and (9) holds for a relative pose \(g_{co}\). Then, field of view maintenance in the sense of Definition 1 is achieved for the pose \(g_{co}\)._  
Let \(p_{co}\) be represented as \(p_{co}=(x_{co},y_{co},z_{co})\). Then, \(z_{co}\geq\sqrt{\Delta_{fov}}\) is derived from the definition of \(T_{M}\) and (9). The distance between \(p_{co}\), which satisfies \(p_{co}^{T}T_{M}p_{co}\geq 0\), \(z_{co}\geq\sqrt{\Delta_{fov}}\) and the set \(\big{\{}q\in\mathbb{R}^{3}\mid q^{T}Tq\leq 0\big{\}}\) take the minimum value when \(p_{co}^{T}T_{M}p_{co}=0\) and \(z_{co}=\sqrt{\Delta_{fov}}\). The minimum distance is \(\sqrt{\Delta_{fov}}\frac{\sin(\phi-\phi_{M})}{\cos\phi_{M}}\).