Map Point Visibility Test

Implement a visibility test for map points in visual SLAM to determine if a point is visible from a given camera pose. This involves evaluating the point's projection onto the image plane and checking various conditions for visibility.

The concept of visibility is crucial in SLAM as it allows for efficient tracking of map points by only considering those that are currently visible. A map point is considered visible if it projects within the image bounds, has a positive depth value, and its viewing angle is within a certain threshold. The pinhole camera model is often used to describe this projection, where a 3D point $P$ is projected onto the image plane using the camera's intrinsic parameters $K$ and extrinsic parameters $R$ and $t$.

To determine visibility, the following steps are taken:

1. Transform the map point to the camera's coordinate system using $P_{cam} = R \cdot P + t$.
2. Check if the point has a positive depth value.
3. Project the point onto the image plane using $p = K \cdot P_{cam}$.
4. Calculate the image coordinates $u = \frac{p_0}{p_2}$ and $v = \frac{p_1}{p_2}$.

This technique is widely used in autonomous vehicles and robotics for SLAM and 3D reconstruction.