Solve PnP on raw fisheye points

Recover camera pose from 3D-to-2D correspondences on a wide-FOV fisheye image, where cv2.solvePnP returns a wrong answer.

This is a task recipe. A naive pinhole PnP only ever considers points with z > 0.

It has no concept of the wider region a fisheye actually sees.

For why the fisheye model's real validity boundary is a tilted half-space rather than z > 0, and how much further it reaches, see Projection validity and FOV.

Prerequisites

• ds_msp installed, plus opencv-python and numpy (both come with it).
• A calibrated camera — here a Double Sphere model with known intrinsics. If you still need to calibrate, start from the README usage.
• At least 4 correspondences whose 3D points land in front of the camera. Fewer than 4 front-facing points and the solve cannot run (see Common failures).

Why cv2.solvePnP fails here

cv2.solvePnP assumes a pinhole projection: a 3D point maps to a pixel through one focal length and an optional polynomial distortion. A fisheye lens does not project that way.

Past ~90° the pinhole math has no valid pixel at all. Feed raw fisheye pixels to cv2.solvePnP and it silently fits the wrong model, returning a pose that is degrees off.

ds_msp solves the right problem in three steps:

1. Unproject each pixel to a 3D unit bearing ray (a direction the lens sees) with the fisheye model, in closed form.
2. Keep only the usable rays — those the model marks valid (unprojection succeeded) and whose ray component z > 0 (in front of the camera, required by the next step).
3. Solve PnP in the normalized plane (x/z, y/z) with an identity intrinsic. The rays are already metric, so no distortion model is needed downstream.

You get the same (success, rvec, tvec) triple as OpenCV, correct on fisheye data.

The two entry points

Two equivalent calls do the same solve. Pick the object API when you already hold a camera, or the functional wrapper when you're dropping this into existing cv2.solvePnP call sites.

Both examples below build the same synthetic scene: a known ground-truth pose, 40 world points projected through a Double Sphere model into fisheye pixels, then recovered.

Object API: cam.solve_pnp

Python 3.8+

"""Recover a fisheye camera's pose from 3D<->2D correspondences with `cam.solve_pnp`.

Builds a known ground-truth pose, projects 3D points through a Double Sphere model to
make 2D fisheye correspondences, asks `solve_pnp` to recover the pose, then measures
the error against ground truth. No external data needed -- everything is synthetic.
"""
import cv2
import numpy as np

from ds_msp import DoubleSphereCamera


def main() -> None:
    cam = DoubleSphereCamera(fx=711.57, fy=711.24, cx=949.18, cy=518.81,
                             xi=0.183, alpha=0.809, width=1920, height=1080)

    # 1. A ground-truth pose (what we want to recover).
    rvec_gt = np.array([0.05, -0.10, 0.02])      # Rodrigues vector, rad
    tvec_gt = np.array([0.30, -0.20, 1.00])      # translation, metres
    R_gt, _ = cv2.Rodrigues(rvec_gt)

    # 2. 40 world points spread in front of the camera.
    rng = np.random.default_rng(0)
    points_3d = rng.uniform([-2, -2, 4], [2, 2, 8], size=(40, 3))   # (40, 3) metres

    # 3. Project them through the fisheye to get 2D correspondences.
    P_cam = (R_gt @ points_3d.T + tvec_gt[:, None]).T               # (40, 3) camera frame
    uv, valid = cam.project(P_cam)                                  # uv: (40, 2) pixels
    points_2d = uv[valid]
    points_3d = points_3d[valid]

    # 4. Recover the pose from the 3D<->2D correspondences.
    success, rvec, tvec = cam.solve_pnp(points_3d, points_2d)
    print(success, len(points_3d))

    # 5. Measure the error against ground truth.
    R, _ = cv2.Rodrigues(rvec)
    rot_err_deg = np.degrees(np.arccos(np.clip((np.trace(R @ R_gt.T) - 1) / 2, -1, 1)))
    t_err_m = np.linalg.norm(tvec - tvec_gt)
    print(f"rotation error: {rot_err_deg:.2e} deg")
    print(f"translation error: {t_err_m:.2e} m")


if __name__ == "__main__":
    main()

$ python -m docs_src.how_to.solve_pnp_on_fisheye.solve_pnp_basic
True 40
rotation error: 0.00e+00 deg
translation error: 7.77e-16 m

All 40 points survive the front-facing filter, and the recovered pose matches ground truth to the float64 round-off floor.

Why is the error exactly zero, not just small?

cv2.SOLVEPNP_ITERATIVE is an iterative Levenberg-Marquardt refine, not a closed-form solve. Here the data is noiseless and the model exactly invertible, so it converges all the way to machine round-off (0.00e+00° rotation, translation error on the order of 1e-15 m) rather than stopping early. The exact trailing digits (7.77e-16 vs 5.09e-16, say) depend on your platform's BLAS/LAPACK backend — both are zero at any precision that matters.

On real detections with pixel noise, expect a sub-pixel reprojection RMS instead — this measurement is a correctness check, not a noise-robustness one.

Functional wrapper: ds_cv.solvePnP

ds_cv.solvePnP takes K and D instead of a camera object, so it drops into an existing cv2.solvePnP call site with minimal changes:

Python 3.8+

"""The OpenCV-style functional wrapper: `ds_cv.solvePnP(points_3d, points_2d, K, D)`.

Same synthetic scene and same solve as `solve_pnp_basic.py`, called through the
`cv2.solvePnP`-shaped wrapper instead of the object method -- so it drops into
existing `cv2.solvePnP` call sites. Confirms both entry points agree.
"""
import cv2
import numpy as np

import ds_msp.cv as ds_cv
from ds_msp import DoubleSphereCamera


def main() -> None:
    cam = DoubleSphereCamera(fx=711.57, fy=711.24, cx=949.18, cy=518.81,
                             xi=0.183, alpha=0.809, width=1920, height=1080)

    rvec_gt = np.array([0.05, -0.10, 0.02])
    tvec_gt = np.array([0.30, -0.20, 1.00])
    R_gt, _ = cv2.Rodrigues(rvec_gt)

    rng = np.random.default_rng(0)
    points_3d = rng.uniform([-2, -2, 4], [2, 2, 8], size=(40, 3))

    P_cam = (R_gt @ points_3d.T + tvec_gt[:, None]).T
    uv, valid = cam.project(P_cam)
    points_2d = uv[valid]
    points_3d = points_3d[valid]

    # cam.K is the pinhole matrix; cam.D = [xi, alpha] are the DS distortion coefficients.
    success, rvec, tvec = ds_cv.solvePnP(points_3d, points_2d, cam.K, cam.D)
    print(success, rvec.shape, tvec.shape)                 # -> (3, 1) (3, 1), cv2-native shape

    R, _ = cv2.Rodrigues(rvec)
    rot_err_deg = np.degrees(np.arccos(np.clip((np.trace(R @ R_gt.T) - 1) / 2, -1, 1)))
    t_err_m = np.linalg.norm(tvec.squeeze() - tvec_gt)
    print(f"rotation error: {rot_err_deg:.2e} deg")
    print(f"translation error: {t_err_m:.2e} m")


if __name__ == "__main__":
    main()

$ python -m docs_src.how_to.solve_pnp_on_fisheye.solve_pnp_cv_wrapper
True (3, 1) (3, 1)
rotation error: 0.00e+00 deg
translation error: 7.77e-16 m

Same scene, same solve, identical error — the wrapper is a thin shim over cam.solve_pnp, not a different algorithm.

Return-shape differences

The two entry points differ only in the shape of what comes back:

• cam.solve_pnp returns squeezed (3,) rvec/tvec.
• ds_cv.solvePnP returns (3, 1) column vectors, matching cv2.solvePnP's native shape.
• Both return (False, ...) if fewer than 4 points survive the front-facing filter.

Contrast: pinhole PnP on the same points

Hand the same fisheye pixels to cv2.solvePnP with the camera's pinhole K. It fits the wrong model:

Python 3.8+

"""Hand the same fisheye pixels to `cv2.solvePnP` with a pinhole `K` -- it fits the wrong model.

Same synthetic scene as `solve_pnp_basic.py`, but solved with plain `cv2.solvePnP`
(pinhole assumption, zero distortion) instead of `cam.solve_pnp`. Contrasts the
recovered-pose error against the fisheye-aware solve.
"""
import cv2
import numpy as np

from ds_msp import DoubleSphereCamera


def main() -> None:
    cam = DoubleSphereCamera(fx=711.57, fy=711.24, cx=949.18, cy=518.81,
                             xi=0.183, alpha=0.809, width=1920, height=1080)

    rvec_gt = np.array([0.05, -0.10, 0.02])
    tvec_gt = np.array([0.30, -0.20, 1.00])
    R_gt, _ = cv2.Rodrigues(rvec_gt)

    rng = np.random.default_rng(0)
    points_3d = rng.uniform([-2, -2, 4], [2, 2, 8], size=(40, 3))

    P_cam = (R_gt @ points_3d.T + tvec_gt[:, None]).T
    uv, valid = cam.project(P_cam)
    points_2d = uv[valid]
    points_3d = points_3d[valid]

    ok, rv, tv = cv2.solvePnP(points_3d.astype(np.float64),
                              points_2d.astype(np.float64),
                              cam.K, np.zeros(5))               # pinhole assumption
    R_bad, _ = cv2.Rodrigues(rv)
    bad_rot = np.degrees(np.arccos(np.clip((np.trace(R_bad @ R_gt.T) - 1) / 2, -1, 1)))
    print(f"cv2 rotation error: {bad_rot:.2f} deg")
    print(f"cv2 translation error: {np.linalg.norm(tv.squeeze() - tvec_gt):.2f} m")


if __name__ == "__main__":
    main()

$ python -m docs_src.how_to.solve_pnp_on_fisheye.pinhole_contrast
cv2 rotation error: 0.57 deg
cv2 translation error: 1.37 m

A 0.57° rotation and 1.37 m translation error from the same data — that gap is the fisheye distortion that cv2.solvePnP cannot model.

Common failures

Three symptoms account for nearly every PnP failure on fisheye data:

Symptom	Cause	Fix
Pose is degrees off, no error raised	Used cv2.solvePnP with pinhole K on raw fisheye pixels	Switch to cam.solve_pnp / ds_cv.solvePnP
solve_pnp returns (False, None, None)	Fewer than 4 points are in front of the camera after unprojection	Add correspondences, or check that your 3D points are actually in view
Recovered pose flips sign	Points behind the camera (z <= 1e-6) leaked in	The z > 1e-6 ray check filters these. Confirm your ground-truth pose puts every point in front: ((R_gt @ P.T).T + t)[:, 2] > 0 should be all True

The solver drops any pixel that unprojects to an invalid or behind-camera ray (z <= 1e-6) before it solves.

If that leaves fewer than 4 points, it returns (False, None, None) rather than guess.

Next steps

• Two views instead of one — to recover the relative pose between two fisheye cameras from matched points (no known 3D), the ray-based cousin of this recipe is Two-view geometry on rays.
• The geometry behind the filter — the real (tilted half-space, not z > 0) validity boundary and how far it reaches: Projection validity and FOV.

Recap: on fisheye data, unproject pixels to rays, keep the front-facing valid ones, then solve PnP in the normalized plane — cam.solve_pnp does all three and recovers pose to the float64 round-off floor (0.00e+00° rotation error, on this synthetic scene).

---

Source: ds_msp/ops/pose.py · DoubleSphereCamera.solve_pnp · ds_msp.cv.solvePnP