def test_half_bt24cell_geom():
cellquatang = 2 * np.arccos(
np.clip(np.sum(half_bt24cell_faces * half_bt24cell_faces[:, None], -1),
-1, 1))
cellquatang = np.minimum(cellquatang, 2 * np.pi - cellquatang)
cellcen = homog.quat.quat_to_xform(half_bt24cell_faces)
cellcenang = homog.angle_of(homog.hinv(cellcen) @ cellcen[:, None])
cellcenanground = np.round(cellcenang * 180 / np.pi, 5)
# print(np.unique(cellcenanground))
assert np.allclose(cellcenang, cellquatang)
# np.fill_diagonal(cellcenang, 10)
q = homog.quat.rand_quat((1000, ))
qang = 2 * np.arccos(
np.clip(np.abs(np.sum(q * half_bt24cell_faces[:, None], -1)), -1, 1))
minqang = np.min(qang, 0)
# print('Qang to closest',
# np.percentile(minqang * 180 / np.pi, np.arange(0, 101, 10)))
assert np.max(minqang) < 62.81 / 180 * np.pi
x = homog.rand_xform(1000)
cellxform = homog.quat.quat_to_xform(half_bt24cell_faces)
xang = homog.angle_of(x @ homog.hinv(cellxform)[:, None])
minxang = np.min(xang, 0)
# print('Xang to closest:',
# np.percentile(minxang * 180 / np.pi, np.arange(0, 101, 10)))
assert np.max(minqang) < 62.81 / 180 * np.pi
def score(self, segpos, *, verbosity=False, **kw):
x_from = segpos[self.from_seg]
x_to = segpos[self.to_seg]
xhat = x_to @ inv(x_from)
trans = xhat[..., :, 3]
if self.nfold is 1:
angle = hm.angle_of(xhat)
carterrsq = np.sum(trans[..., :3]**2, axis=-1)
roterrsq = angle**2
else:
if self.origin_seg is not None:
tgtaxis = segpos[self.origin_seg] @ [0, 0, 1, 0]
tgtcen = segpos[self.origin_seg] @ [0, 0, 0, 1]
axis, angle, cen = hm.axis_ang_cen_of(xhat)
carterrsq = hm.hnorm2(cen - tgtcen)
roterrsq = (1 - np.abs(hm.hdot(axis, tgtaxis))) * np.pi
else: # much cheaper if cen not needed
axis, angle = hm.axis_angle_of(xhat)
carterrsq = roterrsq = 0
carterrsq = carterrsq + hm.hdot(trans, axis)**2
roterrsq = roterrsq + (angle - self.symangle)**2
# if self.relweight is not None:
# # penalize 'relative' error
# distsq = np.sum(trans[..., :3]**2, axis=-1)
# relerrsq = carterrsq / distsq
# relerrsq[np.isnan(relerrsq)] = 9e9
# # too much of a hack??
# carterrsq += self.relweight * relerrsq
if verbosity > 0:
print('axis', axis[0])
print('trans', trans[0])
print('dot trans', hm.hdot(trans, axis)[0])
print('angle', angle[0] * 180 / np.pi)
return np.sqrt(carterrsq / self.tol**2 + roterrsq / self.rot_tol**2)
def test_XformBinner_covrad():
niter = 30
nsamp = 10000
for i in range(niter):
cart_resl = np.random.rand() * 10 + 0.1
ori_resl = np.random.rand() * 50 + 2.5
xforms = homog.rand_xform(nsamp)
xb = XformBinner(cart_resl, ori_resl)
idx = xb.get_bin_index(xforms)
cen, f6 = xb.get_bin_center(idx, debug=True)
cart_dist = np.linalg.norm(xforms[..., :3, 3] - cen[..., :3, 3],
axis=-1)
ori_dist = homog.angle_of(homog.hinv(cen) @ xforms)
# if not np.all(cart_dist < cart_resl):
# print('ori_resl', ori_resl, 'nside:', xb.ori_nside,
# 'max(cart_dist):', np.max(cart_dist), cart_resl)
# if not np.all(cart_dist < cart_resl):
# print('ori_resl', ori_resl, 'nside:', xb.ori_nside,
# 'max(ori_dist):', np.max(ori_dist))
assert np.sum(cart_dist < cart_resl) > nsamp * 0.99
assert np.sum(ori_dist < ori_resl / 180 * np.pi) > nsamp * 0.99
import homog
with redirect_stdout(open(os.devnull,'w')):
from xbin import *
ori_nside = int(sys.argv[1])
Nsamp = 1000000
Nptiles = 101
ptiles = np.concatenate([np.arange(Nptiles),
100 - 10**np.linspace(2, -6, Nptiles)])
estimate = np.zeros_like(ptiles)
coherent = np.zeros_like(ptiles)
while True:
xforms = homog.rand_xform(Nsamp)
xb = XformBinner(ori_nside=ori_nside)
idx = xb.get_bin_index(xforms)
cen = xb.get_bin_center(idx)
ori_dist = homog.angle_of(homog.hinv(cen) @ xforms)
ehat = np.percentile(ori_dist, ptiles)
old = estimate
estimate = np.maximum(estimate, ehat)
if np.any(old != estimate):
print('independent', Nsamp, ori_nside,
str(estimate.tolist()).replace(',','')[1:-1])
sys.stdout.flush()
if np.sum(ehat) > np.sum(coherent):
coherent = ehat
print('coherent', Nsamp, ori_nside,
str(coherent.tolist()).replace(',','')[1:-1])
sys.stdout.flush()