如何计算平移矩阵？基于相机经纬度的像素转3D坐标技术求助

Hey there! Let's break down how to figure out that translation matrix (or more accurately, translation vector—since we're dealing with rigid transforms) you need. Since you've got camera intrinsics, pose angles (yaw/pitch/roll to get rotation matrices), and geotagged images, we can absolutely work this out using just your dataset.

First, Understand the Core Idea #

The translation vector T we're after is simply the 3D position of your camera's origin in your chosen world coordinate system. Because when converting a point from camera space to world space, you're essentially rotating the camera point into world orientation, then shifting it to the camera's actual location in the world.

The catch? Your camera's geotag is in latitude/longitude—we need to convert that to a 3D Cartesian coordinate first (you'll also need the camera's altitude; more on that below).

Step 1: Convert Camera Lat/Lon/Alt to 3D World Coordinates #

You have two common options for world coordinate systems, depending on your dataset's scale:

Option 1: Local ENU Coordinate System (Best for Small-Scale Scenes) #

ENU (East-North-Up) is a local right-handed system where:

• X = East
• Y = North
• Z = Up (towards the sky)

To convert your camera's (lon, lat, alt) to ENU, pick a reference point (like the center of your dataset) as the ENU origin, then use this conversion (Python example):

import math

def latlonalt_to_enu(cam_lon, cam_lat, cam_alt, ref_lon, ref_lat, ref_alt):
    # Convert degrees to radians
    cam_lon_rad = math.radians(cam_lon)
    cam_lat_rad = math.radians(cam_lat)
    ref_lon_rad = math.radians(ref_lon)
    ref_lat_rad = math.radians(ref_lat)

    # WGS84 Earth ellipsoid parameters
    a = 6378137.0  # Semi-major axis
    f = 1/298.257223563  # Flattening
    e2 = 2*f - f**2  # Eccentricity squared

    # Calculate ECEF coordinates for reference point
    N_ref = a / math.sqrt(1 - e2 * math.sin(ref_lat_rad)**2)
    X_ref = (N_ref + ref_alt) * math.cos(ref_lat_rad) * math.cos(ref_lon_rad)
    Y_ref = (N_ref + ref_alt) * math.cos(ref_lat_rad) * math.sin(ref_lon_rad)
    Z_ref = (N_ref*(1 - e2) + ref_alt) * math.sin(ref_lat_rad)

    # Calculate ECEF coordinates for camera
    N_cam = a / math.sqrt(1 - e2 * math.sin(cam_lat_rad)**2)
    X_cam = (N_cam + cam_alt) * math.cos(cam_lat_rad) * math.cos(cam_lon_rad)
    Y_cam = (N_cam + cam_alt) * math.cos(cam_lat_rad) * math.sin(cam_lon_rad)
    Z_cam = (N_cam*(1 - e2) + cam_alt) * math.sin(cam_lat_rad)

    # Convert ECEF difference to ENU
    dX = X_cam - X_ref
    dY = Y_cam - Y_ref
    dZ = Z_cam - Z_ref

    sin_ref_lat = math.sin(ref_lat_rad)
    cos_ref_lat = math.cos(ref_lat_rad)
    sin_ref_lon = math.sin(ref_lon_rad)
    cos_ref_lon = math.cos(ref_lon_rad)

    east = -sin_ref_lon * dX + cos_ref_lon * dY
    north = -sin_ref_lat * cos_ref_lon * dX - sin_ref_lat * sin_ref_lon * dY + cos_ref_lat * dZ
    up = cos_ref_lat * cos_ref_lon * dX + cos_ref_lat * sin_ref_lon * dY + sin_ref_lat * dZ

    return (east, north, up)

The output (east, north, up) is your camera's origin in ENU space—this is your translation vector T = [east, north, up]^T.

Option 2: Global ECEF Coordinate System (Best for Large-Scale Scenes) #

If your dataset spans a huge area, use ECEF (Earth-Centered, Earth-Fixed), a global Cartesian system centered at the Earth's core. Conversion is simpler:

def latlonalt_to_ecef(lon, lat, alt):
    lon_rad = math.radians(lon)
    lat_rad = math.radians(lat)

    a = 6378137.0
    f = 1/298.257223563
    e2 = 2*f - f**2

    N = a / math.sqrt(1 - e2 * math.sin(lat_rad)**2)
    X = (N + alt) * math.cos(lat_rad) * math.cos(lon_rad)
    Y = (N + alt) * math.cos(lat_rad) * math.sin(lon_rad)
    Z = (N*(1 - e2) + alt) * math.sin(lat_rad)

    return (X, Y, Z)

Here, (X,Y,Z) is your camera's origin in ECEF space—this becomes your translation vector T.

Important: You must have the camera's altitude to get a valid 3D coordinate. If your dataset doesn't include it, you can match your camera's lat/lon to a public DEM (Digital Elevation Model) to pull the ground elevation, then add any camera height above ground if you have that info.

Step 2: Combine with Rotation Matrix for Full Conversion #

Now that you have T, let's tie it all together for pixel-to-world conversion:

1. Pixel to Camera Coordinates: Use your camera intrinsic matrix K to unproject the pixel (u,v) to a normalized camera ray. The formula is:

   [x_c / z_c, y_c / z_c, 1]^T = K^{-1} * [u, v, 1]^T
   

   Here, z_c is the depth of the point in camera space (if you don't have depth data, you'll only get a direction ray, not a specific 3D point—you'll need to use stereo reconstruction or structure-from-motion if depth isn't in your dataset). Once you have depth, you get the full camera coordinate P_c = [x_c, y_c, z_c]^T.

2. Camera to World Coordinates: Use your rotation matrix R and translation vector T to transform the camera point to world space. Just double-check your rotation matrix's direction:

   • If R converts camera coordinates to world coordinates (rotates the camera frame into world orientation), use:

     P_world = R * P_c + T
     

   • If R converts world coordinates to camera coordinates (the inverse), use the transpose of R (since rotation matrices are orthogonal, inverse = transpose):

     P_world = R^T * P_c + T
     

Critical Gotchas to Avoid #

• Rotation Matrix Direction: This is the #1 mistake! Always verify what your yaw/pitch/roll-based rotation matrix actually does. Test with a known point if possible.
• Coordinate System Handedness: Ensure your world and camera systems are consistent (both right-handed, for example). Most camera systems use right-handed (X=right, Y=down, Z=forward) and ENU/ECEF are also right-handed, so this should align.
• Altitude Accuracy: Even small errors in altitude can throw off your 3D positions, so get the most accurate altitude data you can.

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