-
Notifications
You must be signed in to change notification settings - Fork 11
/
nonlinear_methods.py
156 lines (109 loc) · 4.88 KB
/
nonlinear_methods.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
import numpy as np
import cv2
import matplotlib.pyplot as plt
from scipy.optimize import least_squares
def get_nonlinear_triangulated_points(points_3D, pose, point_list1, point_list2):
P = np.eye(3,4)
P_dash = pose
tot_points = len(points_3D)
approx_points = []
for p1, p2, point_3D in zip(point_list1, point_list2, points_3D):
x, y, z = point_3D
est_point = [x, y, z]
point_3D_approx = least_squares(nlt_error, est_point, args = (P, P_dash, point_list1, point_list2), max_nfev = 10000)
approx_points.append(point_3D_approx)
return np.array(approx_points)
def nlt_error(point_3D, P1, P2, points_left, points_right):
X, Y, Z = point_3D
point_3D = np.array([X, Y, Z, 1])
p1 = P1 @ point_3D
p2 = P2 @ point_3D
projected_x1, projected_y1 = p1[0]/p1[2], p1[1]/p1[2]
projected_x2, projected_y2 = p2[0]/p2[2], p2[1]/p2[2]
dist1 = (points_left[0] - projected_x1)**2 + (points_left[1] - projected_y1)**2
dist2 = (points_right[0] - projected_x2)**2 + (points_right[1] - projected_y2)**2
error = dist1 + dist2
return error
def get_approx_pose_by_non_linear_pnp(points_3D, pose, point_list1, point_list2):
pose = np.reshape(pose, (-1,1))
pose_est = [elem for elem in pose]
approx_pose = least_squares(non_linear_pnp_error, pose_est, args = (points_3D, point_list1, point_list2), max_nfev = 100000)
return np.reshape(approx_pose, (3,4))
def non_linear_pnp_error(pose, points_3D, points_left, points_right):
P1 = np.eye(3,4)
P2 = np.reshape(pose, (3,4))
## we can have R in the form of R(q), i.e quaternions, to enforce orthogonality for better results
X, Y, Z = point_3D
point_3D = np.array([X, Y, Z, 1])
p1 = P1 @ point_3D
p2 = P2 @ point_3D
projected_x1, projected_y1 = p1[0]/p1[2], p1[1]/p1[2]
projected_x2, projected_y2 = p2[0]/p2[2], p2[1]/p2[2]
dist1 = (points_left[0] - projected_x1)**2 + (points_left[1] - projected_y1)**2
dist2 = (points_right[0] - projected_x2)**2 + (points_right[1] - projected_y2)**2
error = dist1 + dist2
return error
def get_correct_pose(poses, point_list1, point_list2, non_linear_flag):
max_num_positive_depths = 0
correct_pose = None
for pose in poses:
num_positive_depths, better_approx_pose = get_num_positive_depths(pose, point_list1, point_list2, non_linear_flag)
if num_positive_depths > max_num_positive_depths:
max_num_positive_depths = num_positive_depths
correct_pose = better_approx_pose
return correct_pose
def get_num_positive_depths(pose, point_list1, point_list2, non_linear_flag):
rot3 = pose[ROW3, :COL4]
C = pose[:, COL4]
num_positive_depths = 0
points_3D = get_linear_triangulated_points(pose, point_list1, point_list2) # N x 3
## Non linear triangulation can be done here to get better approximation of the 3D point
## points_3D = get_nonlinear_triangulated_points(points_3D, pose, point_list1, point_list2)
## We can then have a better approximation of the pose to get R and t
## better_approx_pose = get_approx_pose_by_non_linear_pnp(points_3D, pose, point_list1, point_list2)
better_approx_pose = pose
if non_linear_flag:
points_3D = get_nonlinear_triangulated_points(points_3D, pose, point_list1, point_list2)
better_approx_pose = get_approx_pose_by_non_linear_pnp(np.array(points_3D), pose, point_list1, point_list2)
rot3 = better_approx_pose[ROW3, :COL4]
C = better_approx_pose[:, COL4]
for point in points_3D:
# cheirality condition
if (rot3 @ (point - C)) > 0:
num_positive_depths += 1
return num_positive_depths, better_approx_pose
def get_linear_triangulated_points(pose, point_list1, point_list2):
P = np.eye(3,4)
P_dash = pose
points_3D = []
num_points = len(point_list1)
for i in range(num_points):
point1 = point_list1[i]
point2 = point_list2[i]
'''
A = np.array([
(point1[Y] * P[ROW3]) - P[ROW2],
P[ROW1] - (point1[X]*P[ROW3]),
(point2[Y] * P_dash[ROW3]) - P_dash[ROW2],
P_dash[ROW1] - (point2[X] * P_dash[ROW3])
])
'''
point1_cross = np.array([
[0, -point1[Z], point1[Y]],
[point1[Z], 0, -point1[X]],
[-point1[Y], point1[X], 0]
])
point2_cross = np.array([
[0, -point2[Z], point2[Y]],
[point2[Z], 0, -point2[X]],
[-point2[Y], point2[X], 0]
])
point1_cross_P = point1_cross @ P
point2_cross_P_dash = point2_cross @ P_dash
A = np.vstack((point1_cross_P, point2_cross_P_dash))
_, _, VT = np.linalg.svd(A)
solution = VT.T[:, -1]
solution /= solution[-1]
points_3D.append([solution[X], solution[Y], solution[Z]])
#yield [solution[X], solution[Y], solution[Z]]
return points_3D