def rectify_fusiello(self, d1=np.zeros(2), d2=np.zeros(2)):
"""
Translation of Andre's IDL function rectify_fusello.
"""
try:
K1, R1, C1, _, _, _, _ = cv2.decomposeProjectionMatrix(self.view1.P)
K2, R2, C2, _, _, _, _ = cv2.decomposeProjectionMatrix(self.view2.P)
except:
return
C1 = cv2.convertPointsFromHomogeneous(C1.T).reshape(3, 1)
C2 = cv2.convertPointsFromHomogeneous(C2.T).reshape(3, 1)
oc1 = mdot(-R1.T, np.linalg.inv(K1), self.view1.P[:, 3])
oc2 = mdot(-R2.T, np.linalg.inv(K2), self.view2.P[:, 3])
v1 = (oc2-oc1).T
v2 = np.cross(R1[2, :], v1)
v3 = np.cross(v1, v2)
R = np.array([v1/np.linalg.norm(v1), v2/np.linalg.norm(v2),
v3/np.linalg.norm(v3)]).reshape(3, 3)
Kn1 = np.copy(K2)
Kn1[0, 1] = 0
Kn2 = np.copy(K2)
Kn2[0, 1] = 0
Kn1[0, 2] = Kn1[0, 2] + d1[0]
Kn1[1, 2] = Kn1[1, 2] + d1[1]
Kn2[0, 2] = Kn2[0, 2] + d2[0]
Kn2[1, 2] = Kn2[1, 2] + d2[1]
t1 = np.matmul(-R, C1)
t2 = np.matmul(-R, C2)
Rt1 = np.concatenate((R, t1), 1)
Rt2 = np.concatenate((R, t2), 1)
Prec1 = np.dot(Kn1, Rt1)
Prec2 = np.dot(Kn2, Rt2)
Tr1 = np.dot(Prec1[:3, :3], np.linalg.inv(self.view1.P[:3, :3]))
Tr2 = np.dot(Prec2[:3, :3], np.linalg.inv(self.view2.P[:3, :3]))
return Prec1, Prec2, Tr1, Tr2
def euler_jacobian(P, H, X, x):
"""
Constructs the Euler Jacobian used in GN_estimation.
"""
n_landmarks = X.shape[0]
J = np.ones((2*n_landmarks, 6))
for i in range(6):
h = mat2vec(H)
h[i] = h[i] + 0.5
H_new = vec2mat(h)
x_new = mdot(P, H_new, X.T)
x_new = np.apply_along_axis(lambda v: v/v[-1], 0, x_new)
J[:, i] = ((x_new - x)/0.5)[:2, :].flatten(order='F')
return J
def iterate_jacobian(self, view_number, H, key_index, db_index,
outlier_threshold=2, iter_number=0):
"""
Function called in main loop of GN_estimation.
"""
if len(db_index):
if view_number == 1:
key_coords = self.view1.key_coords[key_index]
P = self.view1.P
elif view_number == 2:
key_coords = self.view2.key_coords[key_index]
P = self.view2.P
db_landmarks = self.database.landmarks[db_index]
projections = mdot(P, H, db_landmarks.T)
projections = np.apply_along_axis(lambda v: v/v[-1], 0, projections)
J = self.euler_jacobian(P, H, db_landmarks, projections)
squErr = np.sqrt(np.sum(
np.square(projections[:2, :] - key_coords.T), 0))
outliers = detect_outliers(squErr)
if outliers.shape:
key_index= np.delete(key_index, outliers, axis=0)
projections = np.delete(projections, outliers, axis=1)
db_index = np.delete(db_index, outliers, axis=0)
Joutliers = np.array([2*outliers,
(2*outliers + 1)]).flatten()
J = np.delete(J, Joutliers, axis=0)
squErr = np.delete(squErr, outliers)
if iter_number >= 6:
abs_outliers = np.where(squErr > outlier_threshold)[0]
if len(abs_outliers) > 0:
key_index = np.delete(key_index, abs_outliers, axis=0)
projections = np.delete(projections, abs_outliers, axis=1)
db_index = np.delete(db_index, abs_outliers, axis=0)
Joutliers = np.array([2*abs_outliers,
(2*abs_outliers + 1)]).flatten()
J = np.delete(J, Joutliers, axis=0)
else:
J = np.array([])
projections = np.array([])
key_index = np.array([], dtype=int)
return J, projections, key_index, db_index
def estimate_pose_gn(self, X, frameDescriptors, in1, in2, n_iterations=10,
abs_pix_thresh=2):
"""
Matches keypoints from view1 and view2 to database indepedently. If
there are matches, calls GN_estimation() to calculate pose. Retuns time
taken to match keypoints with database, time taken to estimate pose,
and number of keypoints in view1 and view2 used to calculte pose.
"""
flag = 0
matchDBStart = time.perf_counter()
# if self.view1.descriptors and len(self.database):
matches_view1db = self.match_descriptors(self.database.descriptors,
self.view1.descriptors,
matching_type='database')
# else:
# matches_view1db
# if self.view2.descriptors and len(self.database):
matches_view2db = self.match_descriptors(self.database.descriptors,
self.view2.descriptors,
matching_type='database')
# frameIdx_raw and dbIdx_raw contain duplicate/unreliable matches. We
# retain these indices as we add the landmarks with indices
# complementary to these raw indices to the database. For pose
# estimation however, we use frameIdx and dbIdx, which don't contain
# duplicates.
dbIdx1_raw, frameIdx1_raw = self.extract_match_indices(matches_view1db)
dbIdx2_raw, frameIdx2_raw = self.extract_match_indices(matches_view2db)
self.matches_db_view1 = matches_view1db
self.matches_db_view2 = matches_view2db
matches_view1db = self.remove_duplicate_matches(matches_view1db)
matches_view2db = self.remove_duplicate_matches(matches_view2db)
dbIdx1, frameIdx1 = self.extract_match_indices(matches_view1db)
dbIdx2, frameIdx2 = self.extract_match_indices(matches_view2db)
print('Number of db matches view1:', len(frameIdx1))
print('Number of db matches view2:', len(frameIdx2))
matchDBTime = time.perf_counter() - matchDBStart
# Estimate pose
if (len(frameIdx1) and len(frameIdx2)): # Landmarks seen in both views
poseEstTime, key_index1, key_index2, \
used_landmarks1, used_landmarks2, flag = \
self.GN_estimation(
frameIdx1,
frameIdx2,
dbIdx1,
dbIdx2,
n_iterations,
abs_pix_thresh
)
elif len(frameIdx1): # Landmarks seen in view 1 only
poseEstTime, key_index1, key_index2, \
used_landmarks1, used_landmarks2, flag = \
self.GN_estimation(
frameIdx1,
np.array([], dtype=int),
dbIdx1,
np.array([], dtype=int),
n_iterations,
abs_pix_thresh
)
elif len(frameIdx2): # Landmarks seen in view 2 only
poseEstTime, key_index1, key_index2, \
used_landmarks1, used_landmarks2, flag = \
self.GN_estimation(
np.array([], dtype=int),
frameIdx2,
np.array([], dtype=int),
dbIdx2,
n_iterations,
abs_pix_thresh
)
else:
print('No matches with database, returning previous pose.\n')
used_landmarks1 = np.array([], dtype=int)
used_landmarks2 = np.array([], dtype=int)
poseEstTime = 0
flag = 1
return matchDBTime, poseEstTime, used_landmarks1, used_landmarks2, \
flag
H = vec2mat(self.currentPose)
# Add new entries to database
new_landmarks = []
old_landmarks = []
self.used_key_indices_1 = key_index1
self.used_key_indices_2 = key_index2
if self.update_db_cutoff is None or self.frameNumber < self.update_db_cutoff:
if len(in1) and flag == 0:
for i in range(len(in1)):
if (in1[i] not in frameIdx1_raw) and (in2[i] not in frameIdx2_raw):
new_landmarks.append(i)
# elif (in1[i] in key_index1) and (in2[i] in key_index2):
# db_update_index1 = used_landmarks1[np.where(key_index1 == in1[i])[0]]
# db_update_index2 = used_landmarks2[np.where(key_index2 == in2[i])[0]]
# if db_update_index1 == db_update_index2:
# db_update_index = db_update_index1
# self.database.landmarks[db_update_index] = np.dot(np.linalg.inv(H), X[i, :].T).T
# self.database.descriptors[db_update_index] = frameDescriptors[i, :]
X_new = X[new_landmarks, :]
descriptors_new = frameDescriptors[new_landmarks, :]
X_new = mdot(np.linalg.inv(H), X_new.T).T
self.database.update(X_new, descriptors_new)
return matchDBTime, poseEstTime, used_landmarks1, used_landmarks2, \
flag
def estimate_pose_ls(self, X, frameDescriptors):
"""
Estimate pose using Horn's method. Returns time taken to match
descriptors with database, time taken to estimate pose, and number of
3D landmarks used to estimate pose.
"""
flag = 0
# Match 3D points found in current frame with database
matchDBStart = time.perf_counter()
matches_db = self.match_descriptors(self.database.descriptors,
frameDescriptors,
matching_type='database')
# frameIdx_raw and dbIdx_raw contain duplicate/unreliable matches. We
# retain these indices as we add the landmarks with indices
# complementary to these raw indices to the database. For pose
# estimation however, we use frameIdx and dbIdx, which don't contain
# duplicates.
dbIdx_raw, frameIdx_raw = self.extract_match_indices(matches_db)
self.matches_db_view1 = matches_db
self.matches_db_view2 = matches_db
matches_db = self.remove_duplicate_matches(matches_db)
dbIdx, frameIdx = self.extract_match_indices(matches_db)
matchDBTime = time.perf_counter() - matchDBStart
poseEstStart = time.perf_counter()
if (len(frameIdx) >= 3 and len(dbIdx) >= 3):
XMatched = X[frameIdx]
frameDescriptorsMatched = frameDescriptors[frameIdx]
landmarksMatched = self.database.landmarks[dbIdx]
landmarkDescriptorsMatched = self.database.descriptors[dbIdx]
H = self.hornmm(XMatched, landmarksMatched)
# Outlier removal, recalculate pose
squErr = np.sqrt(np.sum(np.square(
(XMatched.T - np.dot(H, landmarksMatched.T))), 0))
# Absolute outlier rejection
outliers = np.where(squErr > 3)[0]
# Relative outlier rejection
# outliers = detect_outliers(squErr)
XMatched = np.delete(XMatched, outliers, axis=0)
db_index_used = np.delete(dbIdx, outliers, axis=0)
landmarksMatched = np.delete(landmarksMatched, outliers, axis=0)
if (len(XMatched) >= 3 and len(landmarksMatched >= 3)):
H = self.hornmm(XMatched, landmarksMatched)
pose_change = np.abs(mat2vec(H) - self.currentPose)
# Reject new pose if change from previous pose is too high
if (pose_change > self.pose_threshold).any():
print('Pose change larger than threshold, returning' +
' previous pose')
db_index_used = np.array([], dtype=int)
flag = 1
else:
# self.database.trim(dbIdx[outliers])
self.currentPose = mat2vec(H)
# Add new landmarks to database
landmarksNew = np.delete(X, frameIdx_raw, axis=0)
landmarkDescriptorsNew = np.delete(frameDescriptors, frameIdx_raw,
axis=0)
# Transform new landmark positions to original pose
landmarksNew = mdot(np.linalg.inv(H), landmarksNew.T).T
self.database.update(landmarksNew, landmarkDescriptorsNew)
usedKeypoints = len(XMatched)
print('USED KEYPOINTS PARAM:', usedKeypoints)
print('DB INDEX USED:', len(db_index_used))
else:
print('Not enough matches with database, returning previous pose\n')
db_index_used = np.array([], dtype=int)
flag = 1
else:
print('Not enough matches with database, returning previous pose\n')
db_index_used = np.array([], dtype=int)
flag = 1
poseEstTime = time.perf_counter() - poseEstStart
return matchDBTime, poseEstTime, db_index_used, flag