-
Notifications
You must be signed in to change notification settings - Fork 1
/
3d_reconstruction_from_two_view_sim.py
172 lines (139 loc) · 5.01 KB
/
3d_reconstruction_from_two_view_sim.py
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
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
import numpy as np
from lib.camera import Camera
from lib.epipolar_geometry import (
calc_camera_matrix,
calc_epipole,
calc_fixed_focal_length,
calc_free_focal_length,
calc_motion_parameters,
reconstruct_3d_points,
detect_mirror_image,
)
from lib.fundamental_matrix import (
calc_fundamental_matrix_8points_method,
calc_fundamental_matrix_extended_fns_method,
calc_fundamental_matrix_taubin_method,
find_fundamental_matrix_cv,
optimize_corresponding_points,
remove_outliers,
)
from lib.visualization import ThreeDimensionalPlotter, TwoDimensionalMatrixPlotter
def set_points():
points = []
for x in np.linspace(-1, 1, 10):
for theta in np.linspace(-np.pi / 2, np.pi / 2, 20):
r = 1 / (x + 2)
y, z = r * np.cos(theta), r * np.sin(theta)
points.append((x, y, z + 3))
return np.vstack(points)
def set_points_box():
points = np.array(
[
[1, 1, 1],
[1, 1, -1],
[1, -1, 1],
[1, -1, -1],
[-1, 1, 1],
[-1, 1, -1],
[-1, -1, 1],
[-1, -1, -1],
]
)
return points + np.array([0, 0, 3])
def calc_true_F(R1, t1, R2, t2, f, f_prime, f0):
return (
np.diag((f0, f0, f))
@ np.cross(t2 - t1, (R2 @ R1.T).T, axisc=0)
@ np.diag((f0, f0, f_prime))
)
def main():
np.random.seed(123)
f0 = 1.0
f_ = 1.0
f_prime_ = 1.0
camera1 = Camera.create([1, 1, 0], [0, 0, 3], f_, f0)
camera2 = Camera.create([2, 2, 1.1], [0.5, 0, 3], f_prime_, f0)
# データ点の設定
X = set_points()
# 2次元画像平面へ射影
x1 = camera1.project_points(X, method="perspective")
x2 = camera2.project_points(X, method="perspective")
# ノイズの追加
# x1 += 0.01 * np.random.randn(*x1.shape)
# x2 += 0.01 * np.random.randn(*x2.shape)
# アウトライアの追加
# x1 = np.vstack((x1, 0.5 * np.random.randn(20, 2)))
# x2 = np.vstack((x2, 0.5 * np.random.randn(20, 2)))
R1, t1 = camera1.get_pose()
R2, t2 = camera2.get_pose()
# アウトライアの除去
x1, x2, outliers = remove_outliers(x1, x2, f0, 0.05)
print(f"number of outlier: {outliers.sum()}")
# 基礎行列の計算
F = calc_fundamental_matrix_extended_fns_method(x1, x2, f0)
# F = calc_fundamental_matrix_8points_method(x1, x2, f0, normalize=True, optimal=True)
# F = calc_fundamental_matrix_taubin_method(x1, x2, f0)
# F = find_fundamental_matrix_cv(x1, x2)
# F_ = calc_true_F(R1, t1, R2, t2, f_, f_prime_, f0)
print(f"F=\n{F}")
# print(f"|F_ - F|=\n{np.linalg.norm(F_ - F)}")
# 対応点の最適補正
x1, x2 = optimize_corresponding_points(F, x1, x2, f0)
# エピポールの計算
epipole = calc_epipole(F)[:, :2]
print(f"e1={epipole[0]}, e2={epipole[1]}")
# 焦点距離f, f_primeの計算
f, f_prime = calc_free_focal_length(F, f0, verbose=False)
# f = f_prime = calc_fixed_focal_length(F, f0)
# f, f_prime = f_, f_prime_
print(f"f={f}, f_prime={f_prime}")
# 運動パラメータの計算
R, t = calc_motion_parameters(F, x1, x2, f, f_prime, f0)
# R, t = R2 @ R1.T, unit_vec(t2 - t1)
print(f"R=\n{R}")
print(f"t={t}")
# カメラ行列の取得
P, P_prime = calc_camera_matrix(f, f_prime, R, t, f0)
print(f"P=\n{P}")
print(f"P_prime=\n{P_prime}")
# 3次元復元
X_ = reconstruct_3d_points(x1, x2, P, P_prime, f0)
# 鏡像の場合符号を反転して修正する
if detect_mirror_image(X_):
X_ *= -1
t *= -1
# シーンデータの表示
plotter_3d = ThreeDimensionalPlotter(figsize=(10, 10))
plotter_3d.set_lim()
plotter_3d.plot_points(X)
for i, camera in enumerate([camera1, camera2], start=1):
plotter_3d.plot_basis(*camera.get_pose(), label=f"Camera{i}")
plotter_3d.show()
plotter_3d.close()
# 復元したシーンデータの表示
plotter_3d = ThreeDimensionalPlotter(figsize=(10, 10))
plotter_3d.set_lim()
plotter_3d.plot_points(X_)
plotter_3d.plot_basis(np.eye(3), np.zeros(3), label="Camera1")
plotter_3d.plot_basis(R, t, label="Camera2")
plotter_3d.show()
plotter_3d.close()
# 投影データと復元後の再投影データの表示
cameras_ = []
for R_pred, t_pred, f_pred in [(np.eye(3), np.zeros(3), f), (R, t, f_prime)]:
K_pred = np.diag([f_pred, f_pred, f0])
cameras_.append(Camera(R_pred, t_pred, K_pred))
x_list_ = []
for camera in cameras_:
x = camera.project_points(X_, method="perspective")
x_list_.append(x)
plotter_2d = TwoDimensionalMatrixPlotter(1, 2, (10, 6))
for i, x in enumerate([x1, x2]):
plotter_2d.select(i)
plotter_2d.set_property(f"Camera{i + 1}", (-1, 1), (-1, 1))
plotter_2d.plot_points(x, color="green", label="Projection")
plotter_2d.plot_points(x_list_[i], color="red", label="Reprojection")
plotter_2d.show()
plotter_2d.close()
if __name__ == "__main__":
main()