# A Vision-Based Approach for Autonomous Motion in Cluttered Environments
## 3 Methods
### Perception System
#### 3.2.2 Camera Projection and Coordinate Transformation  
The D455 depth camera is capable of estimating the depth value of obstacles and representing it as a depth map. In order to run the Raycast Model and occupancy grid map algorithms in Sections 3.2.3 and 3.2.4, we need to know the coordinates of the obstacles in the world coordinate system. In this section, we introduce how to use the pose information provided by T265 and the depth map provided by D455 to estimate the location of obstacles in the world coordinate system. The process can be divided into two steps. First, the camera projection model is used to convert the depth map into the point cloud information of the obstacles in the camera coordinate system. Then the coordinates of the obstacles in the world coordinate system are calculated by using the spatial coordinate transformation.  
Figure 2: Schematic of the framework of the environmental perception system. The system uses a tracking camera and a depth camera as sensors to realize the estimation of the robot’s pose and build a map of the environment.  
The camera projection model is shown in Figure 3. \(P(X,Y,Z)\) is an obstacle point which is projected by the optical center, \(O\), on the imaging plane, \(O^{\prime}X^{\prime}Y^{\prime}\), to obtain \(P^{\prime}(X^{\prime},Y^{\prime})\), and f is the focal distance of the camera. Therefore, according to the geometric triangle properties, the relationship between \(P\) and \(P^{\prime}\) can be obtained as follows:  
\[\left\{\begin{array}{l}X^{\prime}=f\frac{X}{Z}\\ Y^{\prime}=f\frac{Y}{Z}\end{array}\right. \tag{1}\]  
The camera imaging system will convert the light felt at \([X^{\prime},Y^{\prime}]\) into the pixel value of the image at the pixel point \([u,v]\). The transformation from \([X^{\prime},Y^{\prime}]\) to \([u,v]\) requires a scaling and translation. The scaling ratios of the \(X\)-axis and \(Y\)-axis were recorded as \(\alpha\) and \(\beta\), respectively. The translations of the \(X\)-axis and \(Y\)-axis are \(c_{x}\) and \(c_{y}\), respectively. Thus, the relationship between \(P^{\prime}(X^{\prime},Y^{\prime})\) and \([u,v]\) is as follows:  
\[\left\{\begin{array}{l}u=\alpha X^{\prime}+c_{x}\\ v=\beta Y^{\prime}+c_{y}\end{array}\right. \tag{2}\]  
If we substitute Formula (1) into Formula (2), then we can get the following:  
\[\left\{\begin{array}{l}u=\alpha f\frac{X}{Z}+c_{x}\\ v=\beta f\frac{X}{Z}+c_{y}\end{array}\right. \tag{3}\]  
Let \(f_{x}=\alpha f\), \(f_{y}=\beta f\), the Formula (4) can be obtained:  
\[\left\{\begin{array}{l}X=(u-c_{x})\frac{Z}{f_{y}}\\ Y=\left(v-c_{y}\right)\frac{Z}{f_{y}}\end{array}\right. \tag{4}\]  
Therefore, the position, \(P(X,Y,Z)\), of the obstacle corresponding to each pixel, \([u,v]\), in the camera coordinate system, \(OXY\), can be calculated by Formula (4), where the \(Z\) value is obtained by querying the value of the depth image at the pixel, \([u,v]\), and \(f_{x}\), \(f_{y}\), \(c_{x}\), and \(c_{y}\) can be obtained by camera calibration.  
Since the camera is not stationary but moves with the robot, it is also necessary to use Formula (5) to convert the obstacle coordinate, \(P_{C}\), in the camera coordinate system to the coordinate \(P_{W}\) in the world coordinate system. In Formula (5), \(R_{wc}\) and \(t_{wc}\) are the rotation matrix and translation vector of the camera in the world coordinate system. These two can be estimated by T265 camera.  
\[P_{w}=R_{wc}P_{C}+t_{wc} \tag{5}\]  
Figure 3: Schematic diagram of camera projection. \(P\) is the position of the spatial point, \(O\) is the optical center of the camera, \(O^{\prime}X^{\prime}Y^{\prime}\) is the camera imaging plane, and \(P^{\prime}\) is the projection of \(P\) on the imaging plane.