s6.vision.solver

Minimal geometric solvers for triangulation and tip estimation.

Provides pure-geometry routines that operate on Camera and simple vector primitives (Vector2D, Vector3D). Implementations rely on PyTorch to leverage the camera utilities and device/dtype management.

class s6.vision.solver.Solver

Bases: object

classmethod project_search_region(camera: Camera, center_world: Vector3D, radius_m: float) → Tuple[Vector2D, float, float] | None

Project a constant-size 3D spherical search region to the image.

Given a world-space center and a 3D radius (in meters), computes the 2D pixel center and the local pixel half-extents along image x and y by projecting small offsets in the camera frame using the camera’s projection model (including distortion when enabled).

Parameters:

  • camera (Camera) – Calibrated camera instance.

  • center_world (Vector3D) – Region center in world coordinates.

  • radius_m (float) – Search sphere radius in meters.

Returns:

Tuple of (center_px, r_px_x, r_px_y). Returns None if the projection fails or depth is invalid.

Return type:

(Vector2D, float, float) | None

classmethod triangulate(cam_0: Camera, cam_1: Camera, point_0: Vector2D, point_1: Vector2D) → Vector3D

Triangulate a 3D point from two calibrated camera observations.

The method forms two 3D rays from camera centers through the observed pixel coordinates by unprojecting, transforms them into world space, and computes the closest points between the two skew lines. The mid- point of the shortest segment connecting the rays is returned.

Parameters:

  • cam_0 (Camera) – Calibrated cameras providing intrinsics and extrinsics.

  • cam_1 (Camera) – Calibrated cameras providing intrinsics and extrinsics.

  • point_0 (Vector2D) – Pixel observations in the respective image planes.

  • point_1 (Vector2D) – Pixel observations in the respective image planes.

Returns:

Estimated 3D point in world coordinates. If the rays are nearly parallel, a zero vector is returned as a conservative fallback.

Return type:

Vector3D

classmethod solve_tip_point(camera: Camera, point: Vector2D, endpoint: Vector3D, length: float) → Vector3D

Intersects a camera ray with a sphere to estimate an instrument tip.

Given a 2D detection point in camera and a known endpoint of an instrument in 3D along with the instrument length, the method computes the intersection(s) of the camera ray with the sphere centered at endpoint with radius length. It selects the nearest valid intersection in front of the camera.

Parameters:

  • camera (Camera) – Calibrated camera used to form the viewing ray.

  • point (Vector2D) – Pixel coordinate of the tip projection.

  • endpoint (Vector3D) – Known 3D endpoint of the instrument (world frame).

  • length (float) – Distance from the endpoint to the tip.

Returns:

Estimated 3D tip position. Returns a zero vector if no valid intersection lies in front of the camera.

Return type:

Vector3D