distortion.py

from scipy.optimize import minimize
import numpy as np
from math import *
from sympy import *

dist = 1.38
center_x = 160.
center_y = 100.

placement = [
    [1.9, 0.],
    [2.7, 0.],
    [3.5, 0.],
    [4.3, 0.],
    [5.1, 0.],
    [5.7, 0.],
    [0., -2.6],
    [0., -3.1],
    [0., -3.6],
    [0., -4.1],
    [0., -4.6]
    ]
placement = map(lambda x: np.array(x)*0.3048, placement) # ft to m

measurement = [
    [30.8, center_y],
    [84.0, center_y],
    [139.0, center_y],
    [204.5, center_y],
    [255.2, center_y],
    [294.9, center_y],
    [center_x, 164.5],
    [center_x, 129.1],
    [center_x, 95.0],
    [center_x, 59.3],
    [center_x, 22.7]
    ]
measurement = map(lambda x: np.array(x), measurement)

# distortion model
    pix_x_sym, pix_y_sym, center_x_sym, center_y_sym, scale_x_sym, scale_y_sym, k1_sym, k2_sym = symbols('_PIX_X _PIX_Y _CENTER_X _CENTER__Y _SCALE_X _SCALE_Y _K1 _K2')

    x_d = (pix_x_sym-center_x_sym)*scale_x_sym
    y_d = (pix_y_sym-center_y_sym)*scale_y_sym
    r = sqrt(x_d**2+y_d**2)

    x_u = x_d/(1.+k1_sym*r**2+k2_sym*r**4)
    y_u = y_d/(1.+k1_sym*r**2+k2_sym*r**4)

    x_u = x_u.subs([(center_x_sym,center_x),(center_y_sym,center_y)])
    y_u = y_u.subs([(center_x_sym,center_x),(center_y_sym,center_y)])

    x_u = simplify(x_u)
    y_u = simplify(y_u)

correct_meas = lambdify([pix_x_sym, pix_y_sym, scale_x_sym, scale_y_sym, k1_sym, k2_sym],Matrix([[x_u,y_u]]))

def rms_error(x):
    k2 = 0
    scale_x, scale_y, k1, ofs_x, ofs_y = x
    ret = 0.
    for i in range(len(measurement)):
        corrected_meas = correct_meas(measurement[i][0], measurement[i][1], scale_x, scale_y, k1, k2).flatten()

        predicted_meas = placement[i].copy()
        if predicted_meas[0] != 0.:
            predicted_meas[0] -= ofs_x
        if predicted_meas[1] != 0.:
            predicted_meas[1] -= ofs_y

        print predicted_meas, corrected_meas

        predicted_meas /= dist

        diff = corrected_meas-predicted_meas
        #print diff
        ret += diff[0]**2+diff[1]**2
    ret = float(sqrt(ret/len(measurement)))
    return ret

res = minimize(rms_error, [2.*0.57735026919/320.,2.*0.31529878887/200.,0.,0.,0.], bounds = [(None,None),(None,None),(-1.,1.),(None,None),(None,None)])
print res
scale_x, scale_y, k1, ofs_x, ofs_y = res.x
k2 = 0.

x_u = x_u.subs([(scale_x_sym,scale_x),(scale_y_sym,scale_y),(k1_sym,k1),(k2_sym,k2)])
y_u = y_u.subs([(scale_x_sym,scale_x),(scale_y_sym,scale_y),(k1_sym,k1),(k2_sym,k2)])
x_u = simplify(x_u)
y_u = simplify(y_u)

from sympy.printing.ccode import *

class MyPrinter(CCodePrinter):
    def __init__(self,settings={}):
        CCodePrinter.__init__(self, settings)
        self.known_functions = {
            "Abs": [(lambda x: not x.is_integer, "fabsf")],
            "gamma": "tgammaf",
            "sin": "sinf",
            "cos": "cosf",
            "tan": "tanf",
            "asin": "asinf",
            "acos": "acosf",
            "atan": "atanf",
            "atan2": "atan2f",
            "exp": "expf",
            "log": "logf",
            "erf": "erff",
            "sinh": "sinhf",
            "cosh": "coshf",
            "tanh": "tanhf",
            "asinh": "asinhf",
            "acosh": "acoshf",
            "atanh": "atanhf",
            "floor": "floorf",
            "ceiling": "ceilf",
        }

    def _print_Pow(self, expr):
        if "Pow" in self.known_functions:
            return self._print_Function(expr)
        PREC = precedence(expr)

        if expr.exp == -1:
            return '1.0/%s' % (self.parenthesize(expr.base, PREC))
        elif expr.exp < 0:
            expr = 1/expr

        if expr.exp == 0.5:
            return 'sqrtf(%s)' % self._print(expr.base)
        elif expr.exp.is_integer and expr.exp <= 4:
            return "(%s)" % ('*'.join(["(%s)" % (self._print(expr.base)) for _ in range(expr.exp)]),)
        else:
            return 'powf(%s, %s)' % (self._print(expr.base),
                                 self._print(expr.exp))

    def _print_Rational(self, expr):
        p, q = int(expr.p), int(expr.q)
        return '%d.0/%d.0' % (p, q)

def double2float(string):
    import re
    string = re.sub(r"[-+]?[0-9]*\.?[0-9]+([eE][-+]?[0-9]+)?", '\g<0>f', string)

    return string

print "#define IRLOCK_PIXEL_POS_TO_1M_PLANE_POS(_PIX_X,_PIX_Y,_RET_X,_RET_Y) %s %s" % (double2float(MyPrinter().doprint(x_u, '_RET_X')),double2float(MyPrinter().doprint(y_u, '_RET_Y')))