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 _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 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 _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 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)
