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
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
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
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
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