def _two_view_reconstruction_inliers(b1, b2, R, t, threshold): p = pyopengv.triangulation_triangulate(b1, b2, t, R) br1 = p.copy() br1 /= np.linalg.norm(br1, axis=1)[:, np.newaxis] br2 = R.T.dot((p - t).T).T br2 /= np.linalg.norm(br2, axis=1)[:, np.newaxis] ok1 = np.linalg.norm(br1 - b1, axis=1) < threshold ok2 = np.linalg.norm(br2 - b2, axis=1) < threshold return np.nonzero(ok1 * ok2)[0]
def _compute_inliers_bearings(b1, b2, T, threshold=0.01): R = T[:, :3] t = T[:, 3] p = pyopengv.triangulation_triangulate(b1, b2, t, R) br1 = p.copy() br1 /= np.linalg.norm(br1, axis=1)[:, np.newaxis] br2 = R.T.dot((p - t).T).T br2 /= np.linalg.norm(br2, axis=1)[:, np.newaxis] ok1 = multiview.vector_angle_many(br1, b1) < threshold ok2 = multiview.vector_angle_many(br2, b2) < threshold return ok1 * ok2
def compute_inliers_bearings(b1, b2, T): R = T[:, :3] t = T[:, 3] p = pyopengv.triangulation_triangulate(b1, b2, t, R) br1 = p.copy() br1 /= np.linalg.norm(br1, axis=1)[:, np.newaxis] br2 = R.T.dot((p - t).T).T br2 /= np.linalg.norm(br2, axis=1)[:, np.newaxis] ok1 = np.linalg.norm(br1 - b1, axis=1) < 0.01 # TODO(pau): compute angular error and use proper threshold ok2 = np.linalg.norm(br2 - b2, axis=1) < 0.01 return ok1 * ok2
def test_triangulation(): print "Testing triangulation" d = RelativePoseDataset(10, 0.0, 0.0) points1 = pyopengv.triangulation_triangulate( d.bearing_vectors1, d.bearing_vectors2, d.position, d.rotation) assert np.allclose(d.points, points1) points2 = pyopengv.triangulation_triangulate2( d.bearing_vectors1, d.bearing_vectors2, d.position, d.rotation) assert np.allclose(d.points, points2) print "Done testing triangulation"
def test_triangulation(): print("Testing triangulation") d = RelativePoseDataset(10, 0.0, 0.0) points1 = pyopengv.triangulation_triangulate( d.bearing_vectors1, d.bearing_vectors2, d.position, d.rotation) assert np.allclose(d.points, points1) points2 = pyopengv.triangulation_triangulate2( d.bearing_vectors1, d.bearing_vectors2, d.position, d.rotation) assert np.allclose(d.points, points2) print("Done testing triangulation")
def _two_view_reconstruction_inliers(b1, b2, R, t, threshold): """Compute number of points that can be triangulated. Args: b1, b2: Bearings in the two images. R, t: Rotation and translation from the second image to the first. That is the opengv's convention and the opposite of many functions in this module. threshold: max reprojection error in radians. Returns: array: Inlier indices. """ p = pyopengv.triangulation_triangulate(b1, b2, t, R) br1 = p.copy() br1 /= np.linalg.norm(br1, axis=1)[:, np.newaxis] br2 = R.T.dot((p - t).T).T br2 /= np.linalg.norm(br2, axis=1)[:, np.newaxis] ok1 = np.linalg.norm(br1 - b1, axis=1) < threshold ok2 = np.linalg.norm(br2 - b2, axis=1) < threshold return np.nonzero(ok1 * ok2)[0]
def PnPfrom2D(pc_to_align, pc_refs, desc_to_align, desc_refs, inits_T, K, **kwargs): match_function = kwargs.pop('match_function', None) desc_function = kwargs.pop('desc_function', None) only_pc_for_triangulation = kwargs.pop('only_pc_for_triangulation', False) pnp_param = kwargs.pop( 'pnp_param', { 'pnp_algo': 'GAO', 'inliers_threshold': 0.1, 'ransac_threshold': 0.0002, 'iterations': 1000 }) if kwargs: raise TypeError('Unexpected **kwargs: %r' % kwargs) Rr = list() u_dir = list() x_r = list() inliers = list() if only_pc_for_triangulation: pc_match_function = ICPNet.MatchNet( normalize_desc=False, knn='bidirectional', ).to(pc_to_align.device) # First 3D points triangulation if only_pc_for_triangulation: res_match = pc_match_function(pc_refs[0], pc_refs[1], pc_refs[0], pc_refs[1]) else: if desc_function is not None: desc_ref_0 = desc_function(pc_refs[0], desc_refs[0]) desc_ref_1 = desc_function(pc_refs[1], desc_refs[1]) else: desc_ref_0 = pc_refs[0] desc_ref_1 = pc_refs[1] res_match = match_function(pc_refs[0], pc_refs[1], desc_ref_0, desc_ref_1) match_function.unfit() if 'inliers' in res_match.keys(): filtered_pc_ref_0 = pc_refs[0][ 0, :, res_match['inliers'][0].byte()].unsqueeze(0) filtered_desc_ref_0 = desc_refs[0][ 0, :, res_match['inliers'][0].byte()].unsqueeze(0) filtered_pc_ref_1 = res_match['nn'][ 0, :, res_match['inliers'][0].byte()].unsqueeze(0) if filtered_pc_ref_0.size(2) < res_match['inliers'][0].size(0) * 0.10: logger.warning('Not enought triangulated point (only {})'.format( filtered_pc_ref_0.size(2))) logger.warning('Returning nn pose') return {'T': inits_T[0]} else: filtered_pc_ref_0 = pc_refs[0] filtered_desc_ref_0 = desc_refs[0] filtered_pc_ref_1 = res_match['nn'] mean_pc = (filtered_pc_ref_0 + filtered_pc_ref_1) / 2 pc_ref_0 = inits_T[0].inverse().matmul(mean_pc) #pc_ref_0 = inits_T[0].inverse().matmul(filtered_pc_ref_0) keypoints_0 = reproject_back(pc_ref_0, K.squeeze()) bearing_vector_0 = keypoints_to_bearing(keypoints_0, K.squeeze()) nn_match_1 = inits_T[1].inverse().matmul(mean_pc) #nn_match_1 = inits_T[1].inverse().matmul(filtered_pc_ref_1) keypoints_1 = reproject_back(nn_match_1, K.squeeze()) bearing_vector_1 = keypoints_to_bearing(keypoints_1, K.squeeze()) R_r = inits_T[0][0, :3, :3].t().matmul(inits_T[1][0, :3, :3]) t_r = inits_T[0][0, :3, :3].t().matmul(inits_T[1][0, :3, 3] - inits_T[0][0, :3, 3]) tri_points = pyopengv.triangulation_triangulate( bearing_vector_0.t().cpu().numpy(), bearing_vector_1.t().cpu().numpy(), t_r.cpu().numpy(), R_r.cpu().numpy()) tri_pc_ref = filtered_pc_ref_0.clone().detach() tri_pc_ref[:, :3, :] = (inits_T[0][0, :3, :3].matmul(torch.from_numpy(tri_points).t().float().to(tri_pc_ref.device)) \ + inits_T[0][0, :3, 3].view(3, 1)).unsqueeze(0) return PnP(pc_to_align, tri_pc_ref, desc_to_align, filtered_desc_ref_0, inits_T[0], K, **pnp_param, match_function=match_function, desc_function=desc_function)
def triangulationTest(src, dst, position, rotation): points1 = gv.triangulation_triangulate(src, dst, position, rotation) # print points1 points2 = gv.triangulation_triangulate2(src, dst, position, rotation)