### Tweaks DifferentialEvoultionSolver2 to get within ~2 cm per anchor coordinate

parent 78a8417b
 """Simulation of Hangprinter auto-calibration """ from __future__ import division # Always want 3/2 = 1.5 from itertools import product import numpy as np #import mystic #import scipy # Tips on how to use differential solver: # build/lib.linux-x86_64-2.7/mystic/differential_evolution.py # http://www.icsi.berkeley.edu/~storn/code.html # Axes indexing A = 0 ... ... @@ -77,9 +78,10 @@ def positions(n, l, fuzz=10): l : Max length from origo along each axis to sample fuzz: How much each measurement point can differ from the regular grid """ from itertools import product pos = np.array(list(product(np.linspace(-l, l, n), repeat = 3))) \ + 2.*fuzz*(np.random.rand(n**3, 3) - 0.5) \ + [0, 0, l] + [0, 0, 1*l] index_closest_to_origo = np.int(np.shape(pos)/2)-int(n/2) # Make pos a point fairly close to origo tmp = pos.copy() ... ... @@ -124,7 +126,9 @@ def cost(anchors, pos, samp): samp : ux4 matrix of corresponding samples, starting with [0., 0., 0., 0.] """ return np.sum(np.abs(samples(anchors, pos, fuzz = 0) - samp)) #return np.sum((samples(anchors, pos, fuzz = 0) - samp) * (samples(anchors, pos, fuzz = 0) - samp)) # Sum of squares def cost_sq(anchors, pos, samp): return np.sum(pow((samples(anchors, pos, fuzz = 0) - samp), 2)) # Sum of squares def anchorsvec2matrix(anchorsvec): """ Create a 4x3 anchors matrix from 6 element anchors vector. ... ... @@ -147,19 +151,9 @@ def posvec2matrix(v, u): def posmatrix2vec(m): return np.reshape(m, np.shape(m)*3) def solve(samp, cb): def solve(samp, cb, _cost = cost_sq): """Find reasonable positions and anchors given a set of samples. """ u = np.shape(samp) 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 anchors_est = symmetric_anchors(1500.0) pos_est = [[0, 0, 500]]*u x_guess = list(anchorsmatrix2vec(anchors_est)) + list(posmatrix2vec(pos_est)) def costx(posvec, anchvec): """Identical to cost, except the shape of inputs and capture of samp and u ... ... @@ -170,39 +164,137 @@ def solve(samp, cb): """ anchors = anchorsvec2matrix(anchvec) pos = np.reshape(posvec, (u,3)) return cost(anchors, pos, samp) return _cost(anchors, pos, samp) l_anch = 1500.0 l_pos = 750 l_long = 5000.0 l_short = 800.0 u = np.shape(samp) 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 anchors_est = symmetric_anchors(l_anch) pos_est = np.random.rand(u,3)*l_short - [l_short/2, l_short/2, 0] x_guess = list(anchorsmatrix2vec(anchors_est)) + list(posmatrix2vec(pos_est)) # Define bounds lb = [ -l_long, # A_ay > -5000.0 0, # A_bx > 0 0, # A_by > 0 -l_long*cos30, # A_cx > -5000*cos(30) 0, # A_cy > 0 0, # A_dz > 0 -50.0, # x0 > -50.0 -50.0, # y0 > -50.0 -10, # z0 > -10 ] + [-l_short, -l_short, -10]*(u-1) ub = [ 0, # A_ay < 0 l_long*cos30, # A_bx < 5000.0*cos(30) l_long*sin30, # A_by < 5000.0*sin(30) 0, # A_cx < 0 l_long*sin30, # A_cy < 5000.0*sin(30) 2*l_long, # A_dz < 10000.0 50.0, # x0 < 50.0 50.0, # y0 < 50.0 l_short, # z0 < l_short ] + [l_short, l_short, 2*l_short]*(u-1) from mystic.termination import ChangeOverGeneration from mystic.solvers import DifferentialEvolutionSolver2 # Bootstrap to give get x positons to cover some volume NP = 100 # Guessed size of the trial solution population # We know that our anchor guess is in the right directions but our # position guesses are still random. This optimization fixes that NP = 60 # Guessed size of the trial solution population pos_solver = DifferentialEvolutionSolver2(number_of_params_pos, NP) pos_solver.SetStrictRanges(lb[6:], ub[6:]) pos_solver.SetInitialPoints(x_guess[6:]) pos_solver.SetEvaluationLimits(generations=1500) pos_solver.SetEvaluationLimits(generations=2000) pos_solver.Solve(lambda x: costx(x, list(anchorsmatrix2vec(anchors_est))), \ termination = ChangeOverGeneration(generations=300, tolerance=2), \ callback = cb) callback = cb, \ CrossProbability=0.8, ScalingFactor=0.8) x_guess[6:] = pos_solver.bestSolution # Main solver def explode_points(x, fac, diffZ): new_x = list(x) for i in range(0,6): new_x[i] = new_x[i]*fac for i in range(8, len(x), 3): #new_x[i] = new_x[i] + diffZ new_x[i] = new_x[i]*fac return new_x def elevate(x, diffD, diffZ): new_x = list(x) new_x = x + diffD for i in range(8, len(x),3): new_x[i] = x[i] + diffZ return new_x # We know that our anchor guesses are along the right directions # but we don't know if their lengths are right. This optimization fixes that. percentage = 1.0 while(percentage > 0.0005): solver = DifferentialEvolutionSolver2(number_of_params_pos+number_of_params_anch, NP) solver.SetStrictRanges(lb, ub) solver.SetInitialPoints(x_guess) solver.SetEvaluationLimits(generations=30) solver.Solve(lambda x: costx(x[6:], x[0:6]), \ termination = ChangeOverGeneration(generations=300), \ callback = cb, \ CrossProbability=0.8, ScalingFactor=0.8) z_diff = np.abs(x_guess - solver.bestSolution) x_low = explode_points(solver.bestSolution, 1.0 - 0.01*percentage, 0) x_high = explode_points(solver.bestSolution, 1.0 + 0.01*percentage, 0) cost_low = costx(x_low[6:], x_low[0:6]) cost_high = costx(x_high[6:], x_high[0:6]) cost_mid = costx(solver.bestSolution[6:], solver.bestSolution[0:6]) print( "cost_low: %f" % cost_low ) print( "cost_high: %f" % cost_high) print( "cost_mid: %f" % cost_mid ) # Try to set the right radius for the anchors... if (cost_low < 0.999*cost_mid) & (cost_low < cost_high): while((cost_low < 0.999*cost_mid) & (cost_low < cost_high)): x_low = explode_points(x_low, 1.0 - 0.01*percentage, 0) cost_low = costx(x_low[6:], x_low[0:6]) print("Imploding and lowering") x_guess = x_low elif (cost_high < 0.999*cost_mid) & (cost_high < cost_low): while((cost_high < 0.999*cost_mid) & (cost_high < cost_low)): x_high = explode_points(x_high, 1.0 + 0.01*percentage, 0) cost_high = costx(x_high[6:], x_high[0:6]) print("Exploding and heighering") x_guess = x_high else: x_guess = solver.bestSolution percentage = percentage/2 print("Not highering or lowering. z_diff was %f" % z_diff) print("percentage = %f" % percentage) # Main optimization that narrows down the leftover fuzz x_guess = solver.bestSolution #NP = 10*(number_of_params_pos+number_of_params_anch) # Guessed size of the trial solution population NP = 80 from mystic.strategy import Rand1Bin, Best1Exp, Best1Bin, Rand1Exp solver = DifferentialEvolutionSolver2(number_of_params_pos+number_of_params_anch, NP) solver.SetStrictRanges(lb, ub) solver.SetInitialPoints(x_guess) solver.SetEvaluationLimits(generations=5000) solver.SetEvaluationLimits(generations=1000000) solver.Solve(lambda x: costx(x[6:], x[0:6]), \ termination = ChangeOverGeneration(generations=300), \ callback = cb) callback = cb, \ CrossProbability=0.5, ScalingFactor=0.8, strategy = Best1Bin) x_guess = solver.bestSolution return anchorsvec2matrix(solver.bestSolution[0:6]) # Do basic testing and output results # if this is run like a script # and not imported like a module or package if __name__ == "__main__": l_long = 2000 l_short = 1000 l_long = 2500 l_short = 800 n = 3 anchors = irregular_anchors(l_long) pos = positions(n, l_short, fuzz = 5) ... ... @@ -214,6 +306,7 @@ if __name__ == "__main__": plt.ion() # Make anchor figure and position figure. # Put the right answers onto those figures plt.close("all") fig_anch = plt.figure() fig_pos = plt.figure() ax_anch = fig_anch.add_subplot(111, projection='3d') ... ... @@ -237,6 +330,9 @@ if __name__ == "__main__": scat_anch._offsets3d = (anch[:,0], anch[:,1], anch[:,2]) print("Anchor errors: ") print(anchorsvec2matrix(x[0:6]) - anchors) print("cost: %f" % \ cost(anchorsvec2matrix(x[0:6]), np.reshape(x[6:], (u,3)), samp)) elif len(x) == 6: anch = anchorsvec2matrix(x[0:6]) scat_anch._offsets3d = (anch[:,0], anch[:,1], anch[:,2]) ... ... @@ -244,7 +340,7 @@ if __name__ == "__main__": ps = posvec2matrix(x, u) scat_pos._offsets3d = (ps[:,0], ps[:,1], ps[:,2]) plt.draw() plt.pause(0.1) plt.pause(0.001) iter += 1 sample_fuzz = 0 ... ... @@ -252,4 +348,4 @@ if __name__ == "__main__": solution = solve(samp, replot) print("Anchor errors were: ") print(solution - anchors) print("Cost were: %f" % cost(solution, pos, samp)) print("Real cost were: %f" % cost(solution, pos, samp))
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!