Map Point Visibility Test

hardPROSLAMCV: Structure from Motion and SLAM

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 has a positive depth value and projects within the image bounds. 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:

Transform the map point to the camera's coordinate system using $P_{cam} = R \cdot P + t$ .
Check if the point has a positive depth value.
Project the point onto the image plane using $p = K \cdot P_{cam}$ .
Calculate the image coordinates $u = \frac{p_0}{p_2}$ and $v = \frac{p_1}{p_2}$ .
Check the bounds: $0 \le u < W$ and $0 \le v < H$ , where image_size is $(H, W)$ .

\begin{aligned} P_{cam} &= R \cdot P + t \\ p &= K \cdot P_{cam} \\ u &= \frac{p_0}{p_2} \\ v &= \frac{p_1}{p_2} \end{aligned}

Output

Return a dict with:

'visible': True only if the depth is positive and $(u, v)$ is inside the image.
'projection': $[u, v]$ rounded to 1 decimal place, or None if the point is not visible.
'depth': the camera-frame depth (the third coordinate of $P_{cam}$ ), rounded to 2 decimal places when positive.

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

Example

Input

map_point = [0, 0, 5]  # 5m in front
camera_pose = [identity_R, zero_t]
K = [[500,0,320],[0,500,240],[0,0,1]]
image_size = (480, 640)

Output

{'visible': True, 'projection': [320, 240], 'depth': 5.0}

Reasoning

Inputs: map_point is the world point $P = (0, 0, 5)$ ; camera_pose holds a rotation $R = I$ and translation $t = (0, 0, 0)$ ; K has focal length $f = 500$ and principal point $(c_x, c_y) = (320, 240)$ ; image_size is $(H, W) = (480, 640)$ .
To camera frame: $P_{cam} = I \cdot (0, 0, 5) + 0 = (0, 0, 5)$ . Its third coordinate is the depth, $5 > 0$ : the point is in front of the camera.
Project: $p = K \cdot P_{cam} = (500 \cdot 0 + 320 \cdot 5,\; 500 \cdot 0 + 240 \cdot 5,\; 5) = (1600, 1200, 5)$ , so $u = 1600 / 5 = 320$ and $v = 1200 / 5 = 240$ .
Bounds: $0 \le 320 < 640$ and $0 \le 240 < 480$ — inside the image.
Output: {'visible': True, 'projection': [320, 240], 'depth': 5.0}. Notice that a point on the optical axis lands exactly on the principal point; moving it to $(1, 0, 5)$ gives $u = 320 + 500 \cdot \frac{1}{5} = 420$ , while $(0, 0, -5)$ sits behind the camera and is not visible.

Constraints

map_point: 3D point [X, Y, Z]
camera_pose: [R, t] camera extrinsics
K: Camera intrinsics
image_size: (H, W)
Return: Dict with 'visible', 'projection', 'depth'

Learn this in

Visual SLAM & Localization3D Computer Vision

More from CV: Structure from Motion and SLAM

Triangulation of 3D Pointsmedium PnP Pose from Correspondencesmedium Checkerboard Corner Detectionhard Range-Bearing Observationmedium Odometry Pose Updateeasy

Concept primerPREVIEW

Background Knowledge

In feature-based visual SLAM, the world is represented by map points (3D landmarks) and camera poses (position + orientation). At each new frame, the system needs to decide which existing 3D points are potentially visible in the current camera, so it can try to match their descriptors in the image. Checking every map point is too expensive, so we do a visibility test / culling to quickly discard points that cannot be seen.

Geometrically, visibility is determined by the camera projection model. A 3D point in world coordinates is transformed into the camera frame using the camera pose, then projected onto the image plane using intrinsics. For a point to be a useful candidate: it must lie in front of the camera (positive depth), its projection must fall inside the image boundaries, the viewing angle with respect to the point’s preferred viewing direction must not be too oblique, and its distance should match the scale level at which the point’s descriptor was created (to keep matching robust and efficient, as in ORB-SLAM).

Algorithm / Approach

The typical pattern is:

For each candidate 3D map point, perform a cheap sequence of geometric tests in increasing cost:
Transform to camera coordinates.
Check depth sign.
Project to pixel coordinates.
Check image bounds + possible border margin.
Check viewing direction (angle between point’s normal / mean viewing vector and current viewing ray).
Check distance vs. scale range (use point’s min/max distance based on pyramid levels used when the point was observed).
If a point passes all tests, mark it as visible and include it in the candidate set for descriptor matching / tracking.

This is purely geometric; it does not perform any descriptor comparison itself, just filters the set of points to try.

Step-by-Step Strategy

Assume:

Camera pose as $T_{cw} = [R_{cw} | t_{cw}]$ (world → camera).
Camera intrinsics $K = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}$ .
Map point has:
3D position $X_w$
A normal/viewing direction $n$ (e.g., average viewing vector from when it was created)
A min and max distance $d_{\min}, d_{\max}$ derived from its scale level.

Step-by-step:

Transform point to camera frame

Compute $X_c = R_{cw} X_w + t_{cw}$ .
Let $X_c = (X, Y, Z)$ .

Depth (front-of-camera) check

If $Z \le 0$ , the point is behind the camera → not visible.

Project onto image plane

Compute normalized coordinates:
$x_n = X / Z,\quad y_n = Y / Z$ .
Apply intrinsics:
$u = f_x x_n + c_x$
$v = f_y y_n + c_y$ .

Image bounds check

If $u$ or $v$ is outside $[0,$ \text{width} $) \times [0,$ \text{height} $)$ (optionally with a small margin for patch size), cull the point.

PRO

Continue the full explanation

You're reading the free preview. Unlock the complete walkthrough, the code editor, test runner and reference solution with Premium.

Output

Example

Constraints

Learn this in

More from CV: Structure from Motion and SLAM

Continue the full explanation

Editor locked