如何基于PNP算法从等矩形图像估计360相机位姿？优先用OpenCV

Got it, let's break down how to estimate your 360 camera's pose (rotation and position) using OpenCV, since you've got an equirectangular image and 2D-3D correspondences. The key here is handling the equirectangular projection first—since it's not a standard pinhole camera, we can't use solvePnP directly with raw pixel coordinates.

Equirectangular images map the entire sphere to a flat rectangle, so each pixel corresponds to a point on a unit sphere centered at the camera. We need to convert these pixel positions into normalized camera coordinates (points on the z=1 plane of the camera's coordinate system) to work with OpenCV's pinhole-based pose estimation tools.

Here's the conversion logic (assuming your image width is W, height is H, and pixel coordinates are (u, v)):

• Calculate longitude θ: θ = (u / W) * 2π - π (ranges from -π to π, left to right)
• Calculate latitude φ: φ = (v / H) * π - π/2 (ranges from π/2 to -π/2, top to bottom)
• Convert to unit sphere coordinates (camera pointing along +z axis):
  x = cos(φ) * sin(θ)
y = -sin(φ)
z = cos(φ) * cos(θ)
• Project onto the z=1 plane (normalized pinhole coordinates): norm_x = x/z, norm_y = y/z

Skip pixels where z=0 (left/right edges of the equirectangular image)—these correspond to rays perpendicular to the camera's axis and can't be projected onto the z=1 plane.

• Gather at least 6 pairs of 2D pixel points (from your equirectangular image) and their matching 3D world points (the more pairs, the better accuracy).
• Convert all valid 2D pixels to normalized camera coordinates using the formula above.

solvePnP is OpenCV's go-to function for calculating camera rotation (rvec) and translation (tvec) from 3D-2D correspondences. Since we're using normalized coordinates, we can set the camera intrinsic matrix K to the identity matrix.

Full Code Example #

import cv2
import numpy as np

# ----------------------
# Replace these with your actual data
# ----------------------
# 3D world points (X, Y, Z) in your world coordinate system
object_points = np.array([
    [0.0, 0.0, 0.0],
    [1.5, 0.0, 0.0],
    [0.0, 2.0, 0.0],
    [1.5, 2.0, 0.0],
    [0.0, 0.0, 1.0],
    [1.5, 0.0, 1.0]
], dtype=np.float32)

# 2D pixel points from your equirectangular image (u, v)
image_pixels = np.array([
    [2048, 1024],
    [2560, 1024],
    [2048, 768],
    [2560, 768],
    [2048, 1280],
    [2560, 1280]
], dtype=np.float32)

# Dimensions of your equirectangular image
IMG_WIDTH = 4096
IMG_HEIGHT = 2048
# ----------------------

# Convert equirectangular pixels to normalized camera coordinates
normalized_points = []
for u, v in image_pixels:
    theta = (u / IMG_WIDTH) * 2 * np.pi - np.pi
    phi = (v / IMG_HEIGHT) * np.pi - np.pi/2
    
    x = np.cos(phi) * np.sin(theta)
    y = -np.sin(phi)
    z = np.cos(phi) * np.cos(theta)
    
    if np.abs(z) < 1e-6:
        print(f"Skipping pixel ({u}, {v}) - z is too close to 0")
        continue
    norm_x = x / z
    norm_y = y / z
    normalized_points.append([norm_x, norm_y])

normalized_points = np.array(normalized_points, dtype=np.float32)

# Set intrinsic matrix (identity since we're using normalized points)
K = np.eye(3, dtype=np.float32)
dist_coeffs = np.zeros((4, 1), dtype=np.float32)  # No distortion for equirectangular (usually)

# Run solvePnP (use EPNP for better accuracy with multiple points)
success, rvec, tvec = cv2.solvePnP(
    object_points[:len(normalized_points)],  # Match the number of valid points
    normalized_points,
    K,
    dist_coeffs,
    flags=cv2.SOLVEPNP_EPNP
)

if success:
    # Convert rotation vector to rotation matrix
    rotation_matrix, _ = cv2.Rodrigues(rvec)
    print("✅ Pose estimation successful!")
    print("\nRotation Matrix:")
    print(np.round(rotation_matrix, 4))
    print("\nTranslation Vector (camera position relative to world origin):")
    print(np.round(tvec, 4))
    
    # Optional: Validate results by projecting 3D points back to normalized coordinates
    projected_points, _ = cv2.projectPoints(object_points[:len(normalized_points)], rvec, tvec, K, dist_coeffs)
    print("\n📊 Projection Error (per point):")
    for idx, (orig, proj) in enumerate(zip(normalized_points, projected_points)):
        error = np.linalg.norm(orig - proj.flatten())
        print(f"Point {idx+1}: {np.round(error, 6)}")
else:
    print("❌ Failed to estimate pose - check your correspondences or try more points!")

• Consistent Coordinate Systems: Make sure your 3D world points and camera coordinate system align (camera +z axis is forward, +x is right, +y is down). If not, adjust the conversion formula's sign or rotate your world points.
• Outlier Rejection: If you have noisy correspondences, use cv2.solvePnPRansac instead of solvePnP—it automatically filters outliers and gives a more robust result.
• Camera Distortion: If your 360 camera has measurable distortion, calibrate it first to get distortion coefficients, then pass them to solvePnP instead of using zeros.
• Avoid Edge Pixels: As mentioned earlier, pixels near the left/right edges of the equirectangular image can cause issues—stick to points in the central region when possible.

内容的提问来源于stack exchange，提问作者user972014