I want to solve for the extrinsics by using direct linear transformation on corresponding 3D LIDAR points and 2D camera points. I already have the intrinsics.
Problem is, points behind the camera gets re-projected as well (see picture below).
So I constrain to only points "in front of the camera", i.e z > 0. The problem is, on different trials where different sets of points are used, the produced extrinsic matrix produces differing axes. Sometimes, constraining z > 0 gives the right results (centre part of image), whereas other times I need z < 0. So the question is, how do I constrain the Z axes of the camera to be sticking out of the camera?
def with_intrinsic(points2d, points3d, intrinsic): cam1_K_inverse = np.linalg.inv(intrinsic) #direct linear transformation calibration, assumes no intrinsic matrix assert points2d.shape >= 3 assert points3d.shape == points2d.shape 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, 3) assert points2d_inv[0, 2] == 1 for idx in range(points2d.shape): 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