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]