```python
from sympy import *
init_printing()
%matplotlib inline
```


```python
x, y, theta, u, v = symbols("x y theta u v", real=True)
g = symbols("g", real=True, positive=True)
```


```python
# we want to find the minimum distance from an edge pixel to the grid with size g (pixels),
# offset u, v (pixels), and angle theta

# distance to grid will be the minimum of distances to the nearest gridline
# (rounding (x-u, y-v) % g toward zero) after counter-rotation
S, C = sin(theta), cos(theta)
R = Matrix([[C, -S], [S, C]])
Dxy = Abs((R*Matrix([[x], [y]]) - Matrix([[u - g/2], [v - g/2]])) % g - Matrix([[g/2], [g/2]]))
D = Min(Dxy[0], Dxy[1])
```


```python
plot(D.subs([(g, 1), (u, 0), (v, 0), (theta, 0.1), (y, 0)]))
```


```python
cse(diff(D, theta))
```




$$\left ( \left [ \left ( x_{0}, \quad \cos{\left (\theta \right )}\right ), \quad \left ( x_{1}, \quad \sin{\left (\theta \right )}\right ), \quad \left ( x_{2}, \quad x x_{0} - x_{1} y\right ), \quad \left ( x_{3}, \quad \frac{g}{2}\right ), \quad \left ( x_{4}, \quad x x_{1}\right ), \quad \left ( x_{5}, \quad x_{0} y\right ), \quad \left ( x_{6}, \quad - x_{10} + x_{3} + x_{4} + x_{5}\right ), \quad \left ( x_{7}, \quad x_{3} - \left(x_{6}\bmod{g}\right)\right ), \quad \left ( x_{8}, \quad - u + x_{2} + x_{3}\right ), \quad \left ( x_{9}, \quad x_{3} - \left(x_{8}\bmod{g}\right)\right ), \quad \left ( x_{11}, \quad \left|{x_{9}}\right|\right ), \quad \left ( x_{12}, \quad \left|{x_{7}}\right|\right )\right ], \quad \left [ \left(- x_{2} \theta\left(x_{11} - x_{12}\right) \operatorname{sign}{\left (x_{7} \right )} \left(\left. \frac{\partial}{\partial \xi_{1}} \xi_{1}\bmod{g} \right|_{\substack{ \xi_{1}=x_{6} }}\right)\right) - \left(\left(- x_{4} - x_{5}\right) \theta\left(- x_{11} + x_{12}\right) \operatorname{sign}{\left (x_{9} \right )} \left(\left. \frac{\partial}{\partial \xi_{1}} \xi_{1}\bmod{g} \right|_{\substack{ \xi_{1}=x_{8} }}\right)\right)\right ]\right )$$




```python
dx, dy, ds = symbols("dx dy ds", real=True)
diff(abs(dx * cos(theta) + dy * sin(theta)), theta)
```


```python
print_python(diff((dx * cos(theta) + dy * sin(theta))**2, theta))
```

    dx = Symbol('dx')
    theta = Symbol('theta')
    dy = Symbol('dy')
    e = (-2*dx*sin(theta) + 2*dy*cos(theta))*(dx*cos(theta) + dy*sin(theta))



```python
Ix, Iy = Function("Ix", real=True), Function("Iy", real=True)  # x and y image gradients
diff(abs(Ix(x, y) - Ix(x + ds*cos(theta), y + ds*sin(theta))), theta)
```




$$\left(ds \sin{\left (\theta \right )} \left. \frac{\partial}{\partial \xi_{1}} \operatorname{Ix}{\left (\xi_{1},ds \sin{\left (\theta \right )} + y \right )} \right|_{\substack{ \xi_{1}=ds \cos{\left (\theta \right )} + x }} - ds \cos{\left (\theta \right )} \left. \frac{\partial}{\partial \xi_{2}} \operatorname{Ix}{\left (ds \cos{\left (\theta \right )} + x,\xi_{2} \right )} \right|_{\substack{ \xi_{2}=ds \sin{\left (\theta \right )} + y }}\right) \operatorname{sign}{\left (\operatorname{Ix}{\left (x,y \right )} - \operatorname{Ix}{\left (ds \cos{\left (\theta \right )} + x,ds \sin{\left (\theta \right )} + y \right )} \right )}$$




```python
obj = Min(Abs(dx * cos(theta) + dy * sin(theta)),
         Abs(dy * cos(theta) - dx * sin(theta)))
simplify(diff(obj, theta))
```


```python
print_python(cse(Out[30]))
```

    x0 = Symbol('x0')
    theta = Symbol('theta')
    x1 = Symbol('x1')
    x2 = Symbol('x2')
    dx = Symbol('dx')
    dy = Symbol('dy')
    x3 = Symbol('x3')
    x4 = Symbol('x4')
    x5 = Symbol('x5')
    e = ([(x0, cos(theta)), (x1, sin(theta)), (x2, dx*x0 + dy*x1), (x3, dx*x1 - dy*x0), (x4, Abs(x2)), (x5, Abs(x3))], [x2*Heaviside(x4 - x5)*sign(x3) - x3*Heaviside(-x4 + x5)*sign(x2)])



```python
simplify(diff(obj, theta, theta))
```


```python
vs, es = cse([obj, diff(obj, theta), diff(obj, (theta, 2))])
for v, e in vs:
    print(v, '=', e)
for e in es:
    print('=', e)
```

    x0 = cos(theta)
    x1 = sin(theta)
    x2 = dx*x0 + dy*x1
    x3 = Abs(x2)
    x4 = dx*x1
    x5 = dy*x0
    x6 = x4 - x5
    x7 = Abs(x6)
    x8 = sign(x2)
    x9 = -x3 + x7
    x10 = Heaviside(x9)
    x11 = x10*x8
    x12 = Heaviside(x3 - x7)
    x13 = sign(x6)
    x14 = x13*x2
    x15 = x6*x8
    x16 = (x14 + x15)*DiracDelta(x9)
    = Min(x3, x7)
    = x11*(-x4 + x5) + x12*x14
    = 2*x10*x6**2*DiracDelta(x2) - x11*x2 - x12*x13*x6 + 2*x12*x2**2*DiracDelta(x6) - x14*x16 - x15*x16



```python
obj = Min((dx * cos(theta) + dy * sin(theta))**2,
         (dy * cos(theta) - dx * sin(theta))**2)

vs, es = cse([obj, diff(obj, theta)])
for v, e in vs:
    print(v, '=', e)
for e in es:
    print('=', e)
```

    x0 = cos(theta)
    x1 = dx*x0
    x2 = sin(theta)
    x3 = dy*x2
    x4 = x1 + x3
    x5 = x4**2
    x6 = dy*x0
    x7 = dx*x2
    x8 = x6 - x7
    x9 = x8**2
    x10 = -x5
    x11 = (-x6 + x7)**2
    x12 = Heaviside(-x11 + x5)
    x13 = x10 + x11
    x14 = Heaviside(x13)
    = Min(x5, x9)
    = x4*(2*x6 - 2*x7)*Heaviside(x10 + x9) + x8*(-2*x1 - 2*x3)*Heaviside(x5 - x9)
    = -2*x11*x12 + 2*x11*x14 - 16*x11*x5*DiracDelta(x13) + 2*x12*x5 - 2*x14*x5



```python
delta = symbols("delta")  # huber loss

```


```python
# p(Ix(x) | x, u, g) = Ix*cos((x-u)*2pi/g)**2
plot(cos(x)**2)
```


```python
# f = 2*pi/g  - frequency in rad/pix instead of period in pixels
f = symbols("f", real=True, positive=True)
obj2 = Ix(x)*cos((x-u)*f)**2
vs, es = cse([obj2, diff(obj2, u), diff(obj2, f)])
for v, e in vs:
    print(v, '=', e)
for e in es:
    print('=', e)
```

    x0 = Ix(x)
    x1 = -u + x
    x2 = f*x1
    x3 = cos(x2)
    x4 = 2*x0*x3*sin(x2)
    = x0*x3**2
    = f*x4
    = -x1*x4



```python
# hang on, this is just weighted least squares, residual is sin(f*(x-u)), weight is Ix(x)
r = sin(f*(x-u))
J = Matrix([r]).jacobian(Matrix([u, f]))
r, J
```




$$\left ( \sin{\left (f \left(- u + x\right) \right )}, \quad \left[\begin{matrix}- f \cos{\left (f \left(- u + x\right) \right )} & \left(- u + x\right) \cos{\left (f \left(- u + x\right) \right )}\end{matrix}\right]\right )$$




```python
vs, es = cse([r, J])
for v, e in vs:
    print(v, '=', e)
for e in es:
    print('=', e)
```

    x0 = -u + x
    x1 = f*x0
    x2 = cos(x1)
    = sin(x1)
    = Matrix([[-f*x2, x0*x2]])



```python
# problem: the incentive is for f to = 0, and then everything is solved
# we need to encourage a match better
r = sin(f*(x-u))
J = Matrix([r]).jacobian(Matrix([u]))
r, J
```




$$\left ( \sin{\left (f \left(- u + x\right) \right )}, \quad \left[\begin{matrix}- f \cos{\left (f \left(- u + x\right) \right )}\end{matrix}\right]\right )$$




```python
pu, pv = symbols("p_u p_v", real=True)

S, C = sin(theta), cos(theta)
R*Matrix([pu, pv]) + Matrix([x, y])
```




$$\left[\begin{matrix}p_{u} \cos{\left (\theta \right )} - p_{v} \sin{\left (\theta \right )} + x\\p_{u} \sin{\left (\theta \right )} + p_{v} \cos{\left (\theta \right )} + y\end{matrix}\right]$$




```python
(R*Matrix([pu, pv]) + Matrix([x, y])).jacobian(Matrix([theta, x, y]))
```

250ab11f1c1277bf451fc2f6121c2384f9428328
ipynb
Jupyter Notebook
design/coneslam/ceiling-sympy.ipynb
ParikhKadam/cycloid
c5e64e8379f801417a38755eb6b2fde881dabd8c
MIT
2017-08-12T03:58:21.000Z
2022-03-21T13:38:52.000Z
design/coneslam/ceiling-sympy.ipynb
ParikhKadam/cycloid
c5e64e8379f801417a38755eb6b2fde881dabd8c
MIT
2018-12-11T11:31:06.000Z
2019-12-05T18:43:51.000Z
design/coneslam/ceiling-sympy.ipynb
ParikhKadam/cycloid
c5e64e8379f801417a38755eb6b2fde881dabd8c
MIT
2017-08-17T02:05:54.000Z
2022-02-24T20:54:27.000Z
Qwen/Qwen-72B
1. YES
2. YES