def triangulate_ransac_pt(Ps, projs, im_shapes, ransac_thresh, max_pt_dist=5.):
best_inliers = []
CHOSEN_MIN_ANGLE = True
MIN_ANGLE = np.radians(1)
MAX_ANGLE = np.radians(120)
INLIER_MINDIST = False
INLIER_MINANGLE = True
MIN_TRI_DIST = False
TRI_DIST = 1.
ts = np.array([center_from_P(P) for P in Ps])
for inds in ransac_sample(len(Ps), 2, 300):
Ps_s, projs_s, shapes_s = ut.take_inds_each([Ps, projs, im_shapes],
inds)
X = triangulate_nonlinear_pt(Ps_s, projs_s, shapes_s)
if CHOSEN_MIN_ANGLE:
angle = ut.angle_between(-X + ts[inds[0]], -X + ts[inds[1]])
if angle <= MIN_ANGLE or angle >= MAX_ANGLE:
continue
if MIN_TRI_DIST and pylab.dist(ts[inds[0]], ts[inds[1]]) <= TRI_DIST:
continue
inliers = []
for pi, (P, proj) in enumerate(zip(Ps, projs)):
if reproj_error(P, X, proj) <= ransac_thresh \
and in_front_of(P, X) and pylab.dist(center_from_P(P), X) <= max_pt_dist:
inliers.append(pi)
inliers = np.array(inliers)
#[inliers] = np.nonzero(np.array([reproj_error(P, X, proj) <= ransac_thresh for P, proj in zip(Ps, projs)]))
if INLIER_MINDIST:
inliers = greedy_choose_mindist(ts, inliers, 0.5)
#inliers = greedy_choose_minangle(ts, inliers, 0.5)
if len(inliers) < 2:
continue
if INLIER_MINANGLE:
ordered_cams = [i for i in inds if i in inliers] + ut.shuffled(
without(inliers, inds))
inliers = greedy_choose_minangle(Ps, X, ordered_cams, MIN_ANGLE)
if len(inliers) > len(best_inliers):
best_inliers = inliers
if len(best_inliers) == 0:
final = [0, 1]
else:
final = best_inliers
Ps_s, projs_s, shapes_s = ut.take_inds_each([Ps, projs, im_shapes], final)
X = triangulate_nonlinear_pt(Ps_s, projs_s, shapes_s)
print 'num inliers', len(best_inliers), 'of', len(
Ps), 'mean reproj error', np.mean(
[reproj_error(P, X, x) for P, x in zip(Ps_s, projs_s)])
return X, best_inliers
def greedy_choose_minangle(Ps, X, ordered_inds, min_angle):
chosen = []
chosen_rays = []
for i in ordered_inds:
ray_i = -X + center_from_P(Ps[i])
for ray_j in chosen_rays:
if ut.angle_between(ray_i, ray_j) <= min_angle:
break
else:
chosen.append(i)
chosen_rays.append(ray_i)
return chosen
def triangulate_search(Ps, projs, im_shapes):
projs = [p[0] for p in projs]
ts = np.array([center_from_P(P) for P in Ps])
(max_angle, X_best, tri) = (None, None, ut.Struct(inliers=[]))
for inds in ransac_sample(len(Ps), 2, 300):
Ps_s, projs_s, shapes_s = ut.take_inds_each([Ps, projs, im_shapes],
inds)
# # does not seem to help
# F = F_from_Ps(Ps_s[0], Ps_s[1], center_from_P(Ps_s[1]))
# if not F_test_matches(F, projs_s[0][na, :], projs_s[1][na, :])[0]:
# continue
X = triangulate_nonlinear_pt(Ps_s, projs_s, shapes_s)
angle = ut.angle_between(-X + ts[inds[0]], -X + ts[inds[1]])
pt_ok = (in_front_of(Ps_s[0], X) and in_front_of(Ps_s[1], X)) \
and (max(reproj_error(P, X, x) for P, x in zip(Ps_s, projs_s)) <= 100)
#pt_ok = (in_front_of(Ps_s[0], X) and in_front_of(Ps_s[1], X))
# F = F_from_Ps(Ps_s[0], Ps_s[1], center_from_P(Ps_s[1]))
# if max(reproj_error(P, X, x) for P, x in zip(Ps_s, projs_s)) > 100 and F_test_matches(F, projs_s[0][na, :], projs_s[1][na, :])[0]:
# asdf
if pt_ok:
if max_angle <= angle:
max_angle = angle
X_best = X
tri = ut.Struct(inliers=inds,
angle_deg=np.degrees(angle),
in_front=(in_front_of(Ps_s[0], X),
in_front_of(Ps_s[1], X)),
cam_dist=pylab.dist(ts[inds[0]], ts[inds[1]]),
reproj_errors=[
reproj_error(P, X, x)
for P, x in zip(Ps_s, projs_s)
])
return X_best, tri