Commit 24cd696d by Torbjørn Ludvigsen 👷

### Includes search for az bz cz in optimization

parent f1f1edda
 ... ... @@ -15,6 +15,7 @@ D = 3 X = 0 Y = 1 Z = 2 params_anch = 9 def symmetric_anchors(l, az=-120., bz=-120., cz=-120.): anchors = np.array(np.zeros((4, 3))) ... ... @@ -152,26 +153,31 @@ def cost(anchors, pos, samp): return np.sum(np.abs(samples(anchors, pos, fuzz = 0) - samp)) def cost_sq(anchors, pos, samp): #pos[0] = [0.0, 0.0, 0.0] return np.sum(pow((samples_relative_to_origo_no_fuzz(anchors, pos, fuzz = 0) - samp), 2)) # Sum of squares """ (A_ax-x_i)^2 + (A_ay-y_i)^2 + (A_az-z_i)^2 - (A_ax^2 + A_ay^2 + A_az^2) - t_ia + (A_bx-x_i)^2 + (A_by-y_i)^2 + (A_bz-z_i)^2 - (A_bx^2 + A_by^2 + A_bz^2) - t_ib + (A_cx-x_i)^2 + (A_cy-y_i)^2 + (A_cz-z_i)^2 - (A_cx^2 + A_cy^2 + A_cz^2) - t_ic + (A_dx-x_i)^2 + (A_dy-y_i)^2 + (A_dz-z_i)^2 - (A_dx^2 + A_dy^2 + A_dz^2) - t_id """ return np.sum(pow((samples_relative_to_origo_no_fuzz(anchors, pos) - samp), 2)) # Sum of squares def anchorsvec2matrix(anchorsvec, az = 0., bz = 0., cz = 0.): def anchorsvec2matrix(anchorsvec): """ Create a 4x3 anchors matrix from 6 element anchors vector. """ anchors = np.array(np.zeros((4, 3))) anchors[A,Y] = anchorsvec[0]; anchors[A,Z] = az; anchors[B,X] = anchorsvec[1]; anchors[B,Y] = anchorsvec[2]; anchors[B,Z] = bz; anchors[C,X] = anchorsvec[3]; anchors[C,Y] = anchorsvec[4]; anchors[C,Z] = cz; anchors[D,Z] = anchorsvec[5]; anchors[A,Z] = anchorsvec[1]; anchors[B,X] = anchorsvec[2]; anchors[B,Y] = anchorsvec[3]; anchors[B,Z] = anchorsvec[4]; anchors[C,X] = anchorsvec[5]; anchors[C,Y] = anchorsvec[6]; anchors[C,Z] = anchorsvec[7]; anchors[D,Z] = anchorsvec[8]; return anchors def anchorsmatrix2vec(a): return [a[A,Y], a[B, X], a[B,Y], a[C, X], a[C, Y], a[D, Z]] return [a[A,Y], a[A,Z], a[B, X], a[B,Y], a[B,Z], a[C, X], a[C, Y], a[C,Z], a[D, Z]] def posvec2matrix(v, u): return np.reshape(v, (u,3)) ... ... @@ -179,7 +185,7 @@ def posvec2matrix(v, u): def posmatrix2vec(m): return np.reshape(m, np.shape(m)[0]*3) def solve(samp, _cost = cost_sq, az = 0., bz = 0., cz = 0.): def solve(samp, _cost = cost_sq): """Find reasonable positions and anchors given a set of samples. """ def costx(posvec, anchvec): ... ... @@ -187,36 +193,49 @@ def solve(samp, _cost = cost_sq, az = 0., bz = 0., cz = 0.): Parameters ---------- x : [A_ay A_bx A_by A_cx A_cy A_dz x : [A_ay A_az A_bx A_by A_bz A_cx A_cy A_cz A_dz x1 y1 z1 x2 y2 z2 ... xu yu zu """ anchors = anchorsvec2matrix(anchvec, az, bz, cz) anchors = anchorsvec2matrix(anchvec) pos = np.reshape(posvec, (u,3)) return _cost(anchors, pos, samp) l_anch = 1500.0 l_pos = 450 l_long = 4000.0 l_short = 1700.0 u = np.shape(samp)[0] cos30 = np.cos(30*np.pi/180) sin30 = np.sin(30*np.pi/180) number_of_params_pos = 3*u number_of_params_anch = 6 l_long = 4000.0 l_short = 1700.0 data_z_min = -20.0 # Limits of anchor positions: # |ANCHOR_XY| < 4000 # ANCHOR_B_X > 0 # ANCHOR_C_X < 0 # |ANCHOR_ABC_Z| < 1700 # 0 < ANCHOR_D_Z < 4000 # Limits of data collection volume: # |x| < 1700 # |y| < 1700 # -20.0 < z < 3400.0 # Define bounds lb = [ -l_long, # A_ay > -4000.0 0, # A_bx > 0 0, # A_by > 0 -l_long*cos30, # A_cx > -4000*cos(30) 0, # A_cy > 0 0, # A_dz > 0 ] + [-l_short, -l_short, -10.]*u ub = [ 0, # A_ay < 0 l_long*cos30, # A_bx < 4000.0*cos(30) l_long*sin30, # A_by < 4000.0*sin(30) 0, # A_cx < 0 l_long*sin30, # A_cy < 4000.0*sin(30) -l_short, # A_az > -1700.0 0.0, # A_bx > 0 0.0, # A_by > 0 -l_short, # A_bz > -1700.0 -l_long, # A_cx > -4000 0.0, # A_cy > 0 -l_short, # A_cz > -1700.0 0.0, # A_dz > 0 ] + [-l_short, -l_short, data_z_min]*u ub = [ 0.0, # A_ay < 0 l_short, # A_az < 1700 l_long, # A_bx < 4000 l_long, # A_by < 4000 l_short, # A_bz < 1700 0.0, # A_cx < 0 l_long, # A_cy < 4000.0 l_short, # A_cz < 1700 l_long, # A_dz < 4000.0 ] + [l_short, l_short, 2*l_short]*u ... ... @@ -224,9 +243,9 @@ def solve(samp, _cost = cost_sq, az = 0., bz = 0., cz = 0.): from mystic.solvers import DifferentialEvolutionSolver2, PowellDirectionalSolver pos_est0 = np.zeros((u,3)) anchors_est = np.array([[0.0, 0.0, az], [0.0, 0.0, bz], [0.0, 0.0, cz], anchors_est = np.array([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]) x_guess0 = list(anchorsmatrix2vec(anchors_est)) + list(posmatrix2vec(pos_est0)) ... ... @@ -236,22 +255,22 @@ def solve(samp, _cost = cost_sq, az = 0., bz = 0., cz = 0.): target = 1.0 term = Or((COG(generations=100), CollapseAt(target, generations=100))) #print("Solver 0") solver0 = PowellDirectionalSolver(number_of_params_pos+number_of_params_anch) # Solver 0 solver0 = PowellDirectionalSolver(number_of_params_pos+params_anch) solver0.SetEvaluationLimits(evaluations=3200000, generations=10000) solver0.SetTermination(term) solver0.SetInitialPoints(x_guess0) solver0.SetStrictRanges(lb, ub) solver0.Solve(lambda x: costx(x[6:], x[0:6])) solver0.Solve(lambda x: costx(x[params_anch:], x[0:params_anch])) x_guess0 = solver0.bestSolution # PowellDirectional sometimes finds new ways if kickstarted anew for i in range(1,20): #print(", %d" % i) solver0 = PowellDirectionalSolver(number_of_params_pos+number_of_params_anch) solver0 = PowellDirectionalSolver(number_of_params_pos+params_anch) solver0.SetInitialPoints(x_guess0) solver0.SetStrictRanges(lb, ub) solver0.Solve(lambda x: costx(x[6:], x[0:6])) solver0.Solve(lambda x: costx(x[params_anch:], x[0:params_anch])) x_guess0 = solver0.bestSolution return solver0.bestSolution ... ... @@ -261,10 +280,10 @@ if __name__ == "__main__": az = -115. bz = -115. cz = -115. anchors = np.array([[0.0, -1112.0, az], [970.0, 550.0, bz], [-970.0, 550.0, cz], [0.0, 0.0, 2865.0]]) anchors = np.array([[ 0.0, -1112.0, az], [ 970.0, 550.0, bz], [-970.0, 550.0, cz], [ 0.0, 0.0, 2865.0]]) # data 1 # samp = np.array([ ... ... @@ -300,7 +319,47 @@ if __name__ == "__main__": # ]) # data 3 # samp = np.array([ #[571.85 , -773.44 , 578.54 , 59.12], #[647.64 , 654.08 , -858.11 , 90.96], #[-860.99 , 675.57 , 630.82 , 94.65], #[318.70 , 741.68 , 209.66 , -824.57], #[754.92 , 466.22 , 418.71 , -1066.04], #[450.51 , 403.77 , 699.62 , -1046.80], #[1234.76 , 1174.05 , 1183.26 , -1913.14], #[283.26 , 162.84 , 182.21 , -603.33], #[663.00 , -139.32 , 603.13 , -648.02], #[661.00 , 625.20 , -73.58 , -713.35], #[-33.50 , 643.97 , 658.06 , -754.40] # ]) # data 1 & 2 & 3 samp = np.array([ [126.31 , 5.02 , -0.21 , -213.52], [295.03 , -257.68 , 218.73 , -244.16], [511.65 , 94.13 , 116.17 , -585.52], [373.57 , 615.00 , -132.03 , -570.93], [285.95 , 468.10 , -475.99 , -112.57], [411.75 , -471.95 , 279.45 , -61.84], [646.11 , 257.49 , 289.34 , -845.42], [43.83 , 384.27 , 262.25 , -618.82], [-416.94 , 392.71 , 305.03 , -178.76], [-355.53 , 308.31 , 408.93 , -267.15], [191.34 , 555.78 , 209.78 , -741.28], [537.90 , 574.98 , 470.11 , -1102.07], [636.51 , 380.17 , 709.07 , -1118.74], [897.10 , 913.95 , 702.54 , -1473.05], [400.53 , 175.53 , 166.10 , -656.90], [229.27 , 511.14 , -48.41 , -554.31], [-41.69 , -62.87 , 306.76 , -225.31], [272.97 , 176.65 , 381.13 , -717.81], [338.07 , 633.70 , 309.27 , -911.22], [504.47 , 658.88 , 48.60 , -794.42], [504.47 , 658.88 , 48.60 , -794.42], [103.50 , 569.98 , 633.68 , -860.25], [229.37 , 7.32 , 411.98 , -575.81], [428.73 , -413.46 , 250.38 , -133.93], [-506.97 , 343.33 , 327.68 , -4.40], [571.85 , -773.44 , 578.54 , 59.12], [647.64 , 654.08 , -858.11 , 90.96], [-860.99 , 675.57 , 630.82 , 94.65], ... ... @@ -314,28 +373,15 @@ if __name__ == "__main__": [-33.50 , 643.97 , 658.06 , -754.40] ]) u = np.shape(samp)[0] pos = np.zeros((u, 3)) the_cost = 100000.0 best_cost = 100000.0 best_az = 1000.0 best_bz = 1000.0 best_cz = 1000.0 for azi in np.arange(az+10.,az-25.1,-5.): for bzi in np.arange(bz+10.,bz-25.1,-5.): for czi in np.arange(cz+10.,cz-25.1,-5.): solution = solve(samp, cost_sq, az, bz, cz) sol_anch = anchorsvec2matrix(solution[0:6], az, bz, cz) the_cost = cost_sq(anchorsvec2matrix(solution[0:6], az, bz, cz), np.reshape(solution[6:], (u,3)), samp) if(the_cost < best_cost): best_cost = the_cost best_az = azi best_bz = bzi best_cz = czi print("New record cost: %f" % the_cost) print("Output Anchors: ") print(sol_anch) print("Errors: ") print(sol_anch - anchors) solution = solve(samp, cost_sq) sol_anch = anchorsvec2matrix(solution[0:params_anch]) the_cost = cost_sq(anchorsvec2matrix(solution[0:params_anch]), np.reshape(solution[params_anch:], (u,3)), samp) print("cost: %f" % the_cost) print("cost per sample: %f" % (the_cost/u)) print("Output Anchors: ") print(sol_anch) print("Errors: ") print(sol_anch - anchors)
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!