simulation.py 19 KB
Newer Older
1
2
3
4
"""Simulation of Hangprinter auto-calibration
"""
from __future__ import division # Always want 3/2 = 1.5
import numpy as np
5
import scipy.optimize
6
import argparse
7
8
import timeit
import sys
9
10
11
12
13
14
15
16
17

# Axes indexing
A = 0
B = 1
C = 2
D = 3
X = 0
Y = 1
Z = 2
18
params_anch = 9
19
20
A_bx = 2
A_cx = 5
21

Torbjørn Ludvigsen's avatar
Torbjørn Ludvigsen committed
22
def symmetric_anchors(l, az=-120., bz=-120., cz=-120.):
23
24
    anchors = np.array(np.zeros((4, 3)))
    anchors[A, Y] = -l
Torbjørn Ludvigsen's avatar
Torbjørn Ludvigsen committed
25
    anchors[A, Z] = az
26
27
    anchors[B, X] = l*np.cos(np.pi/6)
    anchors[B, Y] = l*np.sin(np.pi/6)
Torbjørn Ludvigsen's avatar
Torbjørn Ludvigsen committed
28
    anchors[B, Z] = bz
29
30
    anchors[C, X] = -l*np.cos(np.pi/6)
    anchors[C, Y] = l*np.sin(np.pi/6)
Torbjørn Ludvigsen's avatar
Torbjørn Ludvigsen committed
31
    anchors[C, Z] = cz
32
33
34
35
36
37
38
    anchors[D, Z] = l
    return anchors

def centered_rand(l):
    """Sample from U(-l, l)"""
    return l*(2.*np.random.rand()-1.)

Torbjørn Ludvigsen's avatar
Torbjørn Ludvigsen committed
39
def irregular_anchors(l, fuzz_percentage = .2, az=-120., bz=-120.,cz=-120.):
40
41
42
43
    """Realistic exact positions of anchors.

    Each dimension of each anchor is treated separately to
    resemble the use case.
44
45
46
    Six anchor coordinates must be constant and known
    for the coordinate system to be uniquely defined by them.
    A 3d coordinate system, like a rigid body, has six degrees of freedom.
47
48
49
50
51
52
53
54
55
56
57
58

    Parameters
    ---------
    l : The line length to create the symmetric anchors first
    fuzz_percentage : Percentage of l that line lenghts are allowed to differ
                      (except Z-difference of B- and C-anchors)
    """
    fuzz = np.array(np.zeros((4, 3)))
    fuzz[A, Y] = centered_rand(l*fuzz_percentage)
    #fuzz[A, Z] = 0 # Fixated
    fuzz[B, X] = centered_rand(l*fuzz_percentage*np.cos(np.pi/6))
    fuzz[B, Y] = centered_rand(l*fuzz_percentage*np.sin(np.pi/6))
59
    #fuzz[B, Z] = 0 # Fixated
60
61
    fuzz[C, X] = centered_rand(l*fuzz_percentage*np.cos(np.pi/6))
    fuzz[C, Y] = centered_rand(l*fuzz_percentage*np.sin(np.pi/6))
62
    #fuzz[C, Z] = 0 # Fixated
63
64
65
    #fuzz[D, X] = 0 # Fixated
    #fuzz[D, Y] = 0 # Fixated
    fuzz[D, Z] = l*fuzz_percentage*np.random.rand() # usually higher than A is long
Torbjørn Ludvigsen's avatar
Torbjørn Ludvigsen committed
66
    return symmetric_anchors(l, az, bz, cz)+fuzz
67
68
69
70
71
72
73
74
75
76
77
78
79

def positions(n, l, fuzz=10):
    """Return (n^3)x3 matrix of positions in fuzzed grid of side length 2*l

    Move to u=n^3 positions in an fuzzed grid of side length 2*l
    centered around (0, 0, l).

    Parameters
    ----------
    n : Number of positions of which to sample along each axis
    l : Max length from origo along each axis to sample
    fuzz: How much each measurement point can differ from the regular grid
    """
80
    from itertools import product
81
82
    pos = np.array(list(product(np.linspace(-l, l, n), repeat = 3))) \
            + 2.*fuzz*(np.random.rand(n**3, 3) - 0.5) \
83
            + [0, 0, 1*l]
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
    index_closest_to_origo = np.int(np.shape(pos)[0]/2)-int(n/2)
    # Make pos[0] a point fairly close to origo
    tmp = pos[0].copy()
    pos[0] = pos[index_closest_to_origo]
    pos[index_closest_to_origo] = tmp
    return pos


def samples(anchors, pos, fuzz=1):
    """Possible relative line length measurments according to anchors and position.

    Parameters
    ----------
    anchors : 4x3 matrix of anhcor positions in mm
    pos : ux3 matrix of positions
    fuzz: Maximum measurment error per motor in mm
    """
    # pos[:,np.newaxis,:]: ux1x3
    # Broadcasting happens u times and we get ux4x3 output before norm operation
    line_lengths = np.linalg.norm(anchors - pos[:,np.newaxis,:], 2, 2)
    return line_lengths - line_lengths[0] + 2.*fuzz*(np.random.rand(np.shape(pos)[0], 1) - 0.5)

106
107
108
109
110
111
112
113
114
115
116
117
118
119
def samples_relative_to_origo(anchors, pos, fuzz=1):
    """Possible relative line length measurments according to anchors and position.

    Parameters
    ----------
    anchors : 4x3 matrix of anhcor positions in mm
    pos : ux3 matrix of positions
    fuzz: Maximum measurment error per motor in mm
    """
    # pos[:,np.newaxis,:]: ux1x3
    # Broadcasting happens u times and we get ux4x3 output before norm operation
    line_lengths = np.linalg.norm(anchors - pos[:,np.newaxis,:], 2, 2)
    return line_lengths - np.linalg.norm(anchors,2,1) + 2.*fuzz*(np.random.rand(np.shape(pos)[0], 1) - 0.5)

120
121
122
123
124
125
126
127
128
129
130
131
132
133
def samples_relative_to_origo_no_fuzz(anchors, pos):
    """Possible relative line length measurments according to anchors and position.

    Parameters
    ----------
    anchors : 4x3 matrix of anhcor positions in mm
    pos : ux3 matrix of positions
    fuzz: Maximum measurment error per motor in mm
    """
    # pos[:,np.newaxis,:]: ux1x3
    # Broadcasting happens u times and we get ux4x3 output before norm operation
    line_lengths = np.linalg.norm(anchors - pos[:,np.newaxis,:], 2, 2)
    return line_lengths - np.linalg.norm(anchors,2,1)

134
135
136
137
138
139
def cost(anchors, pos, samp):
    """If all positions and samples correspond perfectly, this returns 0.

    This is the systems of equations:
    sum for i from 1 to u
      sum for k from a to d
140
    |sqrt(sum for s from x to z (A_ks-s_i)^2) - sqrt(sum for s from x to z (A_ks-s_0)^2) - t_ik|
141
142
143

    or...
    sum for i from 1 to u
144
    |sqrt((A_ax-x_i)^2 + (A_ay-y_i)^2 + (A_az-z_i)^2) - sqrt((A_ax-x_0)^2 + (A_ay-y_0)^2 + (A_az-z_0)^2) - t_ia| +
145
146
147
    |sqrt((A_bx-x_i)^2 + (A_by-y_i)^2 + (A_bz-z_i)^2) - sqrt((A_bx-x_0)^2 + (A_by-y_0)^2 + (A_bz-z_0)^2) - t_ib| +
    |sqrt((A_cx-x_i)^2 + (A_cy-y_i)^2 + (A_cz-z_i)^2) - sqrt((A_cx-x_0)^2 + (A_cy-y_0)^2 + (A_cz-z_0)^2) - t_ic| +
    |sqrt((A_dx-x_i)^2 + (A_dy-y_i)^2 + (A_dz-z_i)^2) - sqrt((A_dx-x_0)^2 + (A_dy-y_0)^2 + (A_dz-z_0)^2) - t_id|
148
149
150
151
152
153
154

    Parameters
    ---------
    anchors : 4x3 matrix of anchor positions
    pos: ux3 matrix of positions
    samp : ux4 matrix of corresponding samples, starting with [0., 0., 0., 0.]
    """
155
    return np.sum(np.abs(samples_relative_to_origo_no_fuzz(anchors, pos) - samp))
156
157

def cost_sq(anchors, pos, samp):
158
    """
159
160
161
162
163
164
165
    For all samples sum
    (Sample value if anchor position A and cartesian position x were guessed   - actual sample)^2

    (sqrt((A_ax-x_i)^2 + (A_ay-y_i)^2 + (A_az-z_i)^2) - sqrt(A_ax^2 + A_ay^2 + A_az^2) - t_ia)^2 +
    (sqrt((A_bx-x_i)^2 + (A_by-y_i)^2 + (A_bz-z_i)^2) - sqrt(A_bx^2 + A_by^2 + A_bz^2) - t_ib)^2 +
    (sqrt((A_cx-x_i)^2 + (A_cy-y_i)^2 + (A_cz-z_i)^2) - sqrt(A_cx^2 + A_cy^2 + A_cz^2) - t_ic)^2 +
    (sqrt((A_dx-x_i)^2 + (A_dy-y_i)^2 + (A_dz-z_i)^2) - sqrt(A_dx^2 + A_dy^2 + A_dz^2) - t_id)^2
166
167
    """
    return np.sum(pow((samples_relative_to_origo_no_fuzz(anchors, pos) - samp), 2)) # Sum of squares
168

169
def anchorsvec2matrix(anchorsvec):
170
171
172
173
    """ Create a 4x3 anchors matrix from 6 element anchors vector.
    """
    anchors = np.array(np.zeros((4, 3)))
    anchors[A,Y] = anchorsvec[0];
174
175
176
177
178
179
180
181
    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];
182
183
184
    return anchors

def anchorsmatrix2vec(a):
185
    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]]
186
187
188
189
190
191
192

def posvec2matrix(v, u):
    return np.reshape(v, (u,3))

def posmatrix2vec(m):
    return np.reshape(m, np.shape(m)[0]*3)

193
def solve(samp, xyz_of_samp, _cost, method, cx_is_positive=False):
194
195
    """Find reasonable positions and anchors given a set of samples.
    """
196
197
198
199
200

    u = np.shape(samp)[0]
    ux = np.shape(xyz_of_samp)[0]
    number_of_params_pos = 3*(u - ux)

201
    def costx(posvec, anchvec):
202
        """Identical to cost, except the shape of inputs and capture of samp, xyz_of_samp, ux, and u
203
204
205

        Parameters
        ----------
206
        x : [A_ay A_az A_bx A_by A_bz A_cx A_cy A_cz A_dz
207
208
               x1   y1   z1   x2   y2   z2   ...  xu   yu   zu
        """
209
        anchors = anchorsvec2matrix(anchvec)
210
211
212
213
        pos = np.zeros((u, 3))
        if(np.size(xyz_of_samp) != 0):
            pos[0:ux] = xyz_of_samp
        pos[ux:] = np.reshape(posvec, (u-ux,3))
214
215
        return _cost(anchors, pos, samp)

216
    l_long = 5000.0
217
218
219
220
221
222
223
224
225
226
227
228
    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
229
    # Define bounds
Torbjørn Ludvigsen's avatar
Torbjørn Ludvigsen committed
230
    lb = [      -l_long, # A_ay > -4000.0
231
232
233
234
235
236
237
238
               -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
239
          ] + [-l_short, -l_short, data_z_min]*(u-ux)
240
241
242
243
244
245
246
247
    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
Torbjørn Ludvigsen's avatar
Torbjørn Ludvigsen committed
248
                 l_long, # A_dz < 4000.0
249
250
251
252
253
          ] + [l_short, l_short, 2*l_short]*(u-ux)

    # If the user has input xyz data, then signs should be ok anyways
    if(ux > 2):
        lb[A_bx] = -l_long
254

255
256
257
258
259
260
261
262
263
264
    # It would work to just swap the signs of bx and cx after the optimization
    # But there are fewer assumptions involved in setting correct bounds from the start instead
    if(cx_is_positive):
        tmp = lb[A_bx]
        lb[A_bx] = lb[A_cx]
        lb[A_cx] = tmp
        tmp = ub[A_bx]
        ub[A_bx] = ub[A_cx]
        ub[A_cx] = tmp

265
    pos_est0 = np.zeros((u-ux,3)) # The positions we need to estimate
266
267
268
    anchors_est = np.array([[0.0, 0.0, 0.0],
                            [0.0, 0.0, 0.0],
                            [0.0, 0.0, 0.0],
Torbjørn Ludvigsen's avatar
Torbjørn Ludvigsen committed
269
                            [0.0, 0.0, 0.0]])
Torbjørn Ludvigsen's avatar
Works    
Torbjørn Ludvigsen committed
270
271
    x_guess0 = list(anchorsmatrix2vec(anchors_est)) + list(posmatrix2vec(pos_est0))

272
273
274
275
276
277
278
279
    if(method == 'PowellDirectionalSolver'):
        from mystic.termination import ChangeOverGeneration, NormalizedChangeOverGeneration, VTR
        from mystic.solvers import PowellDirectionalSolver
        from mystic.termination import Or, CollapseAt, CollapseAs
        from mystic.termination import ChangeOverGeneration as COG
        target = 1.0
        term = Or((COG(generations=100), CollapseAt(target, generations=100)))
        # Solver 0
280
        solver0 = PowellDirectionalSolver(number_of_params_pos+params_anch)
281
282
        solver0.SetEvaluationLimits(evaluations=3200000, generations=10000)
        solver0.SetTermination(term)
Torbjørn Ludvigsen's avatar
Works    
Torbjørn Ludvigsen committed
283
        solver0.SetInitialPoints(x_guess0)
284
        solver0.SetStrictRanges(lb, ub)
285
        solver0.Solve(lambda x: costx(x[params_anch:], x[0:params_anch]))
Torbjørn Ludvigsen's avatar
Works    
Torbjørn Ludvigsen committed
286
        x_guess0 = solver0.bestSolution
287
288
289
290
291
292
293
294
295
296
        # PowellDirectional sometimes finds new ways if kickstarted anew
        for i in range(1,20):
            solver0 = PowellDirectionalSolver(number_of_params_pos+params_anch)
            solver0.SetInitialPoints(x_guess0)
            solver0.SetStrictRanges(lb, ub)
            solver0.Solve(lambda x: costx(x[params_anch:], x[0:params_anch]))
            x_guess0 = solver0.bestSolution
        return x_guess0
    elif(method == 'SLSQP'):
        # 'SLSQP' is crazy fast and lands on 0.0000 error
Torbjørn Ludvigsen's avatar
Torbjørn Ludvigsen committed
297
        x_guess0 = scipy.optimize.minimize(lambda x: costx(x[params_anch:], x[0:params_anch]), x_guess0, method=method, bounds=list(zip(lb,ub)),
298
299
300
301
                options={'disp':True,'ftol':1e-20, 'maxiter':150000})
        return x_guess0.x
    elif(method == 'L-BFGS-B'):
        ## 'L-BFGS-B' Is crazy fast but doesn't quite land at 0.0000 error
Torbjørn Ludvigsen's avatar
Torbjørn Ludvigsen committed
302
        x_guess0 = scipy.optimize.minimize(lambda x: costx(x[params_anch:], x[0:params_anch]), x_guess0, method=method, bounds=list(zip(lb,ub)),
303
304
305
306
307
                options={'ftol':1e-12, 'maxiter':150000})
        return x_guess0.x
    else:
        print("Method %s is not supported!" % method)
        sys.exit(1)
Torbjørn Ludvigsen's avatar
Works    
Torbjørn Ludvigsen committed
308

309

310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
def print_anch(anch):
    print("\n#define ANCHOR_A_Y %5d" % round(anch[A,Y]))
    print("#define ANCHOR_A_Z %5d"   % round(anch[A,Z]))
    print("#define ANCHOR_B_X %5d"   % round(anch[B,X]))
    print("#define ANCHOR_B_Y %5d"   % round(anch[B,Y]))
    print("#define ANCHOR_B_Z %5d"   % round(anch[B,Z]))
    print("#define ANCHOR_C_X %5d"   % round(anch[C,X]))
    print("#define ANCHOR_C_Y %5d"   % round(anch[C,Y]))
    print("#define ANCHOR_C_Z %5d"   % round(anch[C,Z]))
    print("#define ANCHOR_D_Z %5d"   % round(anch[D,Z]))
    print("\nM665 W%.2f E%.2f R%.2f T%.2f Y%.2f U%.2f I%.2f O%.2f P%.2f" % (anch[A,Y],anch[A,Z],anch[B,X],anch[B,Y],anch[B,Z],anch[C,X],anch[C,Y],anch[C,Z],anch[D,Z]))

def print_anch_err(sol_anch, anchors):
    print("\nErr_A_Y: %9.3f" % (sol_anch[A,Y] - anchors[A,Y]))
    print("Err_A_Z: %9.3f" % (sol_anch[A,Z] - anchors[A,Z]))
    print("Err_B_X: %9.3f" % (sol_anch[B,X] - anchors[B,X]))
    print("Err_B_Y: %9.3f" % (sol_anch[B,Y] - anchors[B,Y]))
    print("Err_B_Z: %9.3f" % (sol_anch[B,Z] - anchors[B,Z]))
    print("Err_C_X: %9.3f" % (sol_anch[C,X] - anchors[C,X]))
    print("Err_C_Y: %9.3f" % (sol_anch[C,Y] - anchors[C,Y]))
    print("Err_C_Z: %9.3f" % (sol_anch[C,Z] - anchors[C,Z]))
    print("Err_D_Z: %9.3f" % (sol_anch[D,Z] - anchors[D,Z]))

333
334
335
336
337
class Store_as_array(argparse._StoreAction):
    def __call__(self, parser, namespace, values, option_string=None):
        values = np.array(values)
        return super(Store_as_array,self).__call__(parser, namespace, values, option_string)

338
if __name__ == "__main__":
339
340
    parser = argparse.ArgumentParser(description='Figure out where Hangprinter anchors are by looking at line difference samples.')
    parser.add_argument('-d', '--debug', help='Print debug information', action='store_true')
341
    parser.add_argument('-c', '--cx_is_positive', help='Use this flag if your C anchor should have a positive X-coordinate', action='store_true')
342
    parser.add_argument('-m', '--method', help='Available methods are L-BFGS-B (default), PowellDirectionalSolver (requires a library called Mystic), and SLSQP. As a shorthand, you can use 0, 1, or 2, for referring to the three methods respectively.',
343
                       default='L-BFGS-B')
344
345
    parser.add_argument('-x', '--xyz_of_samp', help='Specify the XYZ-positions where samples were made as numbers separated by spaces.', action=Store_as_array, type=float, nargs='+', default=np.array([]))
    parser.add_argument('-s', '--sample_data', help='Specify the sample data you have found as numbers separated by spaces.', action=Store_as_array, type=float, nargs='+', default=np.array([]))
346
347
    args = vars(parser.parse_args())

348
349
350
351
352
353
    if(args['method'] == "0"):
        args['method'] = 'L-BFGS-B'
    if(args['method'] == "1"):
        args['method'] = 'PowellDirectionalSolver'
    if(args['method'] == "2"):
        args['method'] = 'SLSQP'
354

355
356
    # Rough approximations from manual measuring.
    # Does not affect optimization result. Only used for manual sanity check.
357
358
359
    anchors = np.array([[   0.0, -1112.0,  -115.],
                        [ 970.0,   550.0,  -115.],
                        [-970.0,   550.0,  -115.],
360
                        [   0.0,     0.0, 2865.0]])
361

362
363
364
365
366
367
368
369
370
371
    #a = np.zeros((2,3))
    xyz_of_samp = args['xyz_of_samp']
    if(np.size(xyz_of_samp) != 0):
        if(np.size(xyz_of_samp)%3 != 0):
            print("Error: You specified %d numbers after your -x/--xyz_of_samp option, which is not a multiple of 3 numbers.")
            sys.exit(1)
        xyz_of_samp = xyz_of_samp.reshape((int(np.size(xyz_of_samp)/3),3))
    else:
        xyz_of_samp = np.array([
                               # You might want to manually input positions where you made samples here like
372
373
374
                               #[ -13.82573298,  185.92015633, 664.66427937],
                               #[-389.81246064, -32.85556323 , 587.55219886],
                               #[ 237.76400537, -126.40678778, 239.0320148],
375
376
377
378
379
380
381
382
                               #[ 143.2309169 ,  -15.59590026,  722.89425101],
                               #[-267.39107719, -139.31819633,  934.36563975],
                               #[-469.27951032,  102.82165224,  850.67454249],
                               #[-469.27950169,  102.82167868,  850.6745381 ],
                               #[  59.64224478, -448.29890816,  911.68810588],
                               #[ 273.18632979,   -1.66414539,  591.93608109],
                               #[ 345.42863651,  365.92077557,  180.51780131],
                               #[  -2.49959496, -527.89199888,   53.34811685],
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
                               ])

    samp = args['sample_data']
    if(np.size(samp) != 0):
        if(np.size(samp)%4 != 0):
            print("Please specify A, B, C, and D diffs of sampling points.")
            print("You specified %d numbers after your -s/--sample_data option, which is not a multiple of 4 number of numbers.")
            sys.exit(1)
        samp = samp.reshape((int(np.size(samp)/4),4))
    else:
        # You might want to manually replace this with your collected data
        samp = np.array([
                         [400.53 , 175.53 , 166.10 , -656.90],
                         [229.27 , 511.14 , -48.41 , -554.31],
                         [-41.69 , -62.87 , 306.76 , -225.31],
398
399
400
401
402
403
404
405
                         [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]
406
                        ])
407

408
    u = np.shape(samp)[0]
409
410
411
412
413
414
    ux = np.shape(xyz_of_samp)[0]
    if(ux > u):
        print("Error: You have more xyz positions than samples!")
        print("You have %d xyz positions and %d samples" %(ux, u))
        sys.exit(1)

415
    st1 = timeit.default_timer()
416
    solution = solve(samp, xyz_of_samp, cost_sq, args['method'], args['cx_is_positive'])
417
    st2 = timeit.default_timer()
418
    sol_anch = anchorsvec2matrix(solution[0:params_anch])
419
    sol_pos = np.zeros((u,3))
420
421

    if(np.size(xyz_of_samp) != 0):
422
423
        sol_pos = np.vstack((xyz_of_samp, np.reshape(solution[params_anch:], (u-ux,3))))
        the_cost = cost_sq(anchorsvec2matrix(solution[0:params_anch]), sol_pos, samp)
424
    else:
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
        sol_pos = np.reshape(solution[params_anch:], (u,3))
        the_cost = cost_sq(anchorsvec2matrix(solution[0:params_anch]), sol_pos, samp)
    print("samples:          %d" % u)
    print("input xyz coords: %d" % (3*ux))
    print("total cost:       %f" % the_cost)
    print("cost per sample:  %f" % (the_cost/u))

    if((u+3*ux) < params_anch):
        print("\nError: Lack of data detected.\n       Collect more samples.")
        if(not args['debug']):
            sys.exit(1)
        else:
            print("       Debug flag is set, so printing bogus anchor values anyways.")
    elif((u+3*ux) < params_anch+4):
        print("\nWarning: Data set might be too small.\n         The below values are unreliable unless input data is extremely accurate.")

441
442
443
    print_anch(sol_anch)
    if (args['debug']):
        print_anch_err(sol_anch, anchors)
444
445
        print("Method: %s" % args['method'])
        print("RUN TIME : {0}".format(st2-st1))
446
        print("positions: ")
447
        print(sol_pos)
448