基于OpenCV检测的矩形四点计算俯仰角、横滚角和偏航角

Alright, let's break down how to calculate pitch, roll, and yaw from those four detected corners. First off, this problem boils down to estimating the camera's extrinsic parameters (its orientation relative to the real-world rectangle) using 2D image points and their corresponding 3D world points. Here's a step-by-step guide tailored to your scenario:

Before you start, you need three critical pieces of information:

• Real-world dimensions of the rectangle: You can't recover scale (and thus accurate angles) from 2D points alone. For example, if your rectangle is 40cm wide and 30cm tall, note those values.
• Camera intrinsic parameters: This includes the camera matrix K and distortion coefficients dist. You'll need to calibrate your camera first to get these (OpenCV has built-in tools for this).
• Correct correspondence between 2D and 3D points: You must know which detected 2D corner maps to which real-world 3D corner of the rectangle.

1. Define 3D World Points #

First, set up a world coordinate system. A common choice is to place the rectangle flat on the XY-plane (Z=0 for all corners). Let's say your rectangle has width W and height H. Assign 3D coordinates to each corner—just make sure the order matches your detected 2D points exactly.

Based on your coordinates, let's assume the 2D points are ordered as top-right, top-left, bottom-left, bottom-right (since (308,25) sits high on the right, (38,99) high on the left, (147,477) low on the left, (412,466) low on the right). Your 3D points could look like this:

# Units match your real-world dimensions (e.g., cm)
world_points = np.array([
    [W, 0, 0],   # Top-right
    [0, 0, 0],   # Top-left
    [0, H, 0],   # Bottom-left
    [W, H, 0]    # Bottom-right
], dtype=np.float32)

2. Use the PnP Algorithm #

The Perspective-n-Point (PnP) algorithm is the standard way to solve for camera pose when you have 3D-2D point correspondences. OpenCV provides two reliable implementations:

• cv2.solvePnP: Basic version, works great if your corner detections are clean.
• cv2.solvePnPRansac: Robust version that filters out outliers (ideal if your approxPolyDP detection has minor errors).

You'll pass in your 3D world points, 2D image points, camera matrix K, and distortion coefficients dist. The function returns a rotation vector (rvec) and translation vector (tvec)—the rotation vector encodes the camera's orientation.

3. Convert Rotation Vector to Euler Angles #

The rotation vector from PnP is a compact representation of rotation. To get pitch, roll, and yaw, you first convert it to a 3x3 rotation matrix using cv2.Rodrigues(), then extract the Euler angles from the matrix.

Important: Euler angles depend on the rotation order. The most common order for camera poses is Z-Y-X (yaw → pitch → roll), where:

• Yaw: Rotation around the Z-axis (left/right turn of the camera)
• Pitch: Rotation around the Y-axis (up/down tilt of the camera)
• Roll: Rotation around the X-axis (side-to-side tilt of the camera)

Here's a concrete code snippet using your detected points:

import cv2
import numpy as np

# Your detected 2D image points (order must match world_points!)
image_points = np.array([
    [308, 25],
    [38, 99],
    [147, 477],
    [412, 466]
], dtype=np.float32)

# Example real-world dimensions (replace with your actual rectangle size)
W = 40.0  # Width of rectangle (e.g., cm)
H = 30.0  # Height of rectangle (e.g., cm)

# Example camera intrinsic parameters (replace with your calibrated values!)
# Camera matrix K: [fx, 0, cx; 0, fy, cy; 0, 0, 1]
K = np.array([
    [800, 0, 320],
    [0, 800, 240],
    [0, 0, 1]
], dtype=np.float32)

# Distortion coefficients (set to zeros if you haven't calibrated yet)
dist = np.zeros((5,1), dtype=np.float32)

# Define 3D world points (match order of image_points)
world_points = np.array([
    [W, 0, 0],
    [0, 0, 0],
    [0, H, 0],
    [W, H, 0]
], dtype=np.float32)

# Solve PnP with RANSAC for robustness
success, rvec, tvec = cv2.solvePnPRansac(world_points, image_points, K, dist)

# Convert rotation vector to rotation matrix
R, _ = cv2.Rodrigues(rvec)

# Extract Euler angles (Z-Y-X order: Yaw → Pitch → Roll)
yaw = np.arctan2(R[1,0], R[0,0])
pitch = np.arctan2(-R[2,0], np.sqrt(R[2,1]**2 + R[2,2]**2))
roll = np.arctan2(R[2,1], R[2,2])

# Convert radians to degrees for readability
yaw_deg = np.degrees(yaw)
pitch_deg = np.degrees(pitch)
roll_deg = np.degrees(roll)

print(f"Yaw: {yaw_deg:.2f}°")
print(f"Pitch: {pitch_deg:.2f}°")
print(f"Roll: {roll_deg:.2f}°")

• Point correspondence is make-or-break: If you mix up the order of 2D/3D points, your angles will be completely wrong. Double-check which image corner maps to which real-world corner (plotting the points can help visualize this).
• Camera calibration: Without accurate intrinsic parameters, your angle estimates will have large errors. Spend time calibrating your camera properly using a chessboard or calibration target.
• Scale doesn't affect angles: While real-world dimensions are required for PnP, the actual units (cm vs meters) won't change the calculated angles—they only affect the translation vector tvec.
• Outlier handling: If your approxPolyDP detection has noisy points, solvePnPRansac will filter them out automatically, leading to more stable results.

To avoid confusion, here's how the angles map to camera motion with Z-Y-X order:

• Yaw (+ve): Camera turns to its right
• Pitch (+ve): Camera tilts upward
• Roll (+ve): Camera tilts to its right (clockwise from the camera's perspective)

If you need a different rotation order (e.g., X-Y-Z), you'll have to adjust the Euler angle extraction code accordingly.

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