摄像机校准改变轴的方向

Question

我想通过在相应的3D LIDAR点和2D相机点上使用直接线性变换来求解外在函数。我已经有了内在函数。

问题是，相机后面的点也会重新投影（见下图）。

所以我只限制在摄像机前面的点＃34，即z＆gt;问题是，在使用不同点集的不同试验中，产生的外在矩阵产生不同的轴。有时，约束z> 0给出正确的结果（图像的中心部分），而其他时候我需要z＆lt; 0，我相信是进入相机的z轴。所以问题是，如何限制相机的Z轴伸出相机？

def with_intrinsic(points2d, points3d, intrinsic):
    cam1_K_inverse = np.linalg.inv(intrinsic)

    #direct linear transformation calibration, assumes no intrinsic matrix
    assert points2d.shape[0] >= 3
    assert points3d.shape[0] == points2d.shape[0]

    A = []

    points2d_homo = []
    for u,v in points2d:
        points2d_homo.append([u, v, 1])

    points2d_homo = np.array(points2d_homo).T #columns to be data points


    points2d_inv = np.dot(cam1_K_inverse, points2d_homo).T
    assert points2d_inv.shape == (points2d.shape[0], 3)
    assert points2d_inv[0, 2] == 1

    for idx in range(points2d.shape[0]):
        x3d, y3d, z3d = points3d[idx]
        u, v, _ = points2d_inv[idx]

        A.append([x3d, y3d, z3d, 1, 0, 0, 0, 0, -u * x3d, -u * y3d, -u * z3d, -u])
        A.append([0, 0, 0, 0, x3d, y3d, z3d, 1, -v * x3d, -v * y3d, -v * z3d, -v])

    A = np.array(A)

    U, D, VT = np.linalg.svd(A)
    M = VT.T[:, -1].reshape((3, 4))

    error = get_reprojection_error(points2d, points3d, intrinsic, M)
    logging.debug("error with_intrinsic: %s", error)

    return M

更新：我试图检查是否重新预测了＃34;培训＆＃34;积分将使我z < 0.如果是这样，我做np.dot(R, extrinsic)来围绕轴的1左右旋转关于PI弧度的点。我已经尝试了所有3个轴但仍然没有产生正确的结果。

R1 = np.array([
        [1, 0, 0],
        [0, np.cos(pi), -np.sin(pi)],
        [0, np.sin(pi), np.cos(pi)],
    ])

R2 = np.array([
        [np.cos(pi), 0, np.sin(pi)],
        [0, 1, 0],
        [-np.sin(pi), 0, np.cos(pi)],
    ])

R3 = np.array([
        [np.cos(pi), -np.sin(pi), 0],
        [np.sin(pi), np.cos(pi), 0],
        [0, 0, 1],
])