From 3501bf77050d4572f146fd82f3ccab9168e66860 Mon Sep 17 00:00:00 2001 From: duembgen Date: Thu, 26 Oct 2017 16:21:50 +0200 Subject: [PATCH 1/9] Added new aloc file. --- pylocus/aloc.py | 213 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 213 insertions(+) create mode 100644 pylocus/aloc.py diff --git a/pylocus/aloc.py b/pylocus/aloc.py new file mode 100644 index 0000000..350f211 --- /dev/null +++ b/pylocus/aloc.py @@ -0,0 +1,213 @@ +#!/usr/bin/env python +# module A-LOC +from algorithm_constraints import get_all_constraints, projection +from basics_alternating import get_absolute_angles_tensor +from basics_aloc import from_0_to_2pi, get_point, get_inner_angle, get_absolute_angle, get_theta_tensor +from math import pi, floor, cos, sin +from pylocus.point_set import AngleSet +from plots import plot_theta_errors, plot_point_sets +from .basics import rmse +import numpy as np + + +def reconstruct_from_inner_angles(P0, P1, theta_02, theta_12, theta): + ''' + Returns the reconstruction of pointset from inner angles vector theta. + Args: + P0: point 0 + P1: point 1 + theta_02: absolute orientation of line between P0 and P2 + theta_12: absolute orientation of line between P1 and P2 + theta: vector containing inner angles + Returns: + own: obtained AngleSet object + ''' + N = theta.shape[0] + d = P0.shape[0] + show = False + own = AngleSet(N, d) + if (show): + print('N = %r, d = %r' % (N, d)) + # Initialize to fix orientation + own.points[0, :] = P0 + own.points[1, :] = P1 + theta_01 = get_absolute_angle(P0, P1) + theta_10 = get_absolute_angle(P1, P0) + own.abs_angles[0, 2] = theta_02 + own.abs_angles[2, 0] = from_0_to_2pi(theta_02 + pi) + own.abs_angles[1, 2] = theta_12 + own.abs_angles[2, 1] = from_0_to_2pi(theta_12 + pi) + own.points[2, :] = get_point(own.abs_angles[0, 2], own.abs_angles[ + 1, 2], own.points[0, :], own.points[1, :]) + + # Calculate point coordinates + if N <= 3: + return own + for k in range(3, N): + i = k - 2 # 1 + j = k - 1 # 2 + m = k - 3 # 0 + Pi = own.points[i, :] + Pj = own.points[j, :] + Pm = own.points[m, :] + + theta_ij = own.abs_angles[i, j] + theta_ji = own.abs_angles[j, i] + thetai_jk = theta[i, j, k] + thetaj_ik = theta[j, i, k] + if (show): + print('i,j,k', i, j, k) + print('theta_ij', theta_ij) + print('thetai_jk', thetai_jk) + print('thetaj_ik', thetaj_ik) + + # try case one. + theta_ik1 = from_0_to_2pi(theta_ij + thetai_jk) + theta_jk1 = from_0_to_2pi(theta_ji - thetaj_ik) + Pk1 = get_point(theta_ik1, theta_jk1, Pi, Pj) + test_thetak_mi = get_inner_angle(Pk1, (Pm, Pi)) + truth_thetak_mi = theta[k, m, i] + error1 = abs(test_thetak_mi - truth_thetak_mi) + if error1 > 1e-10: + # try case two. + if (show): + print('wrong choice, thetak_mi should be', truth_thetak_mi, 'is', + test_thetak_mi, 'diff:', truth_thetak_mi - test_thetak_mi) + theta_ik2 = from_0_to_2pi(theta_ij - thetai_jk) + theta_jk2 = from_0_to_2pi(theta_ji + thetaj_ik) + Pk2 = get_point(theta_ik2, theta_jk2, Pi, Pj) + test_thetak_mi = get_inner_angle(Pk2, (Pm, Pi)) + truth_thetak_mi = theta[k, m, i] + error2 = abs(test_thetak_mi - truth_thetak_mi) + if abs(test_thetak_mi - truth_thetak_mi) > 1e-10: + # If both errors are not so great, choose the better one. + # print('new thetak_mi should be :%4.f, is: %4.f, diff: %r' % (truth_thetak_mi_new, test_thetak_mi_new, truth_thetak_mi_new - test_thetak_mi_new)) + # print('previous thetak_mi should be:%4.f, is: %4.f, diff: %r' % (truth_thetak_mi, test_thetak_mi, truth_thetak_mi - test_thetak_mi)) + if error1 > error2: + #if error2 > 1e-3: + # print('none of the options gives angle < 1e-3 (%r,%r)' % (error1, error2)) + #else: + theta_ik = theta_ik2 + theta_jk = theta_jk2 + Pk = Pk2 + else: + #if error1 > 1e-3: + # print('none of the options gives angle < 1e-3 (%r,%r)' % (error1, error2)) + #else: + theta_ik = theta_ik1 + theta_jk = theta_jk1 + Pk = Pk1 + else: + theta_ik = theta_ik2 + theta_jk = theta_jk2 + Pk = Pk2 + else: + theta_ik = theta_ik1 + theta_jk = theta_jk1 + Pk = Pk1 + + own.points[k, :] = Pk + own.abs_angles[i, k] = from_0_to_2pi(theta_ik) + own.abs_angles[j, k] = from_0_to_2pi(theta_jk) + own.abs_angles[k, i] = from_0_to_2pi(theta_ik + pi) + own.abs_angles[k, j] = from_0_to_2pi(theta_jk + pi) + #own.plot_some(range(m,k+1),'some') + own.init() + own.get_theta_tensor() + return own + + +def reconstruct(Pi, Pj, i, j, theta_tensor, Pk='', k=-1, print_out=False): + ''' + Returns the reconstruction of pointset from inner angles vector theta. If Pk and k are set, it corrects for the original flip. + Args: + Pi: point i of base line. + Pj: point j of base line. + i: index of point Pi. + j: index of point Pj. + theta: tensor containing inner angles + Pk: coordinates of point k (cannot be 0 for now.) + k: index of point Pk + Returns: + own: obtained AngleSet object + ''' + + def intersect(Pi, Pj, alpha_ik, alpha_jk): + A = np.matrix([[cos(alpha_ik), -cos(alpha_jk)], + [sin(alpha_ik), -sin(alpha_jk)]]) + b = Pj - Pi + s = np.linalg.solve(A, b) + Pk_1 = Pi + s[0] * np.array([cos(alpha_ik), sin(alpha_ik)]) + Pk_2 = Pj + s[1] * np.array([cos(alpha_jk), sin(alpha_jk)]) + assert np.isclose(Pk_1, Pk_2).all() + return Pk_1 + + # Initialize to fix orientation + N = theta_tensor.shape[0] + own = AngleSet(N, Pi.shape[0]) + own.points[i, :] = Pi + own.points[j, :] = Pj + alpha_ij = get_absolute_angle(Pi, Pj) + alpha_ji = get_absolute_angle(Pj, Pi) + if (print_out): + print('alpha_ij, alpha_ji', 180 * np.array((alpha_ij, alpha_ji)) / pi) + + left_indices = np.delete(np.arange(N), (i, j)) + + # Get directed angles for both points. + __, alphas_tensor, __ = get_absolute_angles_tensor( + theta_tensor, theta_tensor, (i, j), N) + + # Correct for consistent flip. + if abs(alphas_tensor[j, j, k] - alpha_ji) < pi: + if (print_out): + print('flip', j) + alphas_tensor[j, :, :] *= -1 + if abs(alphas_tensor[i, i, k] - alpha_ij) < pi: + if (print_out): + print('flip', i) + alphas_tensor[i, :, :] *= -1 + if (print_out): + print('alphas_tensor', alphas_tensor * 180 / pi) + + # Correct for true flip. + alpha_ij = get_absolute_angle(Pi, Pj) + alphas_tensor[i, i, :] = from_0_to_2pi(alphas_tensor[i, j, :] + alpha_ij) + if k >= 0: + alpha_ik = get_absolute_angle(Pi, Pk) + calc_alpha_ik = alphas_tensor[i, i, k] + diff_calc = from_0_to_2pi(alpha_ij - calc_alpha_ik) + diff = from_0_to_2pi(alpha_ij - alpha_ik) + if diff_calc < pi and diff_calc > 0: + if not (diff < pi and diff > 0): + alphas_tensor *= -1.0 + else: + if not (diff > pi or diff < 0): + alphas_tensor *= -1.0 + + alpha_ji = get_absolute_angle(Pj, Pi) + alphas_tensor[i, i, :] = from_0_to_2pi(alphas_tensor[i, j, :] + alpha_ij) + alphas_tensor[:, range(N), range(N)] = 0.0 + alphas_tensor[j, j, :] = from_0_to_2pi(alphas_tensor[j, i, :] + alpha_ji) + alphas_tensor[:, range(N), range(N)] = 0.0 + alphas_tensor[i, :, i] = from_0_to_2pi(alphas_tensor[i, :, j] + alpha_ji) + alphas_tensor[:, range(N), range(N)] = 0.0 + alphas_tensor[j, :, j] = from_0_to_2pi(alphas_tensor[j, :, i] + alpha_ij) + alphas_tensor[:, range(N), range(N)] = 0.0 + + for k in left_indices: + alpha_ik = alphas_tensor[i, i, k] + alpha_jk = alphas_tensor[j, j, k] + if print_out and k >= 0: + real_alpha_ik = get_absolute_angle(Pi, Pk) + real_alpha_jk = get_absolute_angle(Pj, Pk) + print('real alpha_ik, alpha_jk', 180 * + np.array((real_alpha_ik, real_alpha_jk)) / pi) + print('calc alpha_ik, alpha_jk', 180 * + np.array((alpha_ik, alpha_jk)) / pi) + Pk_1 = intersect(Pi, Pj, alpha_ik, alpha_jk) + own.points[k, :] = Pk_1 + own.init() + own.get_theta_tensor() + return own + From de7a636058c1b5c6da31ab294b8fd4bdd9b3fc38 Mon Sep 17 00:00:00 2001 From: duembgen Date: Thu, 26 Oct 2017 18:22:43 +0200 Subject: [PATCH 2/9] Simplified angles reconstruction. --- pylocus/aloc.py | 230 +++++++++------------------------------ pylocus/basics.py | 6 +- pylocus/basics_angles.py | 7 ++ 3 files changed, 62 insertions(+), 181 deletions(-) diff --git a/pylocus/aloc.py b/pylocus/aloc.py index 350f211..7adf413 100644 --- a/pylocus/aloc.py +++ b/pylocus/aloc.py @@ -4,119 +4,11 @@ from basics_alternating import get_absolute_angles_tensor from basics_aloc import from_0_to_2pi, get_point, get_inner_angle, get_absolute_angle, get_theta_tensor from math import pi, floor, cos, sin -from pylocus.point_set import AngleSet from plots import plot_theta_errors, plot_point_sets from .basics import rmse import numpy as np -def reconstruct_from_inner_angles(P0, P1, theta_02, theta_12, theta): - ''' - Returns the reconstruction of pointset from inner angles vector theta. - Args: - P0: point 0 - P1: point 1 - theta_02: absolute orientation of line between P0 and P2 - theta_12: absolute orientation of line between P1 and P2 - theta: vector containing inner angles - Returns: - own: obtained AngleSet object - ''' - N = theta.shape[0] - d = P0.shape[0] - show = False - own = AngleSet(N, d) - if (show): - print('N = %r, d = %r' % (N, d)) - # Initialize to fix orientation - own.points[0, :] = P0 - own.points[1, :] = P1 - theta_01 = get_absolute_angle(P0, P1) - theta_10 = get_absolute_angle(P1, P0) - own.abs_angles[0, 2] = theta_02 - own.abs_angles[2, 0] = from_0_to_2pi(theta_02 + pi) - own.abs_angles[1, 2] = theta_12 - own.abs_angles[2, 1] = from_0_to_2pi(theta_12 + pi) - own.points[2, :] = get_point(own.abs_angles[0, 2], own.abs_angles[ - 1, 2], own.points[0, :], own.points[1, :]) - - # Calculate point coordinates - if N <= 3: - return own - for k in range(3, N): - i = k - 2 # 1 - j = k - 1 # 2 - m = k - 3 # 0 - Pi = own.points[i, :] - Pj = own.points[j, :] - Pm = own.points[m, :] - - theta_ij = own.abs_angles[i, j] - theta_ji = own.abs_angles[j, i] - thetai_jk = theta[i, j, k] - thetaj_ik = theta[j, i, k] - if (show): - print('i,j,k', i, j, k) - print('theta_ij', theta_ij) - print('thetai_jk', thetai_jk) - print('thetaj_ik', thetaj_ik) - - # try case one. - theta_ik1 = from_0_to_2pi(theta_ij + thetai_jk) - theta_jk1 = from_0_to_2pi(theta_ji - thetaj_ik) - Pk1 = get_point(theta_ik1, theta_jk1, Pi, Pj) - test_thetak_mi = get_inner_angle(Pk1, (Pm, Pi)) - truth_thetak_mi = theta[k, m, i] - error1 = abs(test_thetak_mi - truth_thetak_mi) - if error1 > 1e-10: - # try case two. - if (show): - print('wrong choice, thetak_mi should be', truth_thetak_mi, 'is', - test_thetak_mi, 'diff:', truth_thetak_mi - test_thetak_mi) - theta_ik2 = from_0_to_2pi(theta_ij - thetai_jk) - theta_jk2 = from_0_to_2pi(theta_ji + thetaj_ik) - Pk2 = get_point(theta_ik2, theta_jk2, Pi, Pj) - test_thetak_mi = get_inner_angle(Pk2, (Pm, Pi)) - truth_thetak_mi = theta[k, m, i] - error2 = abs(test_thetak_mi - truth_thetak_mi) - if abs(test_thetak_mi - truth_thetak_mi) > 1e-10: - # If both errors are not so great, choose the better one. - # print('new thetak_mi should be :%4.f, is: %4.f, diff: %r' % (truth_thetak_mi_new, test_thetak_mi_new, truth_thetak_mi_new - test_thetak_mi_new)) - # print('previous thetak_mi should be:%4.f, is: %4.f, diff: %r' % (truth_thetak_mi, test_thetak_mi, truth_thetak_mi - test_thetak_mi)) - if error1 > error2: - #if error2 > 1e-3: - # print('none of the options gives angle < 1e-3 (%r,%r)' % (error1, error2)) - #else: - theta_ik = theta_ik2 - theta_jk = theta_jk2 - Pk = Pk2 - else: - #if error1 > 1e-3: - # print('none of the options gives angle < 1e-3 (%r,%r)' % (error1, error2)) - #else: - theta_ik = theta_ik1 - theta_jk = theta_jk1 - Pk = Pk1 - else: - theta_ik = theta_ik2 - theta_jk = theta_jk2 - Pk = Pk2 - else: - theta_ik = theta_ik1 - theta_jk = theta_jk1 - Pk = Pk1 - - own.points[k, :] = Pk - own.abs_angles[i, k] = from_0_to_2pi(theta_ik) - own.abs_angles[j, k] = from_0_to_2pi(theta_jk) - own.abs_angles[k, i] = from_0_to_2pi(theta_ik + pi) - own.abs_angles[k, j] = from_0_to_2pi(theta_jk + pi) - #own.plot_some(range(m,k+1),'some') - own.init() - own.get_theta_tensor() - return own - - def reconstruct(Pi, Pj, i, j, theta_tensor, Pk='', k=-1, print_out=False): ''' Returns the reconstruction of pointset from inner angles vector theta. If Pk and k are set, it corrects for the original flip. @@ -131,83 +23,63 @@ def reconstruct(Pi, Pj, i, j, theta_tensor, Pk='', k=-1, print_out=False): Returns: own: obtained AngleSet object ''' + from .algorithms import procrustes, apply_transform + from .point_set import create_from_points, AngleSet + from .basics_angles import get_inner_angle def intersect(Pi, Pj, alpha_ik, alpha_jk): A = np.matrix([[cos(alpha_ik), -cos(alpha_jk)], [sin(alpha_ik), -sin(alpha_jk)]]) - b = Pj - Pi - s = np.linalg.solve(A, b) - Pk_1 = Pi + s[0] * np.array([cos(alpha_ik), sin(alpha_ik)]) - Pk_2 = Pj + s[1] * np.array([cos(alpha_jk), sin(alpha_jk)]) - assert np.isclose(Pk_1, Pk_2).all() - return Pk_1 - - # Initialize to fix orientation - N = theta_tensor.shape[0] - own = AngleSet(N, Pi.shape[0]) - own.points[i, :] = Pi - own.points[j, :] = Pj - alpha_ij = get_absolute_angle(Pi, Pj) - alpha_ji = get_absolute_angle(Pj, Pi) - if (print_out): - print('alpha_ij, alpha_ji', 180 * np.array((alpha_ij, alpha_ji)) / pi) - - left_indices = np.delete(np.arange(N), (i, j)) + s = np.linalg.solve(A, Pj) + P2 = s[0] * np.array([cos(alpha_ik), sin(alpha_ik)]) + P2_same = Pj + s[1] * np.array([cos(alpha_jk), sin(alpha_jk)]) + assert np.isclose(P2, P2_same).all() - # Get directed angles for both points. - __, alphas_tensor, __ = get_absolute_angles_tensor( - theta_tensor, theta_tensor, (i, j), N) + theta = get_inner_angle(P0, [P1, P2]) - # Correct for consistent flip. - if abs(alphas_tensor[j, j, k] - alpha_ji) < pi: - if (print_out): - print('flip', j) - alphas_tensor[j, :, :] *= -1 - if abs(alphas_tensor[i, i, k] - alpha_ij) < pi: - if (print_out): - print('flip', i) - alphas_tensor[i, :, :] *= -1 - if (print_out): - print('alphas_tensor', alphas_tensor * 180 / pi) + assert abs(theta - theta_tensor[i, j, l]) < 1e-10, '{}-{} not smaller than 1e-10'.format( + degrees(theta), degrees(theta_tensor[i, j, l])) + theta = get_inner_angle(P1, [P0, P2]) + assert abs(theta - theta_tensor[j, i, l]) < 1e-10, '{}-{} not smaller than 1e-10'.format( + degrees(theta), degrees(theta_tensor[j, i, l])) + return P2 - # Correct for true flip. - alpha_ij = get_absolute_angle(Pi, Pj) - alphas_tensor[i, i, :] = from_0_to_2pi(alphas_tensor[i, j, :] + alpha_ij) - if k >= 0: - alpha_ik = get_absolute_angle(Pi, Pk) - calc_alpha_ik = alphas_tensor[i, i, k] - diff_calc = from_0_to_2pi(alpha_ij - calc_alpha_ik) - diff = from_0_to_2pi(alpha_ij - alpha_ik) - if diff_calc < pi and diff_calc > 0: - if not (diff < pi and diff > 0): - alphas_tensor *= -1.0 - else: - if not (diff > pi or diff < 0): - alphas_tensor *= -1.0 - - alpha_ji = get_absolute_angle(Pj, Pi) - alphas_tensor[i, i, :] = from_0_to_2pi(alphas_tensor[i, j, :] + alpha_ij) - alphas_tensor[:, range(N), range(N)] = 0.0 - alphas_tensor[j, j, :] = from_0_to_2pi(alphas_tensor[j, i, :] + alpha_ji) - alphas_tensor[:, range(N), range(N)] = 0.0 - alphas_tensor[i, :, i] = from_0_to_2pi(alphas_tensor[i, :, j] + alpha_ji) - alphas_tensor[:, range(N), range(N)] = 0.0 - alphas_tensor[j, :, j] = from_0_to_2pi(alphas_tensor[j, :, i] + alpha_ij) - alphas_tensor[:, range(N), range(N)] = 0.0 - - for k in left_indices: - alpha_ik = alphas_tensor[i, i, k] - alpha_jk = alphas_tensor[j, j, k] - if print_out and k >= 0: - real_alpha_ik = get_absolute_angle(Pi, Pk) - real_alpha_jk = get_absolute_angle(Pj, Pk) - print('real alpha_ik, alpha_jk', 180 * - np.array((real_alpha_ik, real_alpha_jk)) / pi) - print('calc alpha_ik, alpha_jk', 180 * - np.array((alpha_ik, alpha_jk)) / pi) - Pk_1 = intersect(Pi, Pj, alpha_ik, alpha_jk) - own.points[k, :] = Pk_1 - own.init() - own.get_theta_tensor() + # Initialize to fix orientation + N = theta_tensor.shape[0] + + points = np.zeros((N, Pi.shape[0])) + + P0 = np.array((0, 0)) + P1 = np.array((Pj[0] - Pi[0], 0)) + + points[i, :] = P0 + points[j, :] = P1 + left_indices = np.delete(range(N), (i, j)) + + first = True + for counter, l in enumerate(left_indices): + alpha_il = theta_tensor[i, j, l] + alpha_jl = pi - theta_tensor[j, i, l] + Pl = intersect(P0, P1, alpha_il, alpha_jl) + if not first: + try: + previous = left_indices[counter - 1] + estimated = get_inner_angle(points[previous, :], (P0, Pl)) + expected = theta_tensor[previous, i, l] + assert abs( + estimated - expected) < 0.1, 'error {}'.format(abs(estimated - expected)) + except: + alpha_il = -alpha_il + alpha_jl = -alpha_jl + Pl = intersect(P0, P1, alpha_il, alpha_jl) + points[l, :] = Pl + first = False + anchors = np.r_[Pi,Pj,Pk].reshape(-1,2) + __, R, t, c = procrustes(anchors, points[[i,j,k],:], scale=False) + fitted = apply_transform(R, t, c, points, anchors) + own = create_from_points(fitted, AngleSet) return own + +def degrees(angle): + return 180 * angle / pi diff --git a/pylocus/basics.py b/pylocus/basics.py index 1ec2f2a..c2d7def 100644 --- a/pylocus/basics.py +++ b/pylocus/basics.py @@ -70,10 +70,10 @@ def eigendecomp(G, d): lamda = np.real(lamda) lamda_sorted = lamda[indices] assert (lamda_sorted[ - :d] > -1e-10).all(), "{} not all positive!".format(lamda_sorted[:d]) - + :d] > -1e-3).all(), "{} not all positive!".format(lamda_sorted) u = u[:, indices] factor = np.zeros((N,)) + lamda_sorted[lamda_sorted < 0] = 0.0 factor[0:d] = np.sqrt(lamda_sorted[:d]) return np.real(factor), np.real(u) @@ -147,3 +147,5 @@ def matrix_from_vector(vector, N): matrix = np.zeros((N, N)) matrix[triu_idx[0], triu_idx[1]] = vector return matrix + + diff --git a/pylocus/basics_angles.py b/pylocus/basics_angles.py index 590a3d4..152c506 100644 --- a/pylocus/basics_angles.py +++ b/pylocus/basics_angles.py @@ -94,3 +94,10 @@ def get_theta_tensor(theta, corners, N): theta_tensor[int(idx[0]), int(idx[1]), int(idx[2])] = theta[k] theta_tensor[int(idx[0]), int(idx[2]), int(idx[1])] = theta[k] return theta_tensor + +def get_index(corners, Pk, Pij): + ''' get index mask corresponding to angle at corner Pk with Pi, Pj.''' + angle1 = [Pk, Pij[0], Pij[1]] + angle2 = [Pk, Pij[1], Pij[0]] + index = np.bitwise_or(corners == angle1, corners == angle2) + return index.all(axis=1) From 88f7069ea9a3db6df78d8f75b43804d1c6f964d0 Mon Sep 17 00:00:00 2001 From: duembgen Date: Thu, 26 Oct 2017 18:31:23 +0200 Subject: [PATCH 3/9] Fixed AngleSet and extracted methods from procrustes. --- pylocus/algorithms.py | 44 +++++++++++++++++++++++-------------- pylocus/plots_cti.py | 7 ++---- pylocus/point_set.py | 51 ++++++++++++++++++++++++++----------------- 3 files changed, 61 insertions(+), 41 deletions(-) diff --git a/pylocus/algorithms.py b/pylocus/algorithms.py index f3081b2..8821559 100644 --- a/pylocus/algorithms.py +++ b/pylocus/algorithms.py @@ -52,26 +52,21 @@ def procrustes(anchors, X, scale=True, print_out=False): :return: the transformed vector X, the rotation matrix, translation vector, and scaling factor. """ - def centralize(X): - n = X.shape[0] - ones = np.ones((n, 1)) - return X - np.multiply(1 / n * np.dot(ones.T, X), ones) m = anchors.shape[0] - N, d = X.shape - assert m >= d, 'Have to give at least d anchor nodes.' - X_m = X[N - m:, :] ones = np.ones((m, 1)) + center_points, center_anchors, points_to_fit = get_centers(X, anchors) - mux = 1 / m * np.dot(ones.T, X_m) - muy = 1 / m * np.dot(ones.T, anchors) - sigmax = 1 / m * np.linalg.norm(X_m - mux)**2 - sigmaxy = 1 / m * np.dot((anchors - muy).T, X_m - mux) + sigmax = 1 / m * np.linalg.norm(points_to_fit - center_points)**2 + sigmaxy = 1 / m * np.dot((anchors - center_anchors).T, points_to_fit - center_points) try: U, D, VT = np.linalg.svd(sigmaxy) - except np.LinAlgError: + except np.linalg.LinAlgError: print('strange things are happening...') - print(sigmaxy) - print(np.linalg.matrix_rank(sigmaxy)) + print('anchors',anchors) + print('X',X) + print('sigma',sigmaxy) + print('rank',np.linalg.matrix_rank(sigmaxy)) + raise #this doesn't work and doesn't seem to be necessary! (why?) # S = np.eye(D.shape[0]) # if (np.linalg.det(U)*np.linalg.det(VT.T) < 0): @@ -89,11 +84,28 @@ def centralize(X): print('Optimal scale would be: {}. Setting it to 1 now.'.format(c)) c = 1.0 R = np.dot(U, VT) - t = muy.T - c * np.dot(R, mux.T) - X_transformed = (c * np.dot(R, (X - mux).T) + muy.T).T + t = center_anchors.T - c * np.dot(R, center_points.T) + X_transformed = apply_transform(R, t, c, X, anchors) return X_transformed, R, t, c +def get_centers(X, anchors): + N, d = X.shape + m = anchors.shape[0] + ones = np.ones((m, 1)) + assert m >= d, 'Have to give at least d anchor nodes.' + + points_to_fit = X[N - m:, :] + center_points = 1 / m * np.dot(ones.T, points_to_fit) + center_anchors = 1 / m * np.dot(ones.T, anchors) + return center_points, center_anchors, points_to_fit + + +def apply_transform(R, t, c, X, anchors): + center_points, center_anchors, points_to_fit = get_centers(X, anchors) + return (c * np.dot(R, (X - center_points).T) + center_anchors.T).T + + def reconstruct_emds(edm, Om, real_points, method=None, **kwargs): """ Reconstruct point set using E(dge)-MDS. """ diff --git a/pylocus/plots_cti.py b/pylocus/plots_cti.py index 5065d96..e2b0b8b 100644 --- a/pylocus/plots_cti.py +++ b/pylocus/plots_cti.py @@ -39,12 +39,9 @@ def plot_tag_position(point_sets, title='', size=[10, 10], filename='', names=No legend = [] for p, points in enumerate(point_sets): - try: - N = points.shape[0] - except: - N = len(points) - if N == 0: + if len(points) == 0: break + N = points.shape[0] if p == 0: for i in range(N): pi = points[i] diff --git a/pylocus/point_set.py b/pylocus/point_set.py index 37e9194..8b99ff7 100644 --- a/pylocus/point_set.py +++ b/pylocus/point_set.py @@ -5,7 +5,7 @@ import numpy as np from .settings import * -from math import pi +from math import pi, acos, cos, sin class PointSet: @@ -41,7 +41,6 @@ def add_noise(self, noise, indices=None): if indices is None: indices = range(self.N) self.points = return_noisy_points(noise, indices, self.points.copy()) - self.init() def set_points(self, mode, points=None, range_=RANGE, size=1): """ Initialize points according to predefined modes. @@ -334,7 +333,7 @@ def get_orientation(k, i, j): theta_jk = from_0_to_2pi(theta_jk) return theta_ik, theta_jk - def return_noisy(self, noise, mode='noisy', idx=0, visualize=False): + def return_noisy(self, noise, mode='normal', idx=0, visualize=False): if mode == 'normal': theta = self.theta.copy() + np.random.normal(0, noise, self.M) if (visualize): @@ -354,7 +353,6 @@ def return_noisy(self, noise, mode='noisy', idx=0, visualize=False): # TODO: where do I need the three below? - def get_tensor_edm(self): D = np.empty([self.N * self.d, self.N * self.d]) for i in range(self.N): @@ -370,7 +368,6 @@ def get_tensor_edm(self): return D def get_closed_form(self, edm): - #TODO: what is this? Daug = self.get_tensor_edm() T = np.empty([self.N, self.N, self.N]) for i in range(self.N): @@ -394,8 +391,7 @@ def get_theta_tensor(self): self.theta_tensor = get_theta_tensor(self.theta, self.corners, self.N) return self.theta_tensor -# TODO: This is for iterative algorithm only... - + # TODO: This is for iterative algorithm only... def get_indices(self, k): """ Get indices of theta vector that have k as first corner. @@ -440,31 +436,30 @@ def get_G(self, k, add_noise=True): G[jdx, idx] = cos(thetak_ij) return G -# TODO: Which of these two is better? And should they really be in this class? - + # TODO: Which of these two is better? And should they really be in this class? def reconstruct_from_inner_angles(self, theta): - from .algorithms import reconstruct_from_inner_angles + from .aloc import reconstruct_from_inner_angles from .algorithms import procrustes + from pylocus.basics_angles import get_theta_tensor + theta_tensor = get_theta_tensor(theta, self.corners, self.N) reconstruction = reconstruct_from_inner_angles( self.points[0, :], self.points[1, :], self.abs_angles[0, 2], self.abs_angles[1, 2], theta_tensor) - new_points, __, __, __ = procrustes( - self.points, reconstruction.points, scale=True) - reconstruction.points = new_points - reconstruction.init() + #new_points, __, __, __ = procrustes( + # self.points, reconstruction.points, scale=True) + #reconstruction.points = new_points + #reconstruction.init() return reconstruction - def reconstruct(self, theta): - from .algorithms import reconstruct - i = 0 - j = 1 + def reconstruct(self, theta, i=0, j=1, k=2): + from .aloc import reconstruct + from pylocus.basics_angles import get_theta_tensor theta_tensor = get_theta_tensor(theta, self.corners, self.N) Pi = self.points[i, :] Pj = self.points[j, :] - k = 2 Pk = self.points[k, :] - reconstruction = reconstruct(Pi, Pj, i, j, theta_tensor, Pk, k) + reconstruction = reconstruct(Pi, Pj, i, j, theta_tensor, Pk, k, print_out=False) return reconstruction def get_convex_polygons(self, m, print_out=False): @@ -473,6 +468,7 @@ def get_convex_polygons(self, m, print_out=False): :return: (ordered) indices of all convex polygones of size m. """ + import itertools convex_polygons = [] for corners in itertools.combinations(np.arange(self.N), m): p = np.zeros(m, np.uint) @@ -516,6 +512,7 @@ def get_polygon_constraints(self, :return A, b: the constraints on the theta-vector of the form A*theta = b """ + from .basics_angles import get_index rows_A = [] rows_b = [] for m in range_polygones: @@ -530,6 +527,7 @@ def get_polygon_constraints(self, return self.A, self.b def get_angle_constraints_m(self, polygons_m, print_out=False): + from .basics_angles import get_index rows = [] m = len(polygons_m[0]) # initialization to empty led to A being filled with first row of @@ -587,6 +585,7 @@ def get_polygon_constraints_m(self, polygons_m, print_out=False): :return A, b: the constraints on the theta-vector of the form A*theta = b """ + from .basics_angles import get_index rows_b = [] rows_A = [] m = len(polygons_m[0]) @@ -617,6 +616,12 @@ def get_polygon_constraints_m(self, polygons_m, print_out=False): self.b = b return A, b + def copy(self): + new = AngleSet(self.N, self.d) + new.points = self.points.copy() + new.init() + return new + class HeterogenousSet(PointSet): """ Class containing heteregenous information in the form of direction vectors. @@ -693,6 +698,12 @@ def get_KE_constraints(self): b = np.zeros((C2.shape[0], 1)) return C2, b + def copy(self): + new = HeterogenousSet(self.N, self.d) + new.points = self.points.copy() + new.init() + return new + def dm_from_edm(edm): from .basics import vector_from_matrix From 3c0cf4ac5bbd9821fd57599aeb72fa4338d065a9 Mon Sep 17 00:00:00 2001 From: duembgen Date: Sun, 5 Nov 2017 12:30:01 +0100 Subject: [PATCH 4/9] Removed wrongly failling assertion. --- pylocus/aloc.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/pylocus/aloc.py b/pylocus/aloc.py index 7adf413..500982b 100644 --- a/pylocus/aloc.py +++ b/pylocus/aloc.py @@ -37,11 +37,12 @@ def intersect(Pi, Pj, alpha_ik, alpha_jk): theta = get_inner_angle(P0, [P1, P2]) - assert abs(theta - theta_tensor[i, j, l]) < 1e-10, '{}-{} not smaller than 1e-10'.format( - degrees(theta), degrees(theta_tensor[i, j, l])) + # TODO not robust to multiples of pi. + #assert abs(theta - theta_tensor[i, j, l]) < 1e-10, '{}-{} not smaller than 1e-10'.format( + # degrees(theta), degrees(theta_tensor[i, j, l])) theta = get_inner_angle(P1, [P0, P2]) - assert abs(theta - theta_tensor[j, i, l]) < 1e-10, '{}-{} not smaller than 1e-10'.format( - degrees(theta), degrees(theta_tensor[j, i, l])) + #assert abs(theta - theta_tensor[j, i, l]) < 1e-10, '{}-{} not smaller than 1e-10'.format( + # degrees(theta), degrees(theta_tensor[j, i, l])) return P2 # Initialize to fix orientation From b5cdc488a1229688e272f772294a212fbf19449c Mon Sep 17 00:00:00 2001 From: Frederike Date: Tue, 22 Oct 2019 18:52:03 +0200 Subject: [PATCH 5/9] Improved and added angle functionalities. --- pylocus/__init__.py | 8 +- pylocus/algorithms.py | 167 +++++++++++++++++++++++-- pylocus/basics.py | 4 + pylocus/basics_angles.py | 106 ++++++++++++++++ pylocus/edm_completion.py | 6 +- pylocus/lateration.py | 253 ++++++++++++++++++++------------------ pylocus/mds.py | 17 ++- pylocus/point_set.py | 22 ++-- 8 files changed, 434 insertions(+), 149 deletions(-) diff --git a/pylocus/__init__.py b/pylocus/__init__.py index 2fb9426..e49a82e 100644 --- a/pylocus/__init__.py +++ b/pylocus/__init__.py @@ -1,7 +1 @@ -from .settings import * -from .algorithms import * -from .basics import * -from .plots_cti import * -from .point_set import * - -__version__ = "0.0.3" +__version__ = "0.0.5dev" diff --git a/pylocus/algorithms.py b/pylocus/algorithms.py index 5cdb63a..c86242b 100644 --- a/pylocus/algorithms.py +++ b/pylocus/algorithms.py @@ -1,5 +1,8 @@ #!/usr/bin/env python # module ALGORITHMS + +from math import pi + import numpy as np IMPLEMENTED_METHODS = ['MDS', @@ -58,6 +61,26 @@ def classical_mds(D): return MDS(D, 1, 'geometric') +def complete_points(all_points, N): + ''' add zero-rows to top of all_points to reach total of N. + :param all_points: m x d array, where m <= N + :param N: final dimension of all_points. + + :return: number of added lines (n), new array all_points of size Nxd + + ''' + m = all_points.shape[0] + d = all_points.shape[1] + n = 1 # number of points to localize + if m < N: + n = N-m + all_points = np.r_[np.zeros((n, d)), all_points] + assert all_points.shape == (N, d) + elif m > N: + raise ValueError("Cannot have more anchor points than edm entries.") + return n, all_points + + def procrustes(anchors, X, scale=True, print_out=False): """ Fit X to anchors by applying optimal translation, rotation and reflection. @@ -65,7 +88,7 @@ def procrustes(anchors, X, scale=True, print_out=False): of coordinates X (output of EDM algorithm) optimally matching anchors in least squares sense. :param anchors: Matrix of shape m x d, where m is number of anchors, d is dimension of setup. - :param X: Matrix of shape N x d, where the last m points will be used to find fit with the anchors. + :param X: Matrix of shape N x d, of which the last m rows will be used to find fit with the anchors. :param scale: set to True if the point set should be scaled to match the anchors. :return: the transformed vector X, the rotation matrix, translation vector, and scaling factor. @@ -74,6 +97,8 @@ def centralize(X): n = X.shape[0] ones = np.ones((n, 1)) return X - np.multiply(1 / n * np.dot(ones.T, X), ones) + + assert X.shape[1] == anchors.shape[1], 'Anchors and X must be of shape (mxd) and (Nxd), respectively.' m = anchors.shape[0] N, d = X.shape assert m >= d, 'Have to give at least d anchor nodes.' @@ -86,10 +111,11 @@ def centralize(X): sigmaxy = 1 / m * np.dot((anchors - muy).T, X_m - mux) try: U, D, VT = np.linalg.svd(sigmaxy) - except np.LinAlgError: + except: print('strange things are happening...') print(sigmaxy) print(np.linalg.matrix_rank(sigmaxy)) + raise #this doesn't work and doesn't seem to be necessary! (why?) # S = np.eye(D.shape[0]) # if (np.linalg.det(U)*np.linalg.det(VT.T) < 0): @@ -109,6 +135,7 @@ def centralize(X): R = np.dot(U, VT) t = muy.T - c * np.dot(R, mux.T) X_transformed = (c * np.dot(R, (X - mux).T) + muy.T).T + assert np.allclose(X_transformed.shape, X.shape) return X_transformed, R, t, c @@ -167,13 +194,24 @@ def reconstruct_cdm(dm, absolute_angles, all_points, W=None): return Y -def reconstruct_mds(edm, all_points, completion='optspace', mask=None, method='geometric', print_out=False, n=1): +def reconstruct_mds(edm, all_points, completion='optspace', mask=None, method='geometric', print_out=False): """ Reconstruct point set using MDS and matrix completion algorithms. + + :param edm: Euclidean distance matrix, NxN. + :param all_points: Mxd vector of anchor coordinates. if M < N, M-N 0-row-vectors will be added to the beginning of the array. If M=N and n is not specified, we assume that the first row corresponds to the point to be localized. + :param completion: can be 'optspace' or 'alternate'. Algorithm to use for matrix completion. See pylocus.edm_completion for details. + :param mask: Optional mask of missing entries. + :param method: method to be used for MDS algorithm. See method "MDS" from pylocus.mds module for details. + """ + from .point_set import dm_from_edm from .mds import MDS - N = all_points.shape[0] d = all_points.shape[1] + N = edm.shape[0] + + n, all_points = complete_points(all_points, N) + if mask is not None: edm_missing = np.multiply(edm, mask) if completion == 'optspace': @@ -191,8 +229,8 @@ def reconstruct_mds(edm, all_points, completion='optspace', mask=None, method='g print('{}: relative error:{}'.format(completion, err)) edm = edm_complete Xhat = MDS(edm, d, method, False).T + assert (~np.isnan(Xhat)).all() Y, R, t, c = procrustes(all_points[n:], Xhat, True) - #Y, R, t, c = procrustes(all_points, Xhat, True) return Y @@ -206,11 +244,19 @@ def reconstruct_sdp(edm, all_points, W=None, print_out=False, lamda=1000, **kwar return Xhat, edm_complete -def reconstruct_srls(edm, all_points, W=None, print_out=False, n=1, rescale=False, - z=None): +def reconstruct_srls(edm, all_points, W=None, print_out=False, rescale=False, z=None): """ Reconstruct point set using S(quared)R(ange)L(east)S(quares) method. + + See reconstruct_mds for edm and all_points parameters. + + :param W: optional weights matrix, same dimension as edm. + :param rescale, z: optional parameters for SRLS. See SRLS documentation for explanation (module pylocus.lateration) """ from .lateration import SRLS, get_lateration_parameters + + N = edm.shape[0] + n, all_points = complete_points(all_points, N) + Y = all_points.copy() indices = range(n) for index in indices: @@ -222,7 +268,17 @@ def reconstruct_srls(edm, all_points, W=None, print_out=False, n=1, rescale=Fals print('w', w) print('r2', r2) - srls = SRLS(anchors, w, r2, rescale, z, print_out) + try: + srls = SRLS(anchors, w, r2, rescale, z, print_out) + if z is not None: + assert srls[2] == z + + except Exception as e: + print(e) + print("Something went wrong; probably bad geometry. (All anchors in the same plane, two distances are exactly the same, etc.)") + print("anchors, w, r2, z:", anchors, w, r2, z) + return None + if rescale: srls = srls[0] # second element of output is the scale Y[index, :] = srls @@ -377,3 +433,98 @@ def reconstruct_dwmds(edm, X0, W=None, n=None, r=None, X_bar=None, print_out=Fal if __name__ == "__main__": print('nothing happens when running this module.') + + +def reconstruct_aloc(Pi, Pj, i, j, theta_tensor, Pk='', k=-1, print_out=False): + from pylocus.point_set import AngleSet + from pylocus.basics_angles import get_absolute_angle + from pylocus.basics_angles import get_absolute_angles_tensor + from pylocus.basics_angles import from_0_to_2pi + from math import cos, sin + ''' + Returns the reconstruction of pointset from inner angles vector theta. If Pk and k are set, it corrects for the original flip. + Args: + Pi: point i of base line. + Pj: point j of base line. + i: index of point Pi. + j: index of point Pj. + theta: tensor containing inner angles + Pk: coordinates of point k (cannot be 0 for now.) + k: index of point Pk + Returns: + own: obtained AngleSet object + ''' + # Initialize to fix orientation + N = theta_tensor.shape[0] + own = AngleSet(N, Pi.shape[0]) + own.points[i, :] = Pi + own.points[j, :] = Pj + alpha_ij = get_absolute_angle(Pi, Pj) + alpha_ji = get_absolute_angle(Pj, Pi) + if (print_out): + print('alpha_ij, alpha_ji', 180 * np.array((alpha_ij, alpha_ji)) / pi) + + left_indices = np.delete(np.arange(N), (i, j)) + + # Get directed angles for both points. + __, alphas_tensor, __ = get_absolute_angles_tensor( + theta_tensor, theta_tensor, (i, j), N) + + # Correct for consistent flip. + if abs(alphas_tensor[j, j, k] - alpha_ji) < pi: + if (print_out): + print('flip', j) + alphas_tensor[j, :, :] *= -1 + if abs(alphas_tensor[i, i, k] - alpha_ij) < pi: + if (print_out): + print('flip', i) + alphas_tensor[i, :, :] *= -1 + if (print_out): + print('alphas_tensor', alphas_tensor * 180 / pi) + + # Correct for true flip. + alpha_ij = get_absolute_angle(Pi, Pj) + alphas_tensor[i, i, :] = from_0_to_2pi(alphas_tensor[i, j, :] + alpha_ij) + if k >= 0: + alpha_ik = get_absolute_angle(Pi, Pk) + calc_alpha_ik = alphas_tensor[i, i, k] + diff_calc = from_0_to_2pi(alpha_ij - calc_alpha_ik) + diff = from_0_to_2pi(alpha_ij - alpha_ik) + if diff_calc < pi and diff_calc > 0: + if not (diff < pi and diff > 0): + alphas_tensor *= -1.0 + else: + if not (diff > pi or diff < 0): + alphas_tensor *= -1.0 + + alpha_ji = get_absolute_angle(Pj, Pi) + alphas_tensor[i, i, :] = from_0_to_2pi(alphas_tensor[i, j, :] + alpha_ij) + alphas_tensor[:, range(N), range(N)] = 0.0 + alphas_tensor[j, j, :] = from_0_to_2pi(alphas_tensor[j, i, :] + alpha_ji) + alphas_tensor[:, range(N), range(N)] = 0.0 + alphas_tensor[i, :, i] = from_0_to_2pi(alphas_tensor[i, :, j] + alpha_ji) + alphas_tensor[:, range(N), range(N)] = 0.0 + alphas_tensor[j, :, j] = from_0_to_2pi(alphas_tensor[j, :, i] + alpha_ij) + alphas_tensor[:, range(N), range(N)] = 0.0 + + for k in left_indices: + alpha_ik = alphas_tensor[i, i, k] + alpha_jk = alphas_tensor[j, j, k] + if print_out and k >= 0: + real_alpha_ik = get_absolute_angle(Pi, Pk) + real_alpha_jk = get_absolute_angle(Pj, Pk) + print('real alpha_ik, alpha_jk', 180 * + np.array((real_alpha_ik, real_alpha_jk)) / pi) + print('calc alpha_ik, alpha_jk', 180 * + np.array((alpha_ik, alpha_jk)) / pi) + A = np.matrix([[cos(alpha_ik), -cos(alpha_jk)], + [sin(alpha_ik), -sin(alpha_jk)]]) + b = Pj - Pi + s = np.linalg.solve(A, b) + Pk_1 = Pi + s[0] * np.array([cos(alpha_ik), sin(alpha_ik)]) + Pk_2 = Pj + s[1] * np.array([cos(alpha_jk), sin(alpha_jk)]) + assert np.isclose(Pk_1, Pk_2).all() + own.points[k, :] = Pk_1 + own.init() + own.get_theta_tensor() + return own diff --git a/pylocus/basics.py b/pylocus/basics.py index cdf5e13..d201915 100644 --- a/pylocus/basics.py +++ b/pylocus/basics.py @@ -82,6 +82,10 @@ def eigendecomp(G, d): assert (lamda_sorted[ :d] > -1e-10).all(), "{} not all positive!".format(lamda_sorted[:d]) + # Set the small negative values of + # lamda to zero. + lamda_sorted[lamda_sorted < 0] = 0 + u = u[:, indices] factor = np.empty((N,), dtype=lamda.dtype) np.sqrt(lamda_sorted[:d], out=factor[0:d]) diff --git a/pylocus/basics_angles.py b/pylocus/basics_angles.py index 8205c2f..b44f7a9 100644 --- a/pylocus/basics_angles.py +++ b/pylocus/basics_angles.py @@ -58,6 +58,104 @@ def get_absolute_angle(Pi, Pj): return from_0_to_2pi(theta_ij) +def get_absolute_angles_tensor(thetas, thetas_noisy, ks, N, tol=1e-10, print_out=False): + ''' + Args: + thetas: Tensor of inner angles from point k to other points. (without noise) + thetas_noisy: Same as thetas, but with noise. + k: Point for which alpha is evaluated. + N: Number of points. + Returns: + + ''' + def get_alpha(alphas_k, corners_k, corner): + idx_old = [idx for idx, tria in enumerate(corners_k) if tria == corner] + assert len(idx_old) == 1, "idx_old:{}".format(idx_old) + idx_old = idx_old[0] + return alphas_k[idx_old] + corners_k = [] + alphas_k = [] + alphas_tensor = np.zeros(thetas.shape) + for k in ks: + other_indices = np.delete(range(N), k) + first = other_indices[0] + left_indices = other_indices[1:] + + # fix direction of first angle. (alpha_01) + second = left_indices[0] + corner_first = (k, first, second) + theta_01 = thetas[corner_first] + theta_01_noisy = thetas_noisy[corner_first] + alpha_01 = theta_01 + alpha_01_noisy = theta_01_noisy + alphas_k.append(alpha_01_noisy) + corners_k.append(corner_first) + alphas_tensor[corner_first] = alpha_01_noisy + alphas_tensor[k, second, first] = -alpha_01_noisy + if (print_out): + print('first: alpha_{}{}={}'.format(first, second, alphas_k[-1])) + + # find directions of other lines accordingly (alpha_0i) + left_indices = other_indices[2:] + for idx, i in enumerate(left_indices): + corner12 = (k, second, i) + theta_12 = thetas[corner12] + theta_12_noisy = thetas_noisy[corner12] + corner02 = (k, first, i) + theta_02 = thetas[corner02] + theta_02_noisy = thetas_noisy[corner02] + corner01 = (k, first, second) + theta_01 = thetas[corner01] + theta_01_noisy = thetas_noisy[corner01] + if (print_out): + print(' theta_{}{}={}'.format(first, second, theta_01)) + print(' theta_{}{}={}'.format(first, i, theta_02)) + print(' theta_{}{}={}'.format(second, i, theta_12)) + if abs(from_0_to_pi(theta_01 + theta_02) - theta_12) < tol: + alpha_02_noisy = -theta_02_noisy + alpha_02 = -theta_02 + else: + alpha_02_noisy = theta_02_noisy + alpha_02 = -theta_02 + alphas_k.append(alpha_02_noisy) + corners_k.append(corner02) + alphas_tensor[corner02] = alpha_02_noisy + alphas_tensor[k, i, first] = -alpha_02_noisy + if (print_out): + print('second alpha_{}{}={}'.format(first, i, alpha_02_noisy)) + + # find directions between all lines (alpha_ij) + left_indices = other_indices[1:] + for idx, i in enumerate(left_indices): + for j in left_indices[idx + 1:]: + corner0i = (k, first, i) + alpha_0i = get_alpha(alphas_k, corners_k, corner0i) + corner0j = (k, first, j) + alpha_0j = get_alpha(alphas_k, corners_k, corner0j) + alpha_ij = alpha_0j - alpha_0i + cornerij = (k, i, j) + theta_ij_noisy = thetas_noisy[cornerij] + if abs(alpha_ij) > pi: + if alpha_ij > 0: + alpha_ij_noisy = 2 * pi - theta_ij_noisy + else: + alpha_ij_noisy = theta_ij_noisy - 2 * pi + else: + if alpha_ij > 0: + alpha_ij_noisy = theta_ij_noisy + else: + alpha_ij_noisy = -theta_ij_noisy + alphas_k.append(alpha_ij_noisy) + corners_k.append(cornerij) + alphas_tensor[cornerij] = alpha_ij_noisy + alphas_tensor[k, j, i] = -alpha_ij_noisy + alphas_k = [from_0_to_2pi(alph) for alph in alphas_k] + corners_k = np.array(corners_k).reshape((-1, 3)) + alphas_ordered = alphas_k + alphas_tensor = from_0_to_2pi(alphas_tensor) + return alphas_ordered, alphas_tensor, corners_k + + def get_inner_angle(Pk, Pij): theta_ki = get_absolute_angle(Pk, Pij[0]) theta_kj = get_absolute_angle(Pk, Pij[1]) @@ -96,3 +194,11 @@ def get_theta_tensor(theta, corners, N): theta_tensor[int(idx[0]), int(idx[1]), int(idx[2])] = theta[k] theta_tensor[int(idx[0]), int(idx[2]), int(idx[1])] = theta[k] return theta_tensor + + +def get_index(corners, Pk, Pij): + ''' get index mask corresponding to angle at corner Pk with Pi, Pj.''' + angle1 = [Pk, Pij[0], Pij[1]] + angle2 = [Pk, Pij[1], Pij[0]] + index = np.bitwise_or(corners == angle1, corners == angle2) + return index.all(axis=1) diff --git a/pylocus/edm_completion.py b/pylocus/edm_completion.py index ea73812..ebd79a2 100644 --- a/pylocus/edm_completion.py +++ b/pylocus/edm_completion.py @@ -1,7 +1,11 @@ #!/usr/bin/env python # module EDM_COMPLETION import numpy as np -from cvxpy import * + +try: + from cvxpy import * +except: + print("WARNING from pylocs.edm_completion module: Failed to load cvxpy. This might lead to errors later on.") from pylocus.basics import get_edm diff --git a/pylocus/lateration.py b/pylocus/lateration.py index ec44866..365b620 100644 --- a/pylocus/lateration.py +++ b/pylocus/lateration.py @@ -3,10 +3,18 @@ import numpy as np from scipy.linalg import eigvals, eigvalsh -from cvxpy import * + +try: + from cvxpy import * +except: + print("WARNING from pylocs.lateration module: Failed to load cvxpy. This might lead to errors later on.") + from pylocus.basics import assert_print, assert_all_print +class GeometryError(Exception): + pass + def get_lateration_parameters(all_points, indices, index, edm, W=None): """ Get parameters relevant for lateration from full all_points, edm and W. @@ -42,24 +50,10 @@ def get_lateration_parameters(all_points, indices, index, edm, W=None): return anchors, w, r2 -def SRLS(anchors, w, r2, rescale=False, z=None, print_out=False): - '''Squared range least squares (SRLS) - - Algorithm written by A.Beck, P.Stoica in "Approximate and Exact solutions of Source Localization Problems". - - :param anchors: anchor points (Nxd) - :param w: weights for the measurements (Nx1) - :param r2: squared distances from anchors to point x. (Nx1) - :param rescale: Optional parameter. When set to True, the algorithm will - also identify if there is a global scaling of the measurements. Such a - situation arise for example when the measurement units of the distance is - unknown and different from that of the anchors locations (e.g. anchors are - in meters, distance in centimeters). - :param z: Optional parameter. Use to fix the z-coordinate of localized point. - :param print_out: Optional parameter, prints extra information. +def solve_GTRS(A, b, D, f): + from scipy import optimize + from scipy.linalg import sqrtm - :return: estimated position of point x. - ''' def y_hat(_lambda): lhs = ATA + _lambda * D assert A.shape[0] == b.shape[0] @@ -70,25 +64,73 @@ def y_hat(_lambda): try: return np.linalg.solve(lhs, rhs) except: - return np.zeros((lhs.shape[1],)) + # TODO: It was empirically found that it is essential that the default is + # not zero, but a small negative value. It is not clear why this is the case + # from a mathematical point of view. + return np.full((lhs.shape[1],), -1e-3) def phi(_lambda): yhat = y_hat(_lambda).reshape((-1, 1)) sol = np.dot(yhat.T, np.dot(D, yhat)) + 2 * np.dot(f.T, yhat) return sol.flatten() - def phi_prime(_lambda): - # TODO: test this. - B = np.linalg.inv(ATA + _lambda * D) - C = A.T.dot(b) - _lambda*f - y_prime = -B.dot(D.dot(B.dot(C)) - f) - y = y_hat(_lambda) - return 2*y.T.dot(D).dot(y_prime) + 2*f.T.dot(y_prime) + ATA = np.dot(A.T, A) - from scipy import optimize - from scipy.linalg import sqrtm + try: + eig = np.sort(np.real(eigvalsh(a=D, b=ATA))) + except: + raise GeometryError('Degenerate configuration.') + if np.abs(eig[-1]) < 1e-10: + lower_bound = -1e3 + else: + lower_bound = - 1.0 / eig[-1] + + inf = 1e5 + xtol = 1e-12 + + lambda_opt = 0 + # We will look for the zero of phi between lower_bound and inf. + # Therefore, the two have to be of different signs. + if (phi(lower_bound) > 0) and (phi(inf) < 0): + try: + # brentq is considered the best rootfinding routine. + lambda_opt, r = optimize.brentq(phi, lower_bound, inf, xtol=xtol, full_output=True) + assert r.converged + except: + print('SRLS error: brentq did not converge even though we found an estimate for lower and upper bonud. Setting lambda to 0.') + else: + try: + lambda_opt = optimize.newton(phi, lower_bound, maxiter=10000, tol=xtol) + assert phi(lambda_opt) < xtol, 'did not find solution of phi(lambda)=0:={}'.format(phi(lambda_opt)) + except AssertionError: + print('SRLS error: newton did not converge. Setting lambda to 0.') + + yhat = y_hat(lambda_opt) + return yhat + + +def SRLS(anchors, w, r2, rescale=False, z=None, print_out=False): + """ Squared range least squares (SRLS) + + Algorithm written by A.Beck, P.Stoica in "Approximate and Exact solutions of Source Localization Problems". + + :param anchors: anchor points (Nxd) + :param w: weights for the measurements (Nx1) + :param r2: squared distances from anchors to point x. (Nx1) + :param rescale: Optional parameter. When set to True, the algorithm will + also identify if there is a global scaling of the measurements. Such a + situation arise for example when the measurement units of the distance is + unknown and different from that of the anchors locations (e.g. anchors are + in meters, distance in centimeters). + :param z: Optional parameter. Use to fix the z-coordinate of localized point. + :param print_out: Optional parameter, prints extra information. + + :return: estimated position of point x. + """ n, d = anchors.shape + if type(r2) == list: + r2 = np.array(r2).reshape((-1, 1)) assert r2.shape[1] == 1 and r2.shape[0] == n, 'r2 has to be of shape Nx1' assert w.shape[1] == 1 and w.shape[0] == n, 'w has to be of shape Nx1' if z is not None: @@ -96,119 +138,92 @@ def phi_prime(_lambda): if rescale and z is not None: raise NotImplementedError('Cannot run rescale for fixed z.') + if rescale and n < d + 2: - raise ValueError( - 'A minimum of d + 2 ranges are necessary for rescaled ranging.') - elif n < d + 1 and z is None: - raise ValueError( - 'A minimum of d + 1 ranges are necessary for ranging.') - elif n < d: - raise ValueError( - 'A minimum of d ranges are necessary for ranging.') + raise ValueError('A minimum of d + 2 ranges are necessary for rescaled ranging.') + elif z is None and n < d + 1: + raise ValueError('A minimum of d + 1 ranges are necessary for ranging.') + elif z is not None and n < d: + raise ValueError('A minimum of d ranges are necessary for ranging.') + + if rescale: + return SRLS_rescale(anchors, w, r2, print_out) + + if z is not None: + return SRLS_fixed_z(anchors, w, r2, z) + + A = np.c_[-2 * anchors, np.ones((n, 1))] + b = r2 - np.power(np.linalg.norm(anchors, axis=1), 2).reshape(r2.shape) Sigma = np.diagflat(np.power(w, 0.5)) + A = Sigma.dot(A) + b = Sigma.dot(b) - if rescale: - A = np.c_[-2 * anchors, np.ones((n, 1)), -r2] - else: - if z is None: - A = np.c_[-2 * anchors, np.ones((n, 1))] - else: - A = np.c_[-2 * anchors[:, :2], np.ones((n, 1))] + D = np.zeros((d + 1, d + 1)) + D[:-1, :-1] = np.eye(D.shape[0]-1) - A = Sigma.dot(A) + f = np.c_[np.zeros((1, d)), -0.5].T - if rescale: - b = - np.power(np.linalg.norm(anchors, axis=1), 2).reshape(r2.shape) - else: - b = r2 - np.power(np.linalg.norm(anchors, axis=1), 2).reshape(r2.shape) - if z is not None: - b = b + 2 * anchors[:, 2].reshape((-1, 1)) * z - z**2 - b = Sigma.dot(b) + yhat = solve_GTRS(A, b, D, f) + return yhat[:d] - ATA = np.dot(A.T, A) - if rescale: - D = np.zeros((d + 2, d + 2)) - D[:d, :d] = np.eye(d) - else: - if z is None: - D = np.zeros((d + 1, d + 1)) - else: - D = np.zeros((d, d)) - D[:-1, :-1] = np.eye(D.shape[0]-1) - - if rescale: - f = np.c_[np.zeros((1, d)), -0.5, 0.].T - elif z is None: - f = np.c_[np.zeros((1, d)), -0.5].T - else: - f = np.c_[np.zeros((1, 2)), -0.5].T - - eig = np.sort(np.real(eigvalsh(a=D, b=ATA))) - if (print_out): - print('ATA:', ATA) - print('rank:', np.linalg.matrix_rank(A)) - print('eigvals:', eigvals(ATA)) - print('condition number:', np.linalg.cond(ATA)) - print('generalized eigenvalues:', eig) - - #eps = 0.01 - if eig[-1] > 1e-10: - lower_bound = - 1.0 / eig[-1] - else: - print('Warning: biggest eigenvalue is zero!') - lower_bound = -1e-5 +def SRLS_rescale(anchors, w, r2, print_out=False): + """ Helper routine for SRLS. """ + n, d = anchors.shape - inf = 1e5 - xtol = 1e-12 + A = np.c_[-2 * anchors, np.ones((n, 1)), -r2] + b = - np.power(np.linalg.norm(anchors, axis=1), 2).reshape(r2.shape) - lambda_opt = 0 - # We will look for the zero of phi between lower_bound and inf. - # Therefore, the two have to be of different signs. - if (phi(lower_bound) > 0) and (phi(inf) < 0): - # brentq is considered the best rootfinding routine. - try: - lambda_opt = optimize.brentq(phi, lower_bound, inf, xtol=xtol) - except: - print('SRLS error: brentq did not converge even though we found an estimate for lower and upper bonud. Setting lambda to 0.') - else: - try: - lambda_opt = optimize.newton(phi, lower_bound, fprime=phi_prime, maxiter=1000, tol=xtol, verbose=True) - assert phi(lambda_opt) < xtol, 'did not find solution of phi(lambda)=0:={}'.format(phi(lambda_opt)) - except: - print('SRLS error: newton did not converge. Setting lambda to 0.') + Sigma = np.diagflat(np.power(w, 0.5)) + A = Sigma.dot(A) + b = Sigma.dot(b) - if (print_out): - print('phi(lower_bound)', phi(lower_bound)) - print('phi(inf)', phi(inf)) - print('phi(lambda_opt)', phi(lambda_opt)) - pos_definite = ATA + lambda_opt*D - eig = np.sort(np.real(eigvals(pos_definite))) - print('should be strictly bigger than 0:', eig) + D = np.zeros((d + 2, d + 2)) + D[:d, :d] = np.eye(d) - # Compute best estimate - yhat = y_hat(lambda_opt) + f = np.c_[np.zeros((1, d)), -0.5, 0.].T - if print_out and rescale: + yhat = solve_GTRS(A, b, D, f) + + if print_out: print('Scaling factor :', yhat[-1]) + return yhat[:d], yhat[-1] - if rescale: - return yhat[:d], yhat[-1] - elif z is None: - return yhat[:d] - else: - return np.r_[yhat[0], yhat[1], z] + +def SRLS_fixed_z(anchors, w, r2, z): + """ Helper routine for SRLS. """ + n, d = anchors.shape + + Sigma = np.diagflat(np.power(w, 0.5)) + + A = np.c_[-2 * anchors[:, :2], np.ones((n, 1))] + + b = r2 - np.power(np.linalg.norm(anchors, axis=1), 2).reshape(r2.shape) + b = b + 2 * anchors[:, 2].reshape((-1, 1)) * z - z**2 + + A = Sigma.dot(A) + b = Sigma.dot(b) + + ATA = np.dot(A.T, A) + + D = np.zeros((d, d)) + D[:-1, :-1] = np.eye(D.shape[0]-1) + + f = np.c_[np.zeros((1, 2)), -0.5].T + + yhat = solve_GTRS(A, b, D, f) + return np.r_[yhat[0], yhat[1], z] def PozyxLS(anchors, W, r2, print_out=False): - ''' Algorithm used by pozyx (https://www.pozyx.io/Documentation/how_does_positioning_work) + """ Algorithm used by pozyx (https://www.pozyx.io/Documentation/how_does_positioning_work) :param anchors: anchor points :param r2: squared distances from anchors to point x. :returns: estimated position of point x. - ''' + """ N = anchors.shape[0] anchors_term = np.sum(np.power(anchors[:-1], 2), axis=1) last_term = np.sum(np.power(anchors[-1], 2)) @@ -223,7 +238,7 @@ def RLS(anchors, W, r, print_out=False, grid=None, num_points=10): Algorithm written by A.Beck, P.Stoica in "Approximate and Exact solutions of Source Localization Problems". - :param anchors: anchor points + :param anchors: anchor point coordinates, N x d :param r2: squared distances from anchors to point x. :param grid: where to search for solution. (min, max) where min and max are lists of d elements, d being the dimension of the setup. If not given, the diff --git a/pylocus/mds.py b/pylocus/mds.py index 54325bb..9108fed 100644 --- a/pylocus/mds.py +++ b/pylocus/mds.py @@ -1,7 +1,10 @@ #!/usr/bin/env python # module MDS import numpy as np -from cvxpy import * +try: + from cvxpy import * +except: + print("WARNING from pylocs.mds module: Failed to load cvxpy. This might lead to errors later on.") from pylocus.basics import eigendecomp @@ -20,7 +23,13 @@ def x_from_eigendecomp(factor, u, dim): def MDS(D, dim, method='simple', theta=False): - """ recover points from euclidean distance matrix using classic MDS algorithm. + """ Recover points from euclidean distance matrix using classic MDS algorithm. + + :param D: Euclidean distance matrix. + :param dim: dimension of points. + :param method: method to use. Can be either "simple", "advanced" or "geometric". + :param theta: set to True if using this function in a 1D-angle-setting. + """ N = D.shape[0] if method == 'simple': @@ -168,7 +177,3 @@ def signedMDS(cdm, W=None): x_est -= np.min(x_est) return x_est, A, np.linalg.pinv(A) - - -if __name__ == "__main__": - print('nothing happens when running this module.') diff --git a/pylocus/point_set.py b/pylocus/point_set.py index cace091..3b31aef 100644 --- a/pylocus/point_set.py +++ b/pylocus/point_set.py @@ -3,9 +3,13 @@ point-to-point distances and angles. """ +import itertools +from math import pi + import numpy as np + from pylocus.settings import * -from math import pi +from pylocus.basics_angles import get_index class PointSet: @@ -267,7 +271,6 @@ def create_theta(self): reconstructed from point coordinates. Also returns the corners corresponding to each entry of theta. """ - import itertools from pylocus.basics_angles import from_0_to_pi theta = np.empty((self.M, )) corners = np.empty((self.M, 3)) @@ -389,7 +392,7 @@ def get_closed_form(self, edm): return T def get_theta_tensor(self): - from pylocuspylocus.basics_angles import get_theta_tensor + from pylocus.basics_angles import get_theta_tensor self.theta_tensor = get_theta_tensor(self.theta, self.corners, self.N) return self.theta_tensor @@ -440,8 +443,10 @@ def get_G(self, k, add_noise=True): return G def reconstruct_from_inner_angles(self, theta): - from .algorithms import reconstruct_from_inner_angles - from .algorithms import procrustes + from pylocus.algorithms import reconstruct_from_inner_angles + from pylocus.algorithms import procrustes + from pylocus.basics_angles import get_theta_tensor + theta_tensor = get_theta_tensor(theta, self.corners, self.N) reconstruction = reconstruct_from_inner_angles( self.points[0, :], self.points[1, :], self.abs_angles[0, 2], @@ -452,8 +457,9 @@ def reconstruct_from_inner_angles(self, theta): reconstruction.init() return reconstruction - def reconstruct(self, theta): - from .algorithms import reconstruct + def reconstruct_aloc(self, theta): + from pylocus.algorithms import reconstruct_aloc + from pylocus.basics_angles import get_theta_tensor i = 0 j = 1 theta_tensor = get_theta_tensor(theta, self.corners, self.N) @@ -461,7 +467,7 @@ def reconstruct(self, theta): Pj = self.points[j, :] k = 2 Pk = self.points[k, :] - reconstruction = reconstruct(Pi, Pj, i, j, theta_tensor, Pk, k) + reconstruction = reconstruct_aloc(Pi, Pj, i, j, theta_tensor, Pk, k) return reconstruction def get_convex_polygons(self, m, print_out=False): From 033f7a47de78f3be28194d9dd7708e0cfa11c942 Mon Sep 17 00:00:00 2001 From: Frederike Date: Wed, 23 Oct 2019 11:23:09 +0200 Subject: [PATCH 6/9] Deleted aloc.py file because it is not useful anymore. --- pylocus/aloc.py | 86 ------------------------------------------------- 1 file changed, 86 deletions(-) delete mode 100644 pylocus/aloc.py diff --git a/pylocus/aloc.py b/pylocus/aloc.py deleted file mode 100644 index 500982b..0000000 --- a/pylocus/aloc.py +++ /dev/null @@ -1,86 +0,0 @@ -#!/usr/bin/env python -# module A-LOC -from algorithm_constraints import get_all_constraints, projection -from basics_alternating import get_absolute_angles_tensor -from basics_aloc import from_0_to_2pi, get_point, get_inner_angle, get_absolute_angle, get_theta_tensor -from math import pi, floor, cos, sin -from plots import plot_theta_errors, plot_point_sets -from .basics import rmse -import numpy as np - - -def reconstruct(Pi, Pj, i, j, theta_tensor, Pk='', k=-1, print_out=False): - ''' - Returns the reconstruction of pointset from inner angles vector theta. If Pk and k are set, it corrects for the original flip. - Args: - Pi: point i of base line. - Pj: point j of base line. - i: index of point Pi. - j: index of point Pj. - theta: tensor containing inner angles - Pk: coordinates of point k (cannot be 0 for now.) - k: index of point Pk - Returns: - own: obtained AngleSet object - ''' - from .algorithms import procrustes, apply_transform - from .point_set import create_from_points, AngleSet - from .basics_angles import get_inner_angle - - def intersect(Pi, Pj, alpha_ik, alpha_jk): - A = np.matrix([[cos(alpha_ik), -cos(alpha_jk)], - [sin(alpha_ik), -sin(alpha_jk)]]) - s = np.linalg.solve(A, Pj) - P2 = s[0] * np.array([cos(alpha_ik), sin(alpha_ik)]) - P2_same = Pj + s[1] * np.array([cos(alpha_jk), sin(alpha_jk)]) - assert np.isclose(P2, P2_same).all() - - theta = get_inner_angle(P0, [P1, P2]) - - # TODO not robust to multiples of pi. - #assert abs(theta - theta_tensor[i, j, l]) < 1e-10, '{}-{} not smaller than 1e-10'.format( - # degrees(theta), degrees(theta_tensor[i, j, l])) - theta = get_inner_angle(P1, [P0, P2]) - #assert abs(theta - theta_tensor[j, i, l]) < 1e-10, '{}-{} not smaller than 1e-10'.format( - # degrees(theta), degrees(theta_tensor[j, i, l])) - return P2 - - # Initialize to fix orientation - N = theta_tensor.shape[0] - - points = np.zeros((N, Pi.shape[0])) - - P0 = np.array((0, 0)) - P1 = np.array((Pj[0] - Pi[0], 0)) - - points[i, :] = P0 - points[j, :] = P1 - left_indices = np.delete(range(N), (i, j)) - - first = True - for counter, l in enumerate(left_indices): - alpha_il = theta_tensor[i, j, l] - alpha_jl = pi - theta_tensor[j, i, l] - Pl = intersect(P0, P1, alpha_il, alpha_jl) - if not first: - try: - previous = left_indices[counter - 1] - estimated = get_inner_angle(points[previous, :], (P0, Pl)) - expected = theta_tensor[previous, i, l] - assert abs( - estimated - expected) < 0.1, 'error {}'.format(abs(estimated - expected)) - except: - alpha_il = -alpha_il - alpha_jl = -alpha_jl - Pl = intersect(P0, P1, alpha_il, alpha_jl) - points[l, :] = Pl - first = False - anchors = np.r_[Pi,Pj,Pk].reshape(-1,2) - __, R, t, c = procrustes(anchors, points[[i,j,k],:], scale=False) - fitted = apply_transform(R, t, c, points, anchors) - own = create_from_points(fitted, AngleSet) - return own - - -def degrees(angle): - return 180 * angle / pi From 7e9f835195e81eee8dea03f78edf7cbf754cdba5 Mon Sep 17 00:00:00 2001 From: duembgen Date: Fri, 8 Nov 2019 11:45:14 +0100 Subject: [PATCH 7/9] Fixed cvxpy requirement. --- .travis.yml | 1 - optional_requirements.txt | 2 -- pylocus/edm_completion.py | 23 ++++++++++------------- pylocus/lateration.py | 16 ++++++---------- pylocus/mds.py | 12 +++++------- pylocus/point_set.py | 10 +++++----- requirements.txt | 6 ++++-- 7 files changed, 30 insertions(+), 40 deletions(-) delete mode 100644 optional_requirements.txt diff --git a/.travis.yml b/.travis.yml index 8df6985..539253b 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,7 +5,6 @@ python: install: - pip install -r requirements.txt - - pip install -r optional_requirements.txt script: - pytest diff --git a/optional_requirements.txt b/optional_requirements.txt deleted file mode 100644 index 596bcca..0000000 --- a/optional_requirements.txt +++ /dev/null @@ -1,2 +0,0 @@ -cvxpy >= 1.0.6 -cvxopt >= 1.1.9 diff --git a/pylocus/edm_completion.py b/pylocus/edm_completion.py index ebd79a2..9ee228c 100644 --- a/pylocus/edm_completion.py +++ b/pylocus/edm_completion.py @@ -2,10 +2,7 @@ # module EDM_COMPLETION import numpy as np -try: - from cvxpy import * -except: - print("WARNING from pylocs.edm_completion module: Failed to load cvxpy. This might lead to errors later on.") +import cvxpy as cp from pylocus.basics import get_edm @@ -94,7 +91,7 @@ def kappa(gram): def kappa_cvx(gram, n): e = np.ones((n, 1)) - return reshape(diag(gram), (n, 1)) * e.T + e * reshape(diag(gram), (1, n)) - 2 * gram + return cp.reshape(cp.diag(gram), (n, 1)) * e.T + e * cp.reshape(cp.diag(gram), (1, n)) - 2 * gram method = kwargs.pop('method', 'maximize') options = {'solver': 'CVXOPT'} @@ -109,19 +106,19 @@ def kappa_cvx(gram, n): V = np.c_[-np.ones((n - 1, 1)) / np.sqrt(n), np.eye(n - 1) - np.ones((n - 1, n - 1)) / (n + np.sqrt(n))].T - H = Variable((n - 1, n - 1), PSD=True) + H = cp.Variable((n - 1, n - 1), PSD=True) G = V * H * V.T # * is overloaded edm_optimize = kappa_cvx(G, n) if method == 'maximize': - obj = Maximize(trace(H) - lamda * - norm(multiply(W, (edm_optimize - edm_missing)), p=1)) + obj = cp.Maximize(cp.trace(H) - lamda * + cp.norm(cp.multiply(W, (edm_optimize - edm_missing)), p=1)) # TODO: add a reference to paper where "minimize" is used instead of maximize. elif method == 'minimize': - obj = Minimize(trace(H) + lamda * - norm(multiply(W, (edm_optimize - edm_missing)), p=1)) + obj = cp.Minimize(cp.trace(H) + lamda * + norm(cp.multiply(W, (edm_optimize - edm_missing)), p=1)) - prob = Problem(obj) + prob = cp.Problem(obj) total = prob.solve(**options) if print_out: @@ -138,9 +135,9 @@ def kappa_cvx(gram, n): if (print_out): if H.value is not None: - print('trace of H:', np.trace(H.value)) + print('cp.trace of H:', cp.trace(H.value)) print('other cost:', lamda * - norm(multiply(W, (edm_complete - edm_missing)), p=1).value) + norm(cp.multiply(W, (edm_complete - edm_missing)), p=1).value) return np.array(edm_complete) diff --git a/pylocus/lateration.py b/pylocus/lateration.py index 365b620..e9d64c6 100644 --- a/pylocus/lateration.py +++ b/pylocus/lateration.py @@ -4,11 +4,7 @@ import numpy as np from scipy.linalg import eigvals, eigvalsh -try: - from cvxpy import * -except: - print("WARNING from pylocs.lateration module: Failed to load cvxpy. This might lead to errors later on.") - +import cvxpy as cp from pylocus.basics import assert_print, assert_all_print @@ -302,8 +298,8 @@ def RLS_SDR(anchors, W, r, print_out=False): m = anchors.shape[0] d = anchors.shape[1] - G = Variable(m + 1, m + 1) - X = Variable(d + 1, d + 1) + G = cp.Variable(m + 1, m + 1) + X = cp.Variable(d + 1, d + 1) constraints = [G[m, m] == 1.0, X[d, d] == 1.0, G >> 0, X >> 0, @@ -313,10 +309,10 @@ def RLS_SDR(anchors, W, r, print_out=False): Ci[:-1, -1] = -anchors[i] Ci[-1, :-1] = -anchors[i].T Ci[-1, -1] = np.linalg.norm(anchors[i])**2 - constraints.append(G[i, i] == trace(Ci * X)) + constraints.append(G[i, i] == cp.trace(Ci * X)) - obj = Minimize(trace(G) - 2 * sum_entries(mul_elemwise(r, G[m, :-1].T))) - prob = Problem(obj, constraints) + obj = cp.Minimize(cp.trace(G) - 2 * cp.sum_entries(cp.mul_elemwise(r, G[m, :-1].T))) + prob = cp.Problem(obj, constraints) ## Solution total = prob.solve(verbose=True) diff --git a/pylocus/mds.py b/pylocus/mds.py index 9108fed..013c638 100644 --- a/pylocus/mds.py +++ b/pylocus/mds.py @@ -1,10 +1,8 @@ #!/usr/bin/env python # module MDS import numpy as np -try: - from cvxpy import * -except: - print("WARNING from pylocs.mds module: Failed to load cvxpy. This might lead to errors later on.") + +import cvxpy as cp from pylocus.basics import eigendecomp @@ -122,12 +120,12 @@ def relaxedEMDS(X0, N, d, C, b, KE, print_out=False, lamda=10): """ Find the set of points from an edge kernel with geometric constraints, using convex rank relaxation. """ E = C.shape[1] - X = Variable((E, E), PSD=True) + X = cp.Variable((E, E), PSD=True) constraints = [C[i, :] * X == b[i] for i in range(C.shape[0])] - obj = Minimize(trace(X) + lamda * norm(KE - X)) - prob = Problem(obj, constraints) + obj = cp.Minimize(cp.trace(X) + lamda * cp.norm(KE - X)) + prob = cp.Problem(obj, constraints) try: # CVXOPT is more accurate than SCS, even though slower. diff --git a/pylocus/point_set.py b/pylocus/point_set.py index 8c98f87..f018722 100644 --- a/pylocus/point_set.py +++ b/pylocus/point_set.py @@ -90,10 +90,10 @@ def set_points(self, mode='', points=None, range_=RANGE, size=1): self.points[other, :] + alpha * u + beta * v) #check if new direction lies between u and v. new_direction = self.points[-1, :] - self.points[other, :] - new_direction = new_direction.reshape( + new_direction = new_direction.cp.reshape( (-1, )) / np.linalg.norm(new_direction) - u = u.reshape((-1, )) / np.linalg.norm(u) - v = v.reshape((-1, )) / np.linalg.norm(v) + u = u.cp.reshape((-1, )) / np.linalg.norm(u) + v = v.cp.reshape((-1, )) / np.linalg.norm(v) #print('{} + {} = {}'.format(acos(np.dot(new_direction,u)),acos(np.dot(new_direction,v)),acos(np.dot(u,v)))) if abs( acos(np.dot(new_direction, u)) + @@ -607,13 +607,13 @@ def get_polygon_constraints_m(self, polygons_m, print_out=False): b = np.hstack(rows_b) num_constraints = A.shape[0] A_repeat = np.repeat(A.astype(bool), 3).reshape((1, -1)) - corners = self.corners.reshape((1, -1)) + corners = self.corners.cp.reshape((1, -1)) corners_tiled = np.tile(corners, num_constraints) if (print_out): print('shape of A {}'.format(A.shape)) if (print_out): print('chosen angles m={}:\n{}'.format(m, (corners_tiled)[A_repeat] - .reshape((-1, m * 3)))) + .cp.reshape((-1, m * 3)))) if (print_out): print('{}-polygones: {}'.format(m, rows_A)) self.A = A diff --git a/requirements.txt b/requirements.txt index 51aa5e0..e2b54ea 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,2 +1,4 @@ -numpy >= 1.14.3 -matplotlib >= 3.0.2 +numpy>=1.14.3 +matplotlib>=3.0.2 +cvxpy==1.0.25 +cvxopt==1.2.3 From b988a14ef9c775e820eecebe6605cbc9940aeb30 Mon Sep 17 00:00:00 2001 From: duembgen Date: Fri, 8 Nov 2019 14:25:49 +0100 Subject: [PATCH 8/9] Removed unused angle functionalities. --- CHANGELOG.md | 8 + CONTRIBUTE.rst => CONTRIBUTE.md | 13 +- README.rst => README.md | 51 ++-- docs/source/conf.py | 2 +- pylocus/__init__.py | 2 +- pylocus/algorithms.py | 95 ------- pylocus/basics_angles.py | 98 -------- pylocus/point_set.py | 431 +------------------------------- setup.py | 2 +- 9 files changed, 48 insertions(+), 654 deletions(-) rename CONTRIBUTE.rst => CONTRIBUTE.md (82%) rename README.rst => README.md (51%) diff --git a/CHANGELOG.md b/CHANGELOG.md index 65e0574..3e21f73 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,14 @@ ## Unreleased +## [0.0.5] - 2019-11-08 +### Changed + +- Remove unused angle algorithms, keeping only basic angle operations. The angle-based algorithms are a niche application and are now in a different respository. + +- Make cvxpy a compulsory dependence. + + ## [0.0.4] - 2019-09-12 ### Changed diff --git a/CONTRIBUTE.rst b/CONTRIBUTE.md similarity index 82% rename from CONTRIBUTE.rst rename to CONTRIBUTE.md index 5dc9a7a..1a9dc14 100644 --- a/CONTRIBUTE.rst +++ b/CONTRIBUTE.md @@ -1,10 +1,9 @@ -How to contribute -================= +# How to contribute -Release new version -------------------- +## Release new version 1. Change the version number in +- pylocus/__init__.py - setup.py - docs/source/conf.py @@ -12,13 +11,17 @@ Release new version 3. Run the following commands +```bash python3 setup.py sdist bdist_wheel python3 -m twine upload --repository-url https://test.pypi.org/legacy/ dist/* python3 -m pip install --index-url https://test.pypi.org/simple/ --no-deps example-pkg-your-username python3 -c "import pylocus.lateration" # test that it worked. +``` 4. If all looks ok, then run - python3 -m twine upload dist/* +``` +python3 -m twine upload dist/* +``` 5. If above does not work, make sure that the information in ~/.pypirc is up to date. diff --git a/README.rst b/README.md similarity index 51% rename from README.rst rename to README.md index b88a991..51690fe 100644 --- a/README.rst +++ b/README.md @@ -1,17 +1,9 @@ -Welcome to pylocus -================== - -.. image:: https://travis-ci.org/LCAV/pylocus.svg?branch=master - :target: https://travis-ci.org/LCAV/pylocus - -Python Localization Package ---------------------------- - +# Welcome to pylocus, a python localization package +[![Build Status](https://travis-ci.org/LCAV/pylocus.svg?branch=master)](https://travis-ci.org/LCAV/pylocus) This package contains Multidimensional Scaling, lateration, and other algorithms useful for localization using distances and/or angles. -Local install -************* +## Local install Since this package is in an early development phase, this is the recommended install method. The latest updates will not always be immediately available on pip, they will be bundled @@ -20,42 +12,31 @@ regularly, you can be sure to be using the latest version. To perform a local install on your computer, run from this folder level (where setup.py is located): -.. code-block:: bash +```bash - pip install -e . +pip install -e . + +``` This installs the package using symbolic links, avoiding the need for a reinstall whenever the source code is changed. If you use conda, then -.. code-block:: bash - - conda develop . +```bash +conda develop . +``` does the same trick. -Install -******* - -To install from pip, simply run : +## Install -.. code-block:: bash +To install from [pip](https://pypi.python.org/pypi/pylocus), simply run : +```bash pip install pylocus +``` -PyPi link : https://pypi.python.org/pypi/pylocus +## Documentation -Requirements -************ - -Depending on which parts of the project you are using, you might need to install more heavy requirements such as cvxpy and cxopt. These are not included in the default install and can be installed by running the following line. - -.. code-block:: bash - - pip install -r optional_requirements.txt - -Documentation -************* +This is a constantly growing package and documentation is work-in-progress. The current version can be found on [ReadTheDocs](http://pylocus.readthedocs.org/en/latest/) See the tutorials folder for some exmaple scripts on how to use this package. More scripts will be added soon. - -This is a constantly growing package and documentation is work-in-progress. The current version can be found on ReadTheDocs: http://pylocus.readthedocs.org/en/latest/ diff --git a/docs/source/conf.py b/docs/source/conf.py index 7d9a8cd..d99c2e5 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -94,7 +94,7 @@ # The short X.Y version. version = u'0.0' # The full version, including alpha/beta/rc tags. -release = u'0.0.4' +release = u'0.0.5' # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. diff --git a/pylocus/__init__.py b/pylocus/__init__.py index e49a82e..b1a19e3 100644 --- a/pylocus/__init__.py +++ b/pylocus/__init__.py @@ -1 +1 @@ -__version__ = "0.0.5dev" +__version__ = "0.0.5" diff --git a/pylocus/algorithms.py b/pylocus/algorithms.py index c86242b..1ede8fc 100644 --- a/pylocus/algorithms.py +++ b/pylocus/algorithms.py @@ -433,98 +433,3 @@ def reconstruct_dwmds(edm, X0, W=None, n=None, r=None, X_bar=None, print_out=Fal if __name__ == "__main__": print('nothing happens when running this module.') - - -def reconstruct_aloc(Pi, Pj, i, j, theta_tensor, Pk='', k=-1, print_out=False): - from pylocus.point_set import AngleSet - from pylocus.basics_angles import get_absolute_angle - from pylocus.basics_angles import get_absolute_angles_tensor - from pylocus.basics_angles import from_0_to_2pi - from math import cos, sin - ''' - Returns the reconstruction of pointset from inner angles vector theta. If Pk and k are set, it corrects for the original flip. - Args: - Pi: point i of base line. - Pj: point j of base line. - i: index of point Pi. - j: index of point Pj. - theta: tensor containing inner angles - Pk: coordinates of point k (cannot be 0 for now.) - k: index of point Pk - Returns: - own: obtained AngleSet object - ''' - # Initialize to fix orientation - N = theta_tensor.shape[0] - own = AngleSet(N, Pi.shape[0]) - own.points[i, :] = Pi - own.points[j, :] = Pj - alpha_ij = get_absolute_angle(Pi, Pj) - alpha_ji = get_absolute_angle(Pj, Pi) - if (print_out): - print('alpha_ij, alpha_ji', 180 * np.array((alpha_ij, alpha_ji)) / pi) - - left_indices = np.delete(np.arange(N), (i, j)) - - # Get directed angles for both points. - __, alphas_tensor, __ = get_absolute_angles_tensor( - theta_tensor, theta_tensor, (i, j), N) - - # Correct for consistent flip. - if abs(alphas_tensor[j, j, k] - alpha_ji) < pi: - if (print_out): - print('flip', j) - alphas_tensor[j, :, :] *= -1 - if abs(alphas_tensor[i, i, k] - alpha_ij) < pi: - if (print_out): - print('flip', i) - alphas_tensor[i, :, :] *= -1 - if (print_out): - print('alphas_tensor', alphas_tensor * 180 / pi) - - # Correct for true flip. - alpha_ij = get_absolute_angle(Pi, Pj) - alphas_tensor[i, i, :] = from_0_to_2pi(alphas_tensor[i, j, :] + alpha_ij) - if k >= 0: - alpha_ik = get_absolute_angle(Pi, Pk) - calc_alpha_ik = alphas_tensor[i, i, k] - diff_calc = from_0_to_2pi(alpha_ij - calc_alpha_ik) - diff = from_0_to_2pi(alpha_ij - alpha_ik) - if diff_calc < pi and diff_calc > 0: - if not (diff < pi and diff > 0): - alphas_tensor *= -1.0 - else: - if not (diff > pi or diff < 0): - alphas_tensor *= -1.0 - - alpha_ji = get_absolute_angle(Pj, Pi) - alphas_tensor[i, i, :] = from_0_to_2pi(alphas_tensor[i, j, :] + alpha_ij) - alphas_tensor[:, range(N), range(N)] = 0.0 - alphas_tensor[j, j, :] = from_0_to_2pi(alphas_tensor[j, i, :] + alpha_ji) - alphas_tensor[:, range(N), range(N)] = 0.0 - alphas_tensor[i, :, i] = from_0_to_2pi(alphas_tensor[i, :, j] + alpha_ji) - alphas_tensor[:, range(N), range(N)] = 0.0 - alphas_tensor[j, :, j] = from_0_to_2pi(alphas_tensor[j, :, i] + alpha_ij) - alphas_tensor[:, range(N), range(N)] = 0.0 - - for k in left_indices: - alpha_ik = alphas_tensor[i, i, k] - alpha_jk = alphas_tensor[j, j, k] - if print_out and k >= 0: - real_alpha_ik = get_absolute_angle(Pi, Pk) - real_alpha_jk = get_absolute_angle(Pj, Pk) - print('real alpha_ik, alpha_jk', 180 * - np.array((real_alpha_ik, real_alpha_jk)) / pi) - print('calc alpha_ik, alpha_jk', 180 * - np.array((alpha_ik, alpha_jk)) / pi) - A = np.matrix([[cos(alpha_ik), -cos(alpha_jk)], - [sin(alpha_ik), -sin(alpha_jk)]]) - b = Pj - Pi - s = np.linalg.solve(A, b) - Pk_1 = Pi + s[0] * np.array([cos(alpha_ik), sin(alpha_ik)]) - Pk_2 = Pj + s[1] * np.array([cos(alpha_jk), sin(alpha_jk)]) - assert np.isclose(Pk_1, Pk_2).all() - own.points[k, :] = Pk_1 - own.init() - own.get_theta_tensor() - return own diff --git a/pylocus/basics_angles.py b/pylocus/basics_angles.py index b44f7a9..dab61e3 100644 --- a/pylocus/basics_angles.py +++ b/pylocus/basics_angles.py @@ -58,104 +58,6 @@ def get_absolute_angle(Pi, Pj): return from_0_to_2pi(theta_ij) -def get_absolute_angles_tensor(thetas, thetas_noisy, ks, N, tol=1e-10, print_out=False): - ''' - Args: - thetas: Tensor of inner angles from point k to other points. (without noise) - thetas_noisy: Same as thetas, but with noise. - k: Point for which alpha is evaluated. - N: Number of points. - Returns: - - ''' - def get_alpha(alphas_k, corners_k, corner): - idx_old = [idx for idx, tria in enumerate(corners_k) if tria == corner] - assert len(idx_old) == 1, "idx_old:{}".format(idx_old) - idx_old = idx_old[0] - return alphas_k[idx_old] - corners_k = [] - alphas_k = [] - alphas_tensor = np.zeros(thetas.shape) - for k in ks: - other_indices = np.delete(range(N), k) - first = other_indices[0] - left_indices = other_indices[1:] - - # fix direction of first angle. (alpha_01) - second = left_indices[0] - corner_first = (k, first, second) - theta_01 = thetas[corner_first] - theta_01_noisy = thetas_noisy[corner_first] - alpha_01 = theta_01 - alpha_01_noisy = theta_01_noisy - alphas_k.append(alpha_01_noisy) - corners_k.append(corner_first) - alphas_tensor[corner_first] = alpha_01_noisy - alphas_tensor[k, second, first] = -alpha_01_noisy - if (print_out): - print('first: alpha_{}{}={}'.format(first, second, alphas_k[-1])) - - # find directions of other lines accordingly (alpha_0i) - left_indices = other_indices[2:] - for idx, i in enumerate(left_indices): - corner12 = (k, second, i) - theta_12 = thetas[corner12] - theta_12_noisy = thetas_noisy[corner12] - corner02 = (k, first, i) - theta_02 = thetas[corner02] - theta_02_noisy = thetas_noisy[corner02] - corner01 = (k, first, second) - theta_01 = thetas[corner01] - theta_01_noisy = thetas_noisy[corner01] - if (print_out): - print(' theta_{}{}={}'.format(first, second, theta_01)) - print(' theta_{}{}={}'.format(first, i, theta_02)) - print(' theta_{}{}={}'.format(second, i, theta_12)) - if abs(from_0_to_pi(theta_01 + theta_02) - theta_12) < tol: - alpha_02_noisy = -theta_02_noisy - alpha_02 = -theta_02 - else: - alpha_02_noisy = theta_02_noisy - alpha_02 = -theta_02 - alphas_k.append(alpha_02_noisy) - corners_k.append(corner02) - alphas_tensor[corner02] = alpha_02_noisy - alphas_tensor[k, i, first] = -alpha_02_noisy - if (print_out): - print('second alpha_{}{}={}'.format(first, i, alpha_02_noisy)) - - # find directions between all lines (alpha_ij) - left_indices = other_indices[1:] - for idx, i in enumerate(left_indices): - for j in left_indices[idx + 1:]: - corner0i = (k, first, i) - alpha_0i = get_alpha(alphas_k, corners_k, corner0i) - corner0j = (k, first, j) - alpha_0j = get_alpha(alphas_k, corners_k, corner0j) - alpha_ij = alpha_0j - alpha_0i - cornerij = (k, i, j) - theta_ij_noisy = thetas_noisy[cornerij] - if abs(alpha_ij) > pi: - if alpha_ij > 0: - alpha_ij_noisy = 2 * pi - theta_ij_noisy - else: - alpha_ij_noisy = theta_ij_noisy - 2 * pi - else: - if alpha_ij > 0: - alpha_ij_noisy = theta_ij_noisy - else: - alpha_ij_noisy = -theta_ij_noisy - alphas_k.append(alpha_ij_noisy) - corners_k.append(cornerij) - alphas_tensor[cornerij] = alpha_ij_noisy - alphas_tensor[k, j, i] = -alpha_ij_noisy - alphas_k = [from_0_to_2pi(alph) for alph in alphas_k] - corners_k = np.array(corners_k).reshape((-1, 3)) - alphas_ordered = alphas_k - alphas_tensor = from_0_to_2pi(alphas_tensor) - return alphas_ordered, alphas_tensor, corners_k - - def get_inner_angle(Pk, Pij): theta_ki = get_absolute_angle(Pk, Pij[0]) theta_kj = get_absolute_angle(Pk, Pij[1]) diff --git a/pylocus/point_set.py b/pylocus/point_set.py index f018722..373138c 100644 --- a/pylocus/point_set.py +++ b/pylocus/point_set.py @@ -2,14 +2,12 @@ """ Module containing classes for handling 2D or 3D point sets including point-to-point distances and angles. """ - import itertools from math import pi import numpy as np from pylocus.settings import * -from pylocus.basics_angles import get_index class PointSet: @@ -19,7 +17,6 @@ class PointSet: :param self.d: Dimension or points (typically 2 or 3). :param self.points: Matrix of points (self.N x self.d). :param self.edm: Matrix (self.Nx self.N) of squared distances (Euclidean distance matrix). - :param self.abs_angles: Matrix (self.N x self.N) of absolute angles. Element (i,j) corresponds to absolute angle from origin to ray from point i to point j. """ def __init__(self, N, d): @@ -28,18 +25,17 @@ def __init__(self, N, d): self.points = np.empty([self.N, self.d]) self.edm = np.empty([self.N, self.N]) + def copy(self): new = PointSet(self.N, self.d) new.points = self.points.copy() - # new.theta = self.theta.copy() - # new.corners = self.corners.copy() - # new.abs_angles = self.abs_angles.copy() new.edm = self.edm.copy() return new + def init(self): self.create_edm() - self.create_abs_angles() + def add_noise(self, noise, indices=None): if indices is None: @@ -47,6 +43,7 @@ def add_noise(self, noise, indices=None): self.points = return_noisy_points(noise, indices, self.points.copy()) self.init() + def set_points(self, mode='', points=None, range_=RANGE, size=1): """ Initialize points according to predefined modes. @@ -187,19 +184,13 @@ def set_points(self, mode='', points=None, range_=RANGE, size=1): self.init() + def create_edm(self): from pylocus.basics import get_edm self.edm = get_edm(self.points) - def plot_all(self, title='', size=[5, 2], filename='', axis='off'): - from .plots_cti import plot_points - plot_points(self.points, title, size, filename, axis) - - def plot_some(self, range_, title='', size=[5, 2]): - from .plots_cti import plot_points - plot_points(self.points[range_, :], title, size) - def create_abs_angles(self): + def get_abs_angles(self): from pylocus.basics_angles import get_absolute_angle abs_angles = np.empty((self.N, self.N)) for i in range(self.N): @@ -213,412 +204,16 @@ def create_abs_angles(self): "Angles do not add up to pi: %r-%r=%r" % \ (abs_angles[i, j], abs_angles[j, i], abs_angles[i, j] - abs_angles[j, i]) - self.abs_angles = abs_angles - - -class AngleSet(PointSet): - """ Class containing absolute/relative angles and linear constraints. + return abs_angles - :param self.theta: Vector of inner angles. - :param self.corners: Matrix of corners corresponding to inner angles. Row (k,i,j) corresponds to theta_k(i,j). - :param self.T: Number of triangles. - :param self.M: Number of inner angles. - :param self.C: Number of linear constraints. - :param self.A: Matrix of constraints (self.C x self.M) - :param self.b: Vector of constraints (self.C x 1) - """ - def __init__(self, N, d): - from scipy import special - PointSet.__init__(self, N, d) - self.T = self.N * (self.N - 1) * (self.N - 2) / 6 - self.M = int(3 * self.T) - self.theta = np.empty([self.M, ]) - self.corners = np.empty([self.M, 3]) - self.abs_angles = np.empty([self.N, self.N]) - self.C = 1 - self.A = np.empty((self.C, self.M)) - self.b = np.empty((self.C, 1)) - - def init(self): - PointSet.init(self) - self.create_theta() - - def create_abs_angles_from_edm(self): - rows, cols = np.indices((self.N, self.N)) - pi_pj_x = (self.points[rows, 0] - self.points[cols, 0]) - pi_pj_y = (self.points[rows, 1] - self.points[cols, 1]) - D = np.sqrt( - np.sum((self.points[rows, :] - self.points[cols, :])**2, axis=2)) - cosine = np.ones([self.N, self.N]) - sine = np.zeros([self.N, self.N]) - cosine[D > 0] = pi_pj_x[D > 0] / D[D > 0] - sine[D > 0] = pi_pj_y[D > 0] / D[D > 0] - Dc = acos(cosine) - for i in range(Dc.shape[0]): - for j in range(Dc.shape[0]): - if cosine[i, j] < 0 and sine[i, j] < 0: - # angle between pi and 3pi/2 - Dc[i, j] = 2 * pi - Dc[i, j] - if cosine[i, j] > 0 and sine[i, j] < 0: - # angle between 3pi/2 and 2pi - Dc[i, j] = 2 * pi - Dc[i, j] - self.abs_angles = Dc - - def create_theta(self): - """ - Returns the set of inner angles (between 0 and pi) - reconstructed from point coordinates. - Also returns the corners corresponding to each entry of theta. - """ - from pylocus.basics_angles import from_0_to_pi - theta = np.empty((self.M, )) - corners = np.empty((self.M, 3)) - k = 0 - indices = np.arange(self.N) - for triangle in itertools.combinations(indices, 3): - for counter, idx in enumerate(triangle): - corner = idx - other = np.delete(triangle, counter) - corners[k, :] = [corner, other[0], other[1]] - theta[k] = self.get_inner_angle(corner, other) - theta[k] = from_0_to_pi(theta[k]) - if DEBUG: - print(self.abs_angles[corner, other[0]], - self.abs_angles[corner, other[1]]) - print('theta', corners[k, :], theta[k]) - k = k + 1 - inner_angle_sum = theta[k - 1] + theta[k - 2] + theta[k - 3] - assert abs(inner_angle_sum - pi) < 1e-10, \ - 'inner angle sum: {} {} {}'.format( - triangle, inner_angle_sum, (theta[k - 1], theta[k - 2], theta[k - 3])) - self.theta = theta - self.corners = corners - return theta, corners - - def get_inner_angle(self, corner, other): - from pylocus.basics_angles import get_inner_angle - return get_inner_angle(self.points[corner, :], ( - self.points[other[0], :], self.points[other[1], :])) - - def get_theta(self, i, j, k): - combination = np.array([i, j, k]) - idx = np.all(self.corners == combination, axis=1) - return self.theta[idx][0] - - def get_orientation(k, i, j): - from pylocus.basics_angles import from_0_to_2pi - """calculate angles theta_ik and theta_jk theta produce point Pk. - Should give the same as get_absolute_angle! """ - theta_ij = own.abs_angles[i, j] - theta_ji = own.abs_angles[j, i] - - # complicated - xi = own.points[i, 0] - xj = own.points[j, 0] - yi = own.points[i, 1] - yj = own.points[j, 1] - w = np.array([yi - yj, xj - xi]) - test = np.dot(own.points[k, :] - own.points[i, :], w) > 0 - - # more elegant - theta_ik = truth.abs_angles[i, k] - diff = from_0_to_2pi(theta_ik - theta_ij) - test2 = (diff > 0 and diff < pi) - assert (test == test2), "diff: %r, scalar prodcut: %r" % (diff, np.dot( - own.points[k, :] - own.points[i, :], w)) - - thetai_jk = truth.get_theta(i, j, k) - thetaj_ik = truth.get_theta(j, i, k) - if test: - theta_ik = theta_ij + thetai_jk - theta_jk = theta_ji - thetaj_ik - else: - theta_ik = theta_ij - thetai_jk - theta_jk = theta_ji + thetaj_ik - theta_ik = from_0_to_2pi(theta_ik) - theta_jk = from_0_to_2pi(theta_jk) - return theta_ik, theta_jk - - def return_noisy(self, noise, mode='noisy', idx=0, visualize=False): - if mode == 'normal': - theta = self.theta.copy() + np.random.normal(0, noise, self.M) - if (visualize): - plot_thetas([self_theta, theta], ['original', 'noise']) - return theta - if mode == 'constant': - theta = self.theta.copy() + noise - if (visualize): - plot_thetas([self_theta, theta], ['original', 'noise']) - return theta - if mode == 'punctual': - theta = self.theta.copy() - theta[idx] += noise - if (visualize): - plot_thetas_in_one([self.theta, theta], ['original', 'noise']) - return theta - - def get_tensor_edm(self): - D = np.empty([self.N * self.d, self.N * self.d]) - for i in range(self.N): - for j in range(self.N): - starti = i * self.d - endi = (i + 1) * (self.d) - startj = j * self.d - endj = (j + 1) * self.d - a = np.outer(self.points[i, :], self.points[j, :]) - b = np.outer(self.points[j, :], self.points[j, :]) - c = np.outer(self.points[i, :], self.points[i, :]) - D[starti:endi, startj:endj] = b + c - 2 * a - return D - - def get_closed_form(self, edm): - Daug = self.get_tensor_edm() - T = np.empty([self.N, self.N, self.N]) - for i in range(self.N): - for j in range(self.N): - for k in range(self.N): - if j == i or k == i: - factor = 0.0 - else: - factor = 1.0 / (self.edm[j, i] * self.edm[k, i]) - diff = self.points[k, :] - self.points[i, :] - starti = i * self.d - endi = (i + 1) * (self.d) - startj = j * self.d - endj = (j + 1) * self.d - Dij = Daug[starti:endi, startj:endj] - T[i, j, k] = factor * np.dot(diff.T, np.dot(Dij, diff)) - return T - - def get_theta_tensor(self): - from pylocus.basics_angles import get_theta_tensor - self.theta_tensor = get_theta_tensor(self.theta, self.corners, self.N) - return self.theta_tensor - -# Iterative angle cleaning algorithm - - def get_indices(self, k): - """ Get indices of theta vector that have k as first corner. - - :param k: Index of corner. - - :return indices_rays: Indices of ray angles in theta vector. - :return indices_triangles: Indices of triangle angles in theta vector. - :return corners_rays: List of corners of ray angles. - :return angles_rays: List of corners of triangles angles. - """ - indices_rays = [] - indices_triangles = [] - corners_rays = [] - angles_rays = [] - for t, triangle in enumerate(self.corners): - if triangle[0] == k: - indices_rays.append(t) - corners_rays.append(triangle) - angles_rays.append(self.theta[t]) - else: - indices_triangles.append(t) - np_corners_rays = np.vstack(corners_rays) - np_angles_rays = np.vstack(angles_rays).reshape((-1, )) - return indices_rays, indices_triangles, np_corners_rays, np_angles_rays - - def get_G(self, k, add_noise=True): - """ get G matrix from angles. """ - G = np.ones((self.N - 1, self.N - 1)) - if (add_noise): - noise = pi * 0.1 * np.random.rand( - (self.N - 1) * (self.N - 1)).reshape((self.N - 1, self.N - 1)) - other_indices = np.delete(range(self.N), k) - for idx, i in enumerate(other_indices): - for jdx, j in enumerate(other_indices): - if (add_noise and - i != j): # do not add noise on diagonal elements. - thetak_ij = self.get_inner_angle(k, - (i, j)) + noise[idx, jdx] - else: - thetak_ij = self.get_inner_angle(k, (i, j)) - G[idx, jdx] = cos(thetak_ij) - G[jdx, idx] = cos(thetak_ij) - return G - - def reconstruct_from_inner_angles(self, theta): - from pylocus.algorithms import reconstruct_from_inner_angles - from pylocus.algorithms import procrustes - from pylocus.basics_angles import get_theta_tensor - - theta_tensor = get_theta_tensor(theta, self.corners, self.N) - reconstruction = reconstruct_from_inner_angles( - self.points[0, :], self.points[1, :], self.abs_angles[0, 2], - self.abs_angles[1, 2], theta_tensor) - new_points, __, __, __ = procrustes( - self.points, reconstruction.points, scale=True) - reconstruction.points = new_points - reconstruction.init() - return reconstruction - - def reconstruct_aloc(self, theta): - from pylocus.algorithms import reconstruct_aloc - from pylocus.basics_angles import get_theta_tensor - i = 0 - j = 1 - theta_tensor = get_theta_tensor(theta, self.corners, self.N) - Pi = self.points[i, :] - Pj = self.points[j, :] - k = 2 - Pk = self.points[k, :] - reconstruction = reconstruct_aloc(Pi, Pj, i, j, theta_tensor, Pk, k) - return reconstruction - - def get_convex_polygons(self, m, print_out=False): - """ - :param m: size of polygones (number of corners) - - :return: (ordered) indices of all convex polygones of size m. - """ - convex_polygons = [] - for corners in itertools.combinations(np.arange(self.N), m): - p = np.zeros(m, np.uint) - p[0] = corners[0] - left = corners[1:] - # loop through second corners - for i, second in enumerate(corners[1:m - 1]): - p[1] = second - left = np.delete(corners, (0, i + 1)) - for j, last in enumerate(corners[i + 2:]): - left = np.delete(corners, (0, i + 1, j + i + 2)) - p[-1] = last - # loop through all permutations of left corners. - for permut in itertools.permutations(left): - p[2:-1] = permut - sum_theta = 0 - # sum over all inner angles. - for k in range(m): - sum_theta += self.get_inner_angle( - p[1], (p[0], p[2])) - p = np.roll(p, 1) - angle = sum_theta - sum_angle = (m - 2) * pi - if (abs(angle - sum_angle) < 1e-14 or - abs(angle) < 1e-14): - if (print_out): - print("convex polygon found: ", p) - convex_polygons.append(p.copy()) - # elif (angle < sum_angle): - # if (print_out): print("non convex polygon found:",p,angle) - elif (angle > sum_angle): - if (print_out): - print("oops") - return convex_polygons - - def get_polygon_constraints(self, - range_polygones=range(3, 5), - print_out=False): - """ - :param range_polygones: list of numbers of polygones to test. - - :return A, b: the constraints on the theta-vector of the form A*theta = b - """ - rows_A = [] - rows_b = [] - for m in range_polygones: - if (print_out): - print('checking {}-polygones'.format(m)) - polygons = self.get_convex_polygons(m) - row_A, row_b = self.get_polygon_constraints_m(polygons, print_out) - rows_A.append(row_A) - rows_b.append(row_b) - self.A = np.vstack(rows_A) - self.b = np.hstack(rows_b) - return self.A, self.b - - def get_angle_constraints_m(self, polygons_m, print_out=False): - rows = [] - m = len(polygons_m[0]) - # initialization to empty led to A being filled with first row of - # currently stored A! - A = np.zeros((1, self.M)) - b = np.empty((1, )) - for p in polygons_m: - if len(p) < 4: - break - if (print_out): - print('sum of angles for p {}'.format(p)) - for j in p: - if (print_out): - print('for corner {}'.format(p[0])) - # for k in range(2, m-1): # how many angles to sum up. - k = m - 2 - row = np.zeros(self.M) - # outer angle - for i in range(1, m - k): - sum_angles = 0 - # inner angles - for l in range(i, i + k): - sum_angles += self.get_inner_angle( - p[0], (p[l], p[l + 1])) - index = get_index(self.corners, p[0], (p[l], p[l + 1])) - if (print_out): - print('+ {} (= index{}: {})'.format( - (p[0], (p[l], p[l + 1])), - np.where(index), self.corners[index, :])) - row[index] = 1 - index = get_index(self.corners, p[0], (p[i], p[i + k])) - if (print_out): - print(' = {} (= index{}: {})'.format((p[0], (p[i], p[ - i + k])), np.where(index), self.corners[index, :])) - row[index] = -1 - rows.append(row) - if (print_out): - print('sum_angles - expected:{}'.format( - sum_angles - self.get_inner_angle( - p[0], (p[i], p[i + k])))) - if np.sum(np.nonzero(A)) == 0: - A = row - else: - A = np.vstack((A, row)) - p = np.roll(p, 1) - if A.shape[0] > 0: - b = np.zeros(A.shape[0]) - self.A = A - self.b = b - return A, b - - def get_polygon_constraints_m(self, polygons_m, print_out=False): - """ - :param range_polygones: list of numbers of polygones to test. + def plot_all(self, title='', size=[5, 2], filename='', axis='off'): + from .plots_cti import plot_points + plot_points(self.points, title, size, filename, axis) - :return A, b: the constraints on the theta-vector of the form A*theta = b - """ - rows_b = [] - rows_A = [] - m = len(polygons_m[0]) - rows_b.append((m - 2) * pi * np.ones( - len(polygons_m), )) - for p in polygons_m: - row = np.zeros((self.theta.shape[0], )) - for k in range(m): - index = get_index(self.corners, p[1], (p[0], p[2])) - row[index] = 1 - p = np.roll(p, 1) - assert np.sum(row) == m - rows_A.append(row) - A = np.vstack(rows_A) - b = np.hstack(rows_b) - num_constraints = A.shape[0] - A_repeat = np.repeat(A.astype(bool), 3).reshape((1, -1)) - corners = self.corners.cp.reshape((1, -1)) - corners_tiled = np.tile(corners, num_constraints) - if (print_out): - print('shape of A {}'.format(A.shape)) - if (print_out): - print('chosen angles m={}:\n{}'.format(m, (corners_tiled)[A_repeat] - .cp.reshape((-1, m * 3)))) - if (print_out): - print('{}-polygones: {}'.format(m, rows_A)) - self.A = A - self.b = b - return A, b + def plot_some(self, range_, title='', size=[5, 2]): + from .plots_cti import plot_points + plot_points(self.points[range_, :], title, size) class HeterogenousSet(PointSet): diff --git a/setup.py b/setup.py index 4e844c7..d6b0c48 100644 --- a/setup.py +++ b/setup.py @@ -11,7 +11,7 @@ setup( name='pylocus', - version='0.0.4', + version='0.0.5', description='Localization Package', From ab0e260210dcf98e23a2b368a314ba11f037afd2 Mon Sep 17 00:00:00 2001 From: duembgen Date: Fri, 8 Nov 2019 14:39:53 +0100 Subject: [PATCH 9/9] Fix project description. --- CONTRIBUTE.md | 6 +++++- setup.py | 8 +++++--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/CONTRIBUTE.md b/CONTRIBUTE.md index 1a9dc14..a67b58f 100644 --- a/CONTRIBUTE.md +++ b/CONTRIBUTE.md @@ -3,6 +3,7 @@ ## Release new version 1. Change the version number in + - pylocus/__init__.py - setup.py - docs/source/conf.py @@ -24,4 +25,7 @@ python3 -m twine upload dist/* ``` -5. If above does not work, make sure that the information in ~/.pypirc is up to date. +(If above does not work, make sure that the information in ~/.pypirc is up to date) + +5. Do not forget to create a new release on github as well. + diff --git a/setup.py b/setup.py index d6b0c48..6122bf5 100644 --- a/setup.py +++ b/setup.py @@ -4,9 +4,11 @@ here = path.abspath(path.dirname(__file__)) -# Get the long description from the README file -with open(path.join(here, 'README.rst'), encoding='utf-8') as f: - long_description = f.read() +long_description = ''' + +Localization package. Source code and more information can be found on github at https://github.com/LCAV/pylocus. + +''' setup( name='pylocus',