solve camera parameters

이미지에 한점이 사영되는 과정

월드좌표계에서 카메라 좌표계로 

\( t_{x} = o_{x}^{c} - o_{x}^{w} \)

\( X^{c} = R(X^{w} - t_{x}) \)

원근 변환

\( X^{p} = X^{c}/Z^{c} \)

이미지플랜으로 사영

\( x = fX^{p} + c_{x} \)

\( y = fY^{p} + c_{y} \)

 

Intrinsic parameters, focal length 와 center of image를 찾는 방법

카메라 코디네이트와 월드 코디네이트가 일치 하고

3D point와 project된 2D point를 알고있을때

\( x:X = f:Z \\ Xf = xZ \\ x = \frac{f}{Z}X + c_{x} \\ xZ = fX + c_{x}Z \)

# 2D point는 n by 2, 3D point는 n by 3 으로 reshape 한다.
pose2d = points2d.reshape(-1, 2)
pose3d = points3d.reshape(-1, 3)

# 3D point에서 x와 z요소를 뽑아 n by 2 행렬로 스택한다.
x3d = np.stack([pose3d[:, 0], pose3d[:, 2]], axis=-1)
# 2D point x에 3D point z요소를 곱한다. n by 1
x2d = (pose2d[:, 0] * pose3d[:, 2])

# x3d * b = x2d 의 수식을 푼다. 
# alpha_x*X + x_0*Z = x*Z <-> alpha_x*X/Z + x_0 = x
# alpha_x: focal length, x_0: center of image plane
# lstsq: least square, recon: singular values are treated as zero
alpha_x, x_0 = list(np.linalg.lstsq(x3d, x2d, rcond=-1)[0].flatten())

y3d = np.stack([pose3d[:, 1], pose3d[:, 2]], axis=-1)
y2d = (pose2d[:, 1] * pose3d[:, 2])
alpha_y, y_0 = list(np.linalg.lstsq(y3d, y2d, rcond=-1)[0].flatten())

return np.array([alpha_x, x_0, alpha_y, y_0])