gha/render_pose.py at master · Vegagotocreat/gha · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
import numpy as np

def rotation(initial_normal, target_normal, center, points ):
    rotation_axis = np.cross(initial_normal, target_normal)
    rotation_axis /= np.linalg.norm(rotation_axis)
    angle = np.arccos(np.dot(initial_normal, target_normal))
    rotation_matrix = np.array([[np.cos(angle) + rotation_axis[0]**2 * (1 - np.cos(angle)),
                             rotation_axis[0] * rotation_axis[1] * (1 - np.cos(angle)) + rotation_axis[2] * np.sin(angle),
                             rotation_axis[0] * rotation_axis[2] * (1 - np.cos(angle)) - rotation_axis[1] * np.sin(angle)],
                            [rotation_axis[1] * rotation_axis[0] * (1 - np.cos(angle)) - rotation_axis[2] * np.sin(angle),
                             np.cos(angle) + rotation_axis[1]**2 * (1 - np.cos(angle)),
                             rotation_axis[1] * rotation_axis[2] * (1 - np.cos(angle)) + rotation_axis[0] * np.sin(angle)],
                            [rotation_axis[2] * rotation_axis[0] * (1 - np.cos(angle)) + rotation_axis[1] * np.sin(angle),
                             rotation_axis[2] * rotation_axis[1] * (1 - np.cos(angle)) - rotation_axis[0] * np.sin(angle),
                             np.cos(angle) + rotation_axis[2]**2 * (1 - np.cos(angle))]])
    rotated_points = np.dot(rotation_matrix, (points - center).T).T + center
    return rotated_points

def lookat(at, eye, up):
    ez = at - eye
    ez = ez / np.linalg.norm(ez)
    ex = np.cross(ez, up)
    ex = ex / np.linalg.norm(ex)
    ey = np.cross(ez, ex)
    ey = ey / np.linalg.norm(ey)
    R_c2w = np.stack([ex, ey, ez],1)
    T_w2c = (eye - at)[:, :, np.newaxis]

    # forward = (at-eye)/np.linalg.norm(at-eye)
    # right = np.cross(up, forward)
    # right = right / np.linalg.norm(right)
    # up_c = np.cross(forward, right)
    # up_c = up_c / np.linalg.norm(up_c)
    # R_c2w = np.column_stack((right, up_c, -forward), -1)
    # T_c2w = -np.dot(R_c2w, eye)
    R_w2c = np.linalg.inv(R_c2w)
    # T_w2c = -np.dot(R_w2c, T_c2w)
    return R_w2c , T_w2c

poses = np.load('/home/A/yangkun/gaussian-splatting-main/w2c_mats_colmap_nvs.npy')
pose_T = poses[:, 0:3, 3]
center = np.mean(pose_T, axis=0)
center[1] -=  0.2
points = []
angles = np.linspace(0, 2*np.pi, 360)
radius = 3.7
for i in range(len(angles)):
    point = [center[0] + radius * np.cos(angles[i]), center[1], center[2] + radius * np.sin(angles[i])]
    points.append(point)
points = np.stack(points, 0)

vector_npara = [0., -1, 0]

centered_points = pose_T - center
U, S, V = np.linalg.svd(centered_points)

up = -V[-1]
# up = np.array([1.83440910e-02, -9.11580134e+00, 3.85821578e+00])
look = rotation(vector_npara, up, center, points)
at1 = center + 2 * up/np.linalg.norm(up)
at = np.array([0.03098101, -1.36628168, 0.91682083])
R_w2c, T_w2c = lookat(at, look, up)
pose_w2c = np.concatenate([R_w2c, T_w2c], -1)
np.save('new_pose_w2c.npy', pose_w2c)