29 lines
601 B
Python
29 lines
601 B
Python
import numpy as np
|
|
|
|
from quaternion import qrot, qinverse
|
|
|
|
|
|
def normalize_screen_coordinates(X, w, h):
|
|
assert X.shape[-1] == 2
|
|
# Normalize so that [0, w] is mapped to [-1, 1], while preserving the aspect ratio
|
|
return X / w * 2 - [1, h / w]
|
|
|
|
|
|
def image_coordinates(X, w, h):
|
|
assert X.shape[-1] == 2
|
|
# Reverse camera frame normalization
|
|
return (X + [1, h / w]) * w / 2
|
|
|
|
|
|
def world_to_camera(X, R, t):
|
|
Rt = qinverse(R)
|
|
Q = np.tile(Rt, (*X.shape[:-1], 1))
|
|
V = X - t
|
|
return qrot(Q, V)
|
|
|
|
|
|
def camera_to_world(X, R, t):
|
|
Q = np.tile(R, (*X.shape[:-1], 1))
|
|
V = X
|
|
return qrot(Q, V) + t
|