📘

Map Point Visibility Test

HardSLAM 📚 CV: 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 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:

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}$ .

\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}

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:

Transform point to camera coords: [0,0,5]
Depth = 5 > 0 ✓
Project: [320, 240] - center of image ✓
Within image bounds ✓ → Point is 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'

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

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.

👑

Continue the full explanation

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

🔒

Editor locked

The code editor is locked for Pro problems. It is only available for free problems. Please upgrade to gain access to the code editor for all problems.

solution.py

Python 3.13

Test Results

0/0

Run code to see test results.