|
|
import cv2 |
|
|
import math |
|
|
import numpy as np |
|
|
from skimage import transform as trans |
|
|
|
|
|
|
|
|
def transform(data, center, output_size, scale, rotation): |
|
|
scale_ratio = scale |
|
|
rot = float(rotation) * np.pi / 180.0 |
|
|
|
|
|
t1 = trans.SimilarityTransform(scale=scale_ratio) |
|
|
cx = center[0] * scale_ratio |
|
|
cy = center[1] * scale_ratio |
|
|
t2 = trans.SimilarityTransform(translation=(-1 * cx, -1 * cy)) |
|
|
t3 = trans.SimilarityTransform(rotation=rot) |
|
|
t4 = trans.SimilarityTransform(translation=(output_size / 2, |
|
|
output_size / 2)) |
|
|
t = t1 + t2 + t3 + t4 |
|
|
M = t.params[0:2] |
|
|
cropped = cv2.warpAffine(data, |
|
|
M, (output_size, output_size), |
|
|
borderValue=0.0) |
|
|
return cropped, M |
|
|
|
|
|
|
|
|
def trans_points2d(pts, M): |
|
|
new_pts = np.zeros(shape=pts.shape, dtype=np.float32) |
|
|
for i in range(pts.shape[0]): |
|
|
pt = pts[i] |
|
|
new_pt = np.array([pt[0], pt[1], 1.], dtype=np.float32) |
|
|
new_pt = np.dot(M, new_pt) |
|
|
|
|
|
new_pts[i] = new_pt[0:2] |
|
|
|
|
|
return new_pts |
|
|
|
|
|
|
|
|
def trans_points3d(pts, M): |
|
|
scale = np.sqrt(M[0][0] * M[0][0] + M[0][1] * M[0][1]) |
|
|
|
|
|
new_pts = np.zeros(shape=pts.shape, dtype=np.float32) |
|
|
for i in range(pts.shape[0]): |
|
|
pt = pts[i] |
|
|
new_pt = np.array([pt[0], pt[1], 1.], dtype=np.float32) |
|
|
new_pt = np.dot(M, new_pt) |
|
|
|
|
|
new_pts[i][0:2] = new_pt[0:2] |
|
|
new_pts[i][2] = pts[i][2] * scale |
|
|
|
|
|
return new_pts |
|
|
|
|
|
|
|
|
def trans_points(pts, M): |
|
|
if pts.shape[1] == 2: |
|
|
return trans_points2d(pts, M) |
|
|
else: |
|
|
return trans_points3d(pts, M) |
|
|
|
|
|
def estimate_affine_matrix_3d23d(X, Y): |
|
|
''' Using least-squares solution |
|
|
Args: |
|
|
X: [n, 3]. 3d points(fixed) |
|
|
Y: [n, 3]. corresponding 3d points(moving). Y = PX |
|
|
Returns: |
|
|
P_Affine: (3, 4). Affine camera matrix (the third row is [0, 0, 0, 1]). |
|
|
''' |
|
|
X_homo = np.hstack((X, np.ones([X.shape[0],1]))) |
|
|
P = np.linalg.lstsq(X_homo, Y)[0].T |
|
|
return P |
|
|
|
|
|
def P2sRt(P): |
|
|
''' decompositing camera matrix P |
|
|
Args: |
|
|
P: (3, 4). Affine Camera Matrix. |
|
|
Returns: |
|
|
s: scale factor. |
|
|
R: (3, 3). rotation matrix. |
|
|
t: (3,). translation. |
|
|
''' |
|
|
t = P[:, 3] |
|
|
R1 = P[0:1, :3] |
|
|
R2 = P[1:2, :3] |
|
|
s = (np.linalg.norm(R1) + np.linalg.norm(R2))/2.0 |
|
|
r1 = R1/np.linalg.norm(R1) |
|
|
r2 = R2/np.linalg.norm(R2) |
|
|
r3 = np.cross(r1, r2) |
|
|
|
|
|
R = np.concatenate((r1, r2, r3), 0) |
|
|
return s, R, t |
|
|
|
|
|
def matrix2angle(R): |
|
|
''' get three Euler angles from Rotation Matrix |
|
|
Args: |
|
|
R: (3,3). rotation matrix |
|
|
Returns: |
|
|
x: pitch |
|
|
y: yaw |
|
|
z: roll |
|
|
''' |
|
|
sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0]) |
|
|
|
|
|
singular = sy < 1e-6 |
|
|
|
|
|
if not singular : |
|
|
x = math.atan2(R[2,1] , R[2,2]) |
|
|
y = math.atan2(-R[2,0], sy) |
|
|
z = math.atan2(R[1,0], R[0,0]) |
|
|
else : |
|
|
x = math.atan2(-R[1,2], R[1,1]) |
|
|
y = math.atan2(-R[2,0], sy) |
|
|
z = 0 |
|
|
|
|
|
|
|
|
rx, ry, rz = x*180/np.pi, y*180/np.pi, z*180/np.pi |
|
|
return rx, ry, rz |
|
|
|
|
|
|
