def triangulatePoints(self, pose_1w, pose_2w, kpn_1, kpn_2):
# P1w = np.dot(K1, M1w) # K1*[R1w, t1w]
# P2w = np.dot(K2, M2w) # K2*[R2w, t2w]
# since we are working with normalized coordinates x_hat = Kinv*x, one has
P1w = pose_1w[:3, :] # [R1w, t1w]
P2w = pose_2w[:3, :] # [R2w, t2w]
point_4d_hom = cv2.triangulatePoints(P1w, P2w, kpn_1.T, kpn_2.T)
point_4d = point_4d_hom / point_4d_hom[3]
if False:
point_reproj = P1w @ point_4d
point_reproj = point_reproj / point_reproj[2] - add_ones(kpn_1).T
err = np.sum(point_reproj**2)
print('reproj err: ', err)
#pts_3d = point_4d[:3, :].T
return point_4d.T
def homogeneous(self):
return add_ones(self.pt)
def add_points(self,
points4d,
mask_pts4d,
f1,
f2,
idx1,
idx2,
img1,
check_parallax=True):
assert (points4d.shape[0] == len(idx1))
new_pts_count = 0
out_mask_pts4d = np.full(points4d.shape[0], False, dtype=bool)
if mask_pts4d is None:
mask_pts4d = np.full(points4d.shape[0], True, dtype=bool)
for i, p in enumerate(points4d):
if not mask_pts4d[i]:
#print('p[%d] not good' % i)
continue
# check parallax is large enough (this is going to filter out all points when the inter-frame motion is almost zero)
if check_parallax is True:
Rwc1 = f1.pose[:3, :3].T
Rwc2 = f2.pose[:3, :3].T
r1 = np.dot(Rwc1, add_ones(f1.kpsn[idx1[i]]))
r2 = np.dot(Rwc2, add_ones(f2.kpsn[idx2[i]]))
cos_parallax = r1.dot(r2) / (np.linalg.norm(r1) *
np.linalg.norm(r2))
if cos_parallax > parameters.kCosMinParallax:
# print('p[',i,']: ',p,' not enough parallax: ', cos_parallax)
continue
# check points are in front of both cameras
x1 = np.dot(f1.pose, p)
x1 = x1 / x1[3]
x2 = np.dot(f2.pose, p)
x2 = x2 / x2[3]
if x1[2] < 0 or x2[2] < 0:
#print('p[', i, ']: ', p, ' not visible')
continue
# project on camera pixels
uv1 = np.dot(f1.K, x1[:3])
uv2 = np.dot(f2.K, x2[:3])
# check reprojection error
invSigma21 = Frame.detector.inv_level_sigmas2[f1.octaves[idx1[i]]]
err1 = (uv1[0:2] / uv1[2]) - f1.kpsu[idx1[i]]
err1 = np.linalg.norm(err1) * invSigma21
invSigma22 = Frame.detector.inv_level_sigmas2[f2.octaves[idx2[i]]]
err2 = (uv2[0:2] / uv2[2]) - f2.kpsu[idx2[i]]
err2 = np.linalg.norm(err2) * invSigma22
if err1 > parameters.kChi2Mono or err2 > parameters.kChi2Mono: # chi-square 2 DOFs (Hartley Zisserman pg 119)
#print('p[%d] big reproj err1 %f, err2 %f' % (i,err1,err2))
continue
# add the point to this map
try:
color = img1[int(round(f1.kps[idx1[i], 1])),
int(round(f1.kps[idx1[i], 0]))]
except IndexError:
color = (255, 0, 0)
pt = MapPoint(self, p[0:3], color)
pt.add_observation(f2, idx2[i])
pt.add_observation(f1, idx1[i])
new_pts_count += 1
out_mask_pts4d[i] = True
return new_pts_count, out_mask_pts4d
def search_frame_for_triangulation(f1, f2, img2, img1 = None):
idxs2_out = []
idxs1_out = []
lines_out = []
num_found_matches = 0
img2_epi = None
if __debug__:
timer = Timer()
timer.start()
f1.update_camera_pose()
f2.update_camera_pose()
O1w = f1.Ow
O2w = f2.Ow
# compute epipoles
e1 = f1.project_point(O2w).ravel() # in first frame
e2 = f2.project_point(O1w).ravel() # in second frame
#print('e1: ', e1)
#print('e2: ', e2)
# if the translation is too small we cannot triangulate
if np.linalg.norm(O1w-O2w) < parameters.kMinTraslation: # we assume the Inializer has been used for building the first map
Printer.red("search for triangulation: impossible with zero translation!")
return idxs1_out, idxs2_out, num_found_matches, img2_epi # EXIT
# compute the fundamental matrix between the two frames
F12, H21 = computeF12(f1, f2)
idxs1 = []
for i, p in enumerate(f1.points):
if p is None: # we consider just unmatched keypoints
kp = f1.kpsu[i]
scale_factor = Frame.detector.scale_factors[f1.octaves[i]]
# discard points which are too close to the epipole
if np.linalg.norm(kp-e1) < parameters.kMinDistanceFromEpipole * scale_factor:
continue
idxs1.append(i)
kpsu1 = f1.kpsu[idxs1]
# compute epipolar lines in second image
lines2 = cv2.computeCorrespondEpilines(kpsu1.reshape(-1,1,2), 2, F12)
lines2 = lines2.reshape(-1,3)
xs2_inf = np.dot(H21, add_ones(kpsu1).T).T # x2inf = H21 * x1 where x2inf is corresponding point according to infinite homography [Hartley Zisserman pag 339]
xs2_inf = xs2_inf[:, 0:2] / xs2_inf[:, 2:]
line_edges = [ epiline_to_end_points(line, e2, x2_inf, f2.W) for line, x2_inf in zip(lines2, xs2_inf) ]
#print("line_edges: ", line_edges)
if __debug__:
print('search_frame_for_triangulation - timer1: ', timer.elapsed())
assert(len(line_edges) == len(idxs1))
timer.start()
len_des2 = len(f2.des)
flag_match = np.full(len_des2, False, dtype=bool)
dist_match = np.zeros(len_des2)
index_match = np.full(len_des2, 0, dtype=int)
for i, idx in enumerate(idxs1): # N.B.: a point in f1 can be matched to more than one point in f2, we avoid this by caching matches with f2 points
f2_idx, dist = find_matches_along_line(f2, e2, line_edges[i], f1.des[idx])
if f2_idx > -1:
if not flag_match[f2_idx]: # new match
flag_match[f2_idx] = True
dist_match[f2_idx] = dist
idxs2_out.append(f2_idx)
idxs1_out.append(idx)
index_match[f2_idx] = len(idxs2_out)-1
assert(f2.points[f2_idx] is None)
assert(f1.points[idx] is None)
if __debug__:
lines_out.append(line_edges[i])
else: # already matched
if dist < dist_match[f2_idx]: # update in case of a smaller distance
dist_match[f2_idx] = dist
index = index_match[f2_idx]
idxs1_out[index] = idx
if __debug__:
lines_out[index] = line_edges[i]
num_found_matches = len(idxs1_out)
assert(len(idxs1_out) == len(idxs2_out))
if __debug__:
#print("num found matches: ", num_found_matches)
if True:
kpsu2 = f2.kpsu[idxs2_out]
img2_epi = draw_lines(img2.copy(), lines_out, kpsu2)
#cv2.imshow("epipolar lines",img2_epi)
#cv2.waitKey(0)
if __debug__:
print('search_frame_for_triangulation - timer2: ', timer.elapsed())
return idxs1_out, idxs2_out, num_found_matches, img2_epi
def unprojectPoints(self, uvs):
return np.dot(self.Kinv, add_ones(uvs).T).T[:, 0:2]
def projectPoints(self, pts_c):
uvs_hom = self.K @ add_ones(pts_c).T
uvs = uvs_hom / uvs_hom[3]
return uvs.T[:, 0:2]