python 已知点云坐标xyz, 及像素坐标 uv, 求相机内参K

任务:  对关键点像素坐标uv及点云坐标xyz进行位置变换后(典型如仿射变换),  相机内参也会随之发生变化:

x *  fx + cx = u

y *  fy + cy = v

在已知多组 关键点点云坐标xyz 及 像素坐标uv, 采用最小二乘法 求 fx, cx, fy cy

import numpy as np
def get_K(xyz, uv):
    '''
    Compute K (camera instrinics) by using given xyz and uv.

    Args:
        xyz: point cloud coordinates, shape (n, 3)
        uv: pixel coordinates shape (n, 2)

    Returns: camera instrinics, shape (3, 3)

    '''

    assert xyz.ndim == 2 and uv.ndim == 2
    assert xyz.shape[0] == uv.shape[0]
    assert xyz.shape[1] == 3 and uv.shape[1] ==2
    xy = xyz[:, :2] / xyz[:, 2:]
    I = np.ones((xyz.shape[0], 1))
    x = np.hstack((xy[:, 0].reshape(-1, 1), I))
    y = np.hstack((xy[:, 1].reshape(-1, 1), I))
    u = np.hstack((uv[:, 0].reshape(-1, 1), I))
    v = np.hstack((uv[:, 1].reshape(-1, 1), I))
    # use least square
    fx, cx = np.linalg.inv(x.T.dot(x)).dot(x.T).dot(u)[:, 0]
    fy, cy = np.linalg.inv(y.T.dot(y)).dot(y.T).dot(v)[:, 0]
    K = np.float32([[fx, 0, cx],
                    [0, fy, cy],
                    [0,  0,  1]])
    return K

if __name__ == '__main__':
    
    
    np.random.seed(0)
    '''
    注意! 造数据应确保xyz是通过uv, K, depth求出的, 即xyz 与 uv有对应关系, 不能瞎造
    错误造数据形如 :
            uv = np.random.randint(0, 640, (20, 2))
            xyz = np.random.rand(20, 3)
            K = get_K(xyz, uv)
    即 xyz也是造的, 则反求出的 K一定错的.
    '''
    
    # 随便造一些数据
    # 造 K
    K = np.array([[355, 0, 315],
                  [0, 355, 254],
                  [0, 0, 1]])

    # 造 uv
    uv = np.random.randint(0, 640, (20, 2))
    # 造 与uv 对应的深度depth (depth一般由深度相机获得)
    depth = np.random.rand(20, 1)
    # 通过 uv, K, depth 求 xyz
    u = uv[:, 0]
    v = uv[:, 1]
    fx,fy=K[0,0],K[1,1]
    cx,cy=K[0,2],K[1,2]
    x = (u - cx) / fx 
    y = (v - cy) / fy  
    xyz = np.hstack((x.reshape(-1, 1) * depth, y.reshape(-1, 1) * depth, depth))
    '''
    至于位置变换啥的就自己写吧, 注意uv和xyz要一起变换
    '''
    
    # 验证 K, 看看反求的 K_val 是否能和 K对上    
    K_val = get_K(xyz, uv)