# James Gregson's Website

## Reordering Matrix Products

Here are a few operations for dealing with objective functions of matrix-valued variables:

## Starting Definitions

Let $$A$$ be a $$N\times M$$ matrix with entry $$A_{ij}$$ being the entry at row $$i$$ and column $$j$$. The $$i$$'th row of $$A$$ is then $$A_{i*}$$ and the $$j$$'th column is $$A_{*j}$$.

## Row and Column Vectorization

Define the column vectorization operator which returns a single column vector containing the columns of $$A$$ stacked

\begin{equation*} \newcommand{\cvec}{{\bf\mbox{cvec}}} \cvec(A) = \begin{bmatrix} A_{*1} \\ A_{*2} \\ \vdots \\ A_{*M} \end{bmatrix} \end{equation*}

likewise the row vectorization operator returns a single column vector containing the (transposes of) rows of $$A$$ stacked:

\begin{equation*} \newcommand{\rvec}{{\bf\mbox{rvec}}} \rvec(A) = \cvec(A^T) = \begin{bmatrix} A_{1*}^T \\ A_{2*}^T \\ \vdots \\ A_{N*} \end{bmatrix} \end{equation*}

In numpy, matrices are stored in row-major order, so the Python definition of these operaions is inverted as below:

def cvec( A ):
"""Stack columns of A into single column vector"""
return rvec(A.T)

def rvec( A ):
"""Stack rows of A into a single column vector"""
return A.ravel()


Similarly, the inverses of $$\cvec(A)$$ and $$\rvec(A)$$ unpack the vectorized values back into a matrix of the original shape:

\begin{align*} \newcommand{\cmat}{{\bf\mbox{cmat}}} \newcommand{\rmat}{{\bf\mbox{rmat}}} \cmat\left( \cvec(A), M \right) = A \\ \rmat\left( \rvec(A), N \right) = A \\ \end{align*}

with corresponding python definitions:

def rmat( v, nr ):
"""Reshape vector v into matrix with nr rows"""
return v.reshape((nr,-1))

def cmat( v, nc ):
"""Reshape vector v into matrix with nc columns"""
return v.reshape((cols,-1)).T


Finally, define two additional operators. The first of these is the spread operator, which takes the Kronecker product between an identity matrix and the input matrix, resulting in copies of the input matrix along the diagonal:

\begin{equation*} \newcommand{\spread}{{\bf\mbox{spread}}_{#2}\left(#1\right)} \spread{A}{r} = I_{r\times r} \otimes A = \begin{bmatrix} A & 0 & \dots & 0 \\ 0 & A & \dots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \dots & A \end{bmatrix} \end{equation*}

The second of these is the split operator which reverses the order of the arguments to the Kronecker product, basically replacing each entry $$A_{ij}$$ with $$A_{i,j} I_{r\times r}$$:

\begin{equation*} \newcommand{\split}{{\bf\mbox{split}}_{#2}\left(#1\right)} \split{A}{r} = A \otimes I_{r\times r} = \begin{bmatrix} A_{11} I_{r\times r} & A_{12} I_{r\times r} & \dots & A_{1M} I_{r\times r} \\ A_{21} I_{r\times r} & A_{22} I_{r\times r} & \dots & A_{2M} I_{r\times r} \\ \vdots & \vdots & \ddots & \vdots \\ A_{N1} I_{r\times r} & A_{N2} I_{r\times r} & \dots & A_{NM} I_{r\times r} \end{bmatrix} \end{equation*}

Python implementation of these are one-liners that just call the existing numpy Kronecker product:

def spread( M, n ):
"""Make n copies of M along the matrix diagonal"""
return np.kron( np.eye(n), M )

def split( M, n ):
"""Replace each entry of M with a n-by-n identity matrix scaled by the entry value"""
return np.kron( M, np.eye(n) )


With these, it is possible to define matrix products. There are also some fun properties that come from the Kronecker product:

## Matrix Products

Let $$X$$ be a $$N\times K$$ matrix and $$Y$$ be a $$K\times M$$ matrix, then their product $$W = XY$$ will be a $$N\times M$$ matrix and the following are true:

\begin{align*} \rvec(W) &=& \split{X}{M} \rvec(Y) \\ \cvec(W) &=& \spread{X}{M} \cvec(Y) \\ \rvec(W) &=& \spread{Y^T}{N} \rvec(X) \\ \cvec(W) &=& \split{Y^T}{N} \cvec(X) \end{align*}

What these let you do is unbury any (single) term that's in the middle of some horrendous expression. For example, say there's an expression $$W = AXB$$ with $$A \in \mathcal{R}^{N\times R}$$, $$X \in \mathcal{R}^{R\times S}$$ and $$B \in \mathcal{R}^{S \times M}$$ and you want to isolate $$X$$ as a column vector. Then you have:

\begin{align*} \cvec(W) &=& \spread{A}{M} \split{B^T}{R} \cvec(X) \\ &=& \left( I_{M\times M} \otimes A \right) \left( B^T \otimes I_{R\times R} \right) \cvec(X) \\ &=& \left( I_{M\times M} B^T \right) \otimes \left( A I_{R\times R} \right) \cvec(X) \\ &=& \left( B^T \otimes A \right) \cvec(X) \end{align*}

The step in the third line is the Kronecker product identity:

\begin{equation*} \left( A \otimes B \right) \left( C \otimes D \right ) = \left( A C \right) \otimes \left( B D \right) \end{equation*}

The same steps can also be done if you want row-major storage of $$X$$:

\begin{align*} \rvec(W) &=& \split{A}{M} \spread{B^T}{R} \rvec(X) \\ &=& \left( A \otimes I_{M\times M} \right) \left( I_{R\times R} \otimes B^T \right) \rvec(X) \\ &=& \left( A \otimes B^T \right) \rvec(X) \end{align*}

In these cases the dimensions work out correctly but you don't need to worry too much about them since the dimensions of $$X$$ are constrained by the column could in $$A$$ and the row count in $$B$$. This only applies when operating with them symbolically of course.

## Frobenius Norm Problems

With this information, it becomes pretty easy to deal with Frobenius norm problems like:

\begin{equation*} X = \mbox{argmin} \frac{1}{2} \| A X B - Y \|_F^2 \end{equation*}

I have found these frustrating in the past since I invariably end up in indexing hell. But it becomes much easier with the identities above. Applying them first of all isolates the X term to the right-hand-side of the expression and also converts the Frobenius norm to an $$L_2$$ norm. Making the substitutions $$\tilde{x} = \cvec(X)$$ and $$\tilde{y} = \cvec(y)$$,

\begin{align*} \tilde{x}_* &=& \mbox{argmin} \frac{1}{2} \| \left( B^T \otimes A \right) \tilde{x} - \tilde{y} \|_2^2 \\ &=& \mbox{argmin} \frac{1}{2} \tilde{x}^T \left( B \otimes A^T \right) \left( B^T \otimes A \right) \tilde{x} - \tilde{x}^T \left( B \otimes A^T \right) \tilde{y} + \mbox{constants} \\ &=& \mbox{argmin} \frac{1}{2} \tilde{x}^T \left( B B^T \otimes A^T A \right) \tilde{x} - \tilde{x}^T \left( B \otimes A^T \right) \tilde{y} \end{align*}

This now looks like a standard least-squares problem, albeit one with this Kronecker product mashed into it. Setting the gradient to zero gives the solution as:

\begin{align*} \tilde{x}_* &=& \left( B B^T \otimes A^T A \right)^{-1} \left( B \otimes A^T \right ) \tilde{y} \\ &=& \left( (B B^T)^{-1} \otimes (A^T A)^{-1} \right) \left( B \otimes A^T \right) \tilde{y} \\ &=& \left( (B B^T)^{-1} B \otimes (A^T A)^{-1} A^T \right) \tilde{y} \end{align*}

This result has a similar form to $$AXB = \left( B^T \otimes A\right)\cvec(X)$$, which can be used to get back to the matrix form of the expression:

\begin{align*} X &=& (A^T A)^{-1} A^T Y \left( (B B^T)^{-1} B\right)^T \\ &=& (A^T A)^{-1} A^T Y B^T (B B^T)^{-1} \end{align*}

It's also possible to come at this from a different direction

\begin{align*} X &=& \mbox{argmin} \frac{1}{2} \| A X B - Y \|_F^2 \\ &=& \mbox{argmin} \frac{1}{2} \mbox{Tr}\left( (AXB - Y)(AXB - Y)^T \right) \end{align*}

The gradient of this is given in the Matrix Cookbook.

\begin{equation*} \frac{\partial}{\partial X} \frac{1}{2} \mbox{Tr}\left( (AXB - Y)(AXB - Y)^T \right) = A^T \left( A X B - Y \right) B^T \end{equation*}

Setting the gradient of this to zero and isolating $$X$$ gives the following:

\begin{align*} A^T A X B B^T - A^T Y B^T = 0 \\ X = (A^T A)^{-1} A^T Y B^T (B B^T)^{-1} \end{align*}

which matches the previous result.

#### Nov 10, 2018

\begin{equation*} \newcommand{pd}{\frac{\partial #1}{\partial #2}} \end{equation*}

Let $$x = \left[ x_1, x_2, x_3 \right]^T$$ and $$y_1 = f(x)$$ be a scalar function. Then $$\nabla y_1$$ is:

\begin{equation*} \nabla y_1 = \left[ \pd{f}{x_1}, \pd{f}{x_2}, \pd{f}{x_3} \right] \end{equation*}

i.e., a row-vector. This allows it to be compatible with the corresponding definition of the Jacobian when, i.e, when

\begin{equation*} y = \begin{bmatrix} y_1 \\ y_2 \end{bmatrix} = \begin{bmatrix} f_1(x) \\ f_2(x) \end{bmatrix} \end{equation*}

then the Jacobian of $$y$$ is:

\begin{equation*} J_y = \begin{bmatrix} \pd{f_1}{x_1} & \pd{f_1}{x_2} & \pd{f_1}{x_3} \\ \pd{f_2}{x_1} & \pd{f_2}{x_2} & \pd{f_2}{x_3} \end{bmatrix} = \begin{bmatrix} \nabla f_1 \\ \nabla f_2 \end{bmatrix} \end{equation*}

This also allows linearization of the scalar or vector function to be consistent. Here $$\delta x$$

\begin{align*} y_1(x+\delta x) \approx y_1(x) + \nabla y_1 \delta x \\ y (x+\delta x) \approx y(x) + J_y \delta x \end{align*}

That's great, but where it is useful is in differentiating more complex expressions. For example in the context of optimization or regression $$E = \frac{1}{2} \| y \|^2$$. $$E$$ is a scalar function of a vector function of a vector and it's a pain to differentiate this symbolically with respect to the parameters $$x$$.

What helps here is this:

\begin{equation*} \pd{E}{x} = \pd{E}{y} \pd{y}{x} = y^T J_y \end{equation*}

The transpose on the $$y$$ factor is included because it it fits the definition that the gradient of a scalar function taking a vector argument is a row-vector. The above expression expands to:

\begin{equation*} \pd{E}{x} = \left[ y_1, y_2 \right] \begin{bmatrix} \pd{f_1}{x_1} & \pd{f_1}{x_2} & \pd{f_1}{x_3} \\ \pd{f_2}{x_1} & \pd{f_2}{x_2} & \pd{f_2}{x_3} \end{bmatrix} = \begin{bmatrix} y_1 \nabla y_1 + y_2 \nabla y_2 \end{bmatrix} \end{equation*}

which shows that the terms involving $$y_1$$ and $$y_2$$ are correctly decoupled and only combine through the summation implied by the 2-norm.

What about when $$y = A x - b$$? Well, it's pretty easy to substitute in:

\begin{align*} \pd{E}{y} = y^T \\ \pd{y}{x} = A \\ \pd{E}{x} = (A x - b)^T A \end{align*}

Wait! What's up with that? Isn't it supposed to be $$A^T(A x -b)$$? Well, yes, but that's when gradients are column vectors. Which they're not here. And if you transpose the row vector result you get the expected column vector result.

\begin{equation*} \pd{E}{x}^T = \left( (A x - b)^T A \right)^T = A^T (A x -b) \end{equation*}

So what happens if you decide to make gradients column vectors? Well, for starters, you have to redefine the del operator from $$\nabla = \left[ \pd{}{x_1}, \pd{}{x_2}, \dots, \pd{}{x_N} \right]$$ to:

\begin{equation*} \nabla = \begin{bmatrix} \pd{}{x_1} \\ \pd{}{x_2} \\ \vdots \\ \pd{}{x_3} \end{bmatrix} \end{equation*}

Redefining fundamental mathematical things should be the first clue this is wrong but pressing on using the same definitions, you get:

\begin{equation*} \nabla y_1 = \begin{bmatrix} \pd{y_1}{x_1} \\ \pd{y_1}{x_2} \\ \pd{y_1}{x_3} \end{bmatrix} \end{equation*}

Stacking these gradients column-wise gives something resembling the Jacobian but which is actually its transpose:

\begin{equation*} \pd{y}{x} = J_y^T = \begin{bmatrix} \pd{y_1}{x_1} & \pd{y_2}{x_1} \\ \pd{y_1}{x_2} & \pd{y_2}{x_2} \\ \pd{y_1}{x_3} & \pd{y_2}{x_3} \end{bmatrix} \end{equation*}

The chain rule then gives:

\begin{equation*} \pd{E}{x} = \pd{E}{y} \pd{y}{x} \end{equation*}

which leads to total garbage since the array dimensions are not compatible for matrix multiplication anymore.

\begin{equation*} \pd{E}{x} \neq y J^T = \begin{bmatrix} y_1 \\ y_2 \end{bmatrix} \begin{bmatrix} \pd{y_1}{x_1} & \pd{y_2}{x_1} \\ \pd{y_1}{x_2} & \pd{y_2}{x_2} \\ \pd{y_1}{x_3} & \pd{y_2}{x_3} \end{bmatrix} \hspace{0.25cm}{\bf\mbox{Incompatible!}} \end{equation*}

But this can be fixed by changing the order of the chain rule from $$\pd{E}{x} = \pd{E}{y} \pd{y}{x}$$ to $$\pd{E}{x} = \pd{y}{x} \pd{E}{y}$$ to get:

\begin{equation*} \pd{E}{x} = J^T y \end{equation*}

which for the example of $$y = A x - b$$ gives the expected column oriented gradient of $$\nabla E = A^T (A x - b)$$. The difference here is that matrix composition left multiplies new transformations onto an existing one while the chain rule is typically written with right multiplication. This is really the only difference between the column-oriented and row-oriented gradients, other than the discrepancy between the definitions of the gradient and Jacobian matrix. When the Jacobian and gradient are defined correctly (i.e. row-oriented) this does not occur.

What this ends up meaning is that in order for the results to work out correctly with column-oriented gradients you have to carry the Jacobian transpose throughout your work and remember to re-order the chain rule, otherwise you need to forensically reconstruct what the dimensions should be from the scalar terms of your objective function (in optimization anway). I feel this is too high a price to pay.

On the other hand, defining points in one domain as column-vectors and gradients as row-vectors means that expressions involving both need some added boilerplate, e.g.:

\begin{equation*} v = x - \delta \nabla y \hspace{0.25cm}{\bf\mbox{Incompatible!}} \end{equation*}

with $$\delta$$ a scalar no longer have compatible dimensions. Instead the equation above would need to be expressed as:

\begin{equation*} v = x - \delta (\nabla y)^T \end{equation*}

This is awkward but tends not to be a big problem since many programming langages have good array broadcasting that allows the transpose to be omitted when programming.

## Some Random Example

Let:

\begin{equation*} A = \begin{bmatrix} a_{11} & a_{12} \\ a_{21} & a_{22} \\ a_{31} & a_{32} \end{bmatrix} \end{equation*}

and:

\begin{equation*} B = \begin{bmatrix} b_{11} & b_{12} & b_{13} & b_{14} \\ b_{21} & b_{22} & b_{23} & b_{24} \end{bmatrix} \end{equation*}

and let $$A$$ & $$B$$ be functions of $$\theta \in \mathcal{R}^5$$. Now try to minimize the Frobenius norm of $$Y = AB$$:

\begin{equation*} F(\theta) = \frac{1}{2} \| Y \|_F^2 = \mbox{Tr}\left(Y Y^T\right) \end{equation*}

$$F(\theta)$$ is a scalar and $$\theta$$ is a vector so the gradient should be a row-vector of size $$1\times5$$ in the end. Starting with the chain rule an issue is evident:

\begin{equation*} \nabla F(\theta) = Y \pd{Y}{\theta} \end{equation*}

$$Y$$ is the wrong shape $$3\times4$$ and $$\pd{Y}{\theta}$$ is a $$3 \times 4 \times 5$$ array where the final index (k) holds $$\pd{Y}{\theta_k}$$. In order to make this work, we can concatenate the rows $$y_{i*}$$ of $$Y$$ to get a $$1\times 12$$ matrix $$\tilde{Y}$$ i.e.

\begin{equation*} \tilde{Y} = \begin{bmatrix} y_{1*} & y_{2*} & y_{3*} \end{bmatrix} = \mbox{vec}(Y^T)^T \end{equation*}

where $$\mbox{vec}(P)$$ stacks the columns of $$P$$ into a column vector. We can likewise stack the columns $$y_{*j}$$ of $$Y$$ to get a $$12x1$$ column vector $$\hat{Y}=\mbox{vec}(Y)$$. The derivative of this with respect to $$\theta$$ is just the Jacobian of each column of $$Y$$, stacked row-wise:

\begin{equation*} \pd{\hat{Y}}{\theta} = \begin{bmatrix} \pd{y_{*1}}{\theta} \\ \pd{y_{*2}}{\theta} \\ \pd{y_{*3}}{\theta} \\ \pd{y_{*4}}{\theta} \end{bmatrix} \end{equation*}

By doing this, the sum over all matrix entries implied by the Frobenius norm is absorbed as an inner product of $$\tilde{Y} \in \mathcal{R}^{1\times12}$$ and $$\pd{\hat{Y}}{\theta} \in \mathcal{R}^{12\times5}$$. This gives the correct dimensions for the gradient of $$1\times 5$$. Note that everything works pretty cleanly by adopting the row-vector form for gradients.

We still need to compute the $$\pd{Y_{*i}}{\theta}$$ terms. Starting from the definition of $$Y = AB$$ it's pretty easy to see that $$y_{*i} = A b_{*i}$$. We want to get $$\pd{y_{*i}}{\theta} \in \mathcal{R}^{3\times5}$$ and so go to the chain rule:

\begin{equation*} \pd{y_{*i}}{\theta} = A \pd{b_{*i}}{\theta} + \pd{A}{\theta} b_{*i} \end{equation*}

The first term is no problem, it is simply a transform ($$A$$) applied to the Jacobian of $$b_{*i}$$. The shapes work out correctly. The second term is more problematic, $$\pd{A}{\theta}$$ is a $$3\times 2 \times 5$$ array while $$b_{*i}$$ is $$2 \times 1$$. One way to address this is to stack rows of $$A$$ into a column vector using the vectorization operator and splay $$b_{*i}$$ using the Kronecker Product:

\begin{equation*} A b_{*i} = \begin{bmatrix} b_{*i}^T & {\bf 0} & {\bf 0} \\ {\bf 0} & b_{*i}^T & {\bf 0} \\ {\bf 0} & {\bf 0} & b_{*i}^T \end{bmatrix} \begin{bmatrix} a_{1*}^T \\ a_{2*}^T \\ a_{3*}^T \end{bmatrix} = \left( {\bf I_{3\times3}} \otimes b_{*i}^T \right) \mbox{vec}(A^T) \end{equation*}

## Rotation Matrix Euler Angles

This page describes a basic approach to extracting Euler angles from rotation matrices. This follows what OpenCV does, (which is where I got the process) but I also wanted to understand why this approach was used.

Here I'm considering rotation performed first around x-axis ($$\alpha$$), then y-axis ($$\beta$$) and finally z-axis ($$\gamma$$). For this, the rotation matrix is shown below. Other rotation orders are analogous.

\begin{equation*} R(\alpha,\beta,\gamma) = \left[\begin{matrix}\cos{\left (\beta \right )} \cos{\left (\gamma \right )} & \sin{\left (\alpha \right )} \sin{\left (\beta \right )} \cos{\left (\gamma \right )} - \sin{\left (\gamma \right )} \cos{\left (\alpha \right )} & \sin{\left (\alpha \right )} \sin{\left (\gamma \right )} + \sin{\left (\beta \right )} \cos{\left (\alpha \right )} \cos{\left (\gamma \right )} & 0\\\sin{\left (\gamma \right )} \cos{\left (\beta \right )} & \sin{\left (\alpha\right )} \sin{\left (\beta \right )} \sin{\left (\gamma \right )} + \cos{\left (\alpha \right )} \cos{\left (\gamma \right )} & - \sin{\left (\alpha \right )} \cos{\left (\gamma \right )} + \sin{\left (\beta \right )} \sin{\left (\gamma \right )} \cos{\left (\alpha \right )} & 0\\- \sin{\left (\beta \right )} & \sin{\left (\alpha \right )} \cos{\left (\beta \right )} & \cos{\left (\alpha \right )} \cos{\left (\beta \right )} & 0\\0 & 0 & 0 & 1\end{matrix}\right] \end{equation*}

To solve for the Euler angles, we can use a little bit of trigonometry. The best way, in my opinion, is to find pairs of entries that are each products of two factors. Using the identity $$\sin^2 + \cos^2 = 1$$ can be used to isolate specific angles, e.g. $$R_{0,0}$$ and $$R_{0,1}$$ can be used to find $$\cos(\beta)$$ which unlocks the rest of the angles:

\begin{align*} (-1)^k \cos(\beta + k \pi) = \sqrt{ \cos^2(\beta) ( \sin^2(\gamma) + \cos^2(\gamma)) } = \sqrt{ R_{0,0}^2 + R_{1,0}^2 } \\ \cos(\beta) \approx \sqrt{ R_{0,0}^2 + R_{1,0}^2 } \end{align*}

Pay attention to the first line: For any true value of $$\beta$$, this formula will return the same value for $$\beta + k\pi$$ where $$k$$ is an arbitrary integer. This will in turn determine the angles $$\alpha$$ and $$\gamma$$ in order to be compatible. The approximate equality will return $$\beta \in [-\pi/2, \pi/2]$$.

Once $$\cos(\beta)$$ is found we can find the angles directly using atan2:

\begin{align*} \beta = \mbox{atan2}( -R_{2,0}, \cos(\beta) ) \\ \alpha = \mbox{atan2}\left( \frac{R_{2,1}}{\cos(\beta)}, \frac{R_{2,2}}{\cos(\beta)} \right) \\ \gamma = \mbox{atan2}\left( \frac{R_{1,0}}{\cos(\beta)}, \frac{R_{0,0}}{\cos(\beta)} \right) \end{align*}

The other issue occurs when $$|\cos(\beta)| = 0$$, which causes division by zero in the equations for $$\alpha,\gamma$$ ($$\beta$$ is still well defined). In this case there are a number of options. What OpenCV does is to arbitrarily decide that $$\gamma = 0$$, which means that $$\sin(\gamma) = 0$$ and $$\cos(\gamma) = 1$$. The formulas in this case are:

\begin{align*} \beta = \mbox{atan2}( -R_{2,0}, \cos(\beta) ) \\ \alpha = \mbox{atan2}( -R_{1,2}, R_{1,1} ) \\ \gamma = 0 \end{align*}

There are, of course, other options. Python code implementing these operations is below, along with unit testing for the degenerate and non-degerate cases.

import unittest
import numpy

def x_rotation( theta ):
"""Generate a 4x4 homogeneous rotation matrix about x-axis"""
c = numpy.cos(theta)
s = numpy.sin(theta)
return numpy.array([
[ 1, 0, 0, 0],
[ 0, c,-s, 0],
[ 0, s, c, 0],
[ 0, 0, 0, 1]
])

def y_rotation( theta ):
"""Generate a 4x4 homogeneous rotation matrix about y-axis"""
c = numpy.cos(theta)
s = numpy.sin(theta)
return numpy.array([
[ c, 0, s, 0],
[ 0, 1, 0, 0],
[-s, 0, c, 0],
[ 0, 0, 0, 1]
])

def z_rotation( theta ):
"""Generate a 4x4 homogeneous rotation matrix about z-axis"""
c = numpy.cos(theta)
s = numpy.sin(theta)
return numpy.array([
[ c,-s, 0, 0],
[ s, c, 0, 0],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]
])

def xyz_rotation( angles ):
"""Generate 4x4 homogeneous rotation in x then y then z order"""
return numpy.dot( z_rotation(angles), numpy.dot( y_rotation(angles), x_rotation(angles) ) )

def xyz_rotation_angles( R, eps=1e-8, check=False ):
"""Back out the Euler angles that would lead to the given matrix"""

if check and numpy.linalg.norm( numpy.dot( R.transpose(), R ) - numpy.eye(4) ) > eps:
raise ValueError('Input matrix is not a pure rotation, R\'R != I')

cos_beta = numpy.sqrt( R[0,0]**2 + R[1,0]**2 )
if numpy.abs(cos_beta) > eps:
alpha = numpy.arctan2( R[2,1]/cos_beta, R[2,2]/cos_beta )
beta  = numpy.arctan2(-R[2,0], cos_beta )
gamma = numpy.arctan2( R[1,0]/cos_beta, R[0,0]/cos_beta )
return numpy.array((alpha,beta,gamma))
else:
alpha = numpy.arctan2(-R[1,2], R[1,1] )
beta  = numpy.arctan2(-R[2,0], cos_beta )
gamma = 0
return numpy.array((alpha,beta,gamma))

class test_euler_angles(unittest.TestCase):
def test_angles(self):
"""Do fuzz testing on arbitrary rotations, limiting beta to valid range"""
for i in range(1000):
alpha = (numpy.random.rand()-0.5)*numpy.pi*2
beta  = (numpy.random.rand()-0.5)*numpy.pi*0.9999
gamma = (numpy.random.rand()-0.5)*numpy.pi*2

ang = xyz_rotation_angles( xyz_rotation([alpha,beta,gamma]))
self.assertAlmostEqual( alpha, ang )
self.assertAlmostEqual( beta,  ang )
self.assertAlmostEqual( gamma, ang )

def test_degeneracies(self):
"""Do fuzz testing on the degenerate condition of beta = +- pi/2"""
for i in range(1000):
alpha = (numpy.random.rand()-0.5)*numpy.pi*2
beta  = numpy.sign(numpy.random.randn())*numpy.pi/2
gamma = (numpy.random.rand()-0.5)*numpy.pi*2
R     = xyz_rotation((alpha,beta,gamma))
ang   = xyz_rotation_angles( R )
R2    = xyz_rotation( ang )
# check that beta is recovered, gamma is set to zero and
# the rotation matrices match to high precision
self.assertAlmostEqual( beta, ang )
self.assertAlmostEqual( ang, 0 )
for j in range(16):
self.assertAlmostEqual( R.ravel()[j], R2.ravel()[j] )

if __name__ == '__main__':
unittest.main()


## Transformation Matrix Jacobians

Starting with the $$4\times4$$ homogeneous transform matrix with parameters $$\beta = [ \theta_x, \theta_y, \theta_z, t_x, t_y, t_z ]^T$$ where rotations are performed in XYZ order and using the following substitutions:

\begin{align*} c_x = \cos(\theta_x) \\ s_x = \sin(\theta_x) \\ c_y = \cos(\theta_y) \\ s_y = \sin(\theta_y) \\ c_z = \cos(\theta_z) \\ s_z = \sin(\theta_z) \\ \end{align*}

the vector function mapping a point $$p = [p_x, p_y, p_z, 1]^T$$ in the body coordinate system to a point in the world coordinate system $$w = [w_x, w_y, w_z, 1]^T$$ is:

\begin{equation*} \begin{bmatrix} w_x \\ w_y \\ w_z \\ 1 \end{bmatrix} = F( p, \beta ) = \left[\begin{matrix}c_{y} c_{z} & - c_{x} s_{z} + c_{z} s_{x} s_{y} & c_{x} c_{z} s_{y} + s_{x} s_{z} & t_{x}\\c_{y} s_{z} & c_{x} c_{z} + s_{x} s_{y} s_{z} & c_{x} s_{y} s_{z} - c_{z} s_{x} & t_{y}\\- s_{y} & c_{y} s_{x} & c_{x} c_{y} & t_{z}\\0 & 0 & 0 & 1\end{matrix}\right]\begin{bmatrix} p_x \\ p_y \\ p_z \\ 1 \end{bmatrix} \end{equation*}

and the Jacobian with respect to the parameters $$\beta$$ is:

\begin{equation*} J_F = \left[\begin{matrix}p_{y} \left(c_{x} c_{z} s_{y} + s_{x} s_{z}\right) + p_{z} \left(c_{x} s_{z} - c_{z} s_{x} s_{y}\right) & c_{x} c_{y} c_{z} p_{z} + c_{y} c_{z} p_{y} s_{x} - c_{z}p_{x} s_{y} & - c_{y} p_{x} s_{z} + p_{y} \left(- c_{x} c_{z} - s_{x} s_{y} s_{z}\right) + p_{z} \left(- c_{x} s_{y} s_{z} + c_{z} s_{x}\right) & 1 & 0 & 0\\p_{y} \left(c_{x} s_{y} s_{z} - c_{z} s_{x}\right) + p_{z} \left(- c_{x} c_{z} - s_{x} s_{y} s_{z}\right) & c_{x} c_{y} p_{z} s_{z} + c_{y} p_{y} s_{x} s_{z} - p_{x} s_{y} s_{z} & c_{y} c_{z} p_{x} + p_{y} \left(- c_{x} s_{z} + c_{z} s_{x} s_{y}\right) + p_{z} \left(c_{x} c_{z} s_{y} + s_{x} s_{z}\right) & 0 & 1 & 0\\c_{x} c_{y} p_{y} - c_{y} p_{z} s_{x} & - c_{x} p_{z} s_{y} - c_{y} p_{x} -p_{y} s_{x} s_{y} & 0 & 0 & 0 & 1\\0 & 0 & 0 & 0 & 0 & 0\end{matrix}\right] \end{equation*}

Python code for these respective operations is below:

def make_transform( beta ):
c_x = numpy.cos(beta)
s_x = numpy.sin(beta)
c_y = numpy.cos(beta)
s_y = numpy.sin(beta)
c_z = numpy.cos(beta)
s_z = numpy.sin(beta)
t_x = beta
t_y = beta
t_z = beta
return numpy.array([
[c_y*c_z, -c_x*s_z + c_z*s_x*s_y, c_x*c_z*s_y + s_x*s_z, t_x],
[c_y*s_z, c_x*c_z + s_x*s_y*s_z, c_x*s_y*s_z - c_z*s_x, t_y],
[-s_y, c_y*s_x, c_x*c_y, t_z],
[0, 0, 0, 1]
])

def make_transform_jacobian( beta, p ):
c_x = numpy.cos(beta)
s_x = numpy.sin(beta)
c_y = numpy.cos(beta)
s_y = numpy.sin(beta)
c_z = numpy.cos(beta)
s_z = numpy.sin(beta)
t_x = beta
t_y = beta
t_z = beta
p_x = p
p_y = p
p_z = p
return numpy.array([
[p_y*(c_x*c_z*s_y + s_x*s_z) + p_z*(c_x*s_z - c_z*s_x*s_y), c_x*c_y*c_z*p_z + c_y*c_z*p_y*s_x - c_z*p_x*s_y, -c_y*p_x*s_z + p_y*(-c_x*c_z - s_x*s_y*s_z) + p_z*(-c_x*s_y*s_z + c_z*s_x), 1, 0, 0],
[p_y*(c_x*s_y*s_z - c_z*s_x) + p_z*(-c_x*c_z - s_x*s_y*s_z), c_x*c_y*p_z*s_z + c_y*p_y*s_x*s_z - p_x*s_y*s_z, c_y*c_z*p_x + p_y*(-c_x*s_z + c_z*s_x*s_y) + p_z*(c_x*c_z*s_y + s_x*s_z), 0, 1, 0],
[c_x*c_y*p_y - c_y*p_z*s_x, -c_x*p_z*s_y - c_y*p_x - p_y*s_x*s_y, 0, 0, 0, 1],
[0, 0, 0, 0, 0, 0]
])


I generated these using sympy to build the transformations and used a finite-difference Jacobian function to check the output.

## Create Docker Image

This process follows this link . First update apt and install docker.io

$sudo apt-get update$ sudo apt install docker.io


Next start the service and enable it at boot time:

$sudo systemctl start docker$ sudo systemctl enable docker


$sudo usermod -a -G docker$USER


and log out of your account and back in.

## Create Dockerfile

Create a blank Dockerfile

$touch Dockerfile  and then edit it to look like the following: # start with ubuntu FROM ubuntu:16.04 # update the software repository RUN apt-get update RUN apt-get install -y python3-dev # add user appuser and switch to it RUN useradd -ms /bin/bash appuser USER appuser WORKDIR /home/appuser # copy over a test message and python # script to print it to the screen COPY data/ ./data/ COPY print_message.py . # run the script at container startup CMD ["python3", "print_message.py"]  Next make a directory to share with the image and put a simple message file into it. $ mkdir data
$echo "Hello from docker" > data/message.txt  Then create a python script to print the message: if __name__ == '__main__': with open('data/message.txt') as f: data = f.read() print( data )  Build the image and run it: $ docker build -t test_image
\$ docker run -it test_image
Hello from docker


And that's all there is to it.

## Deconvolution via ADMM in Python - Part 2: Code

In the previous part of this post I introduced deconvolution and built up the core of a total-variation deconvolution algorithm based on ADMM.

def deconvolve_ADMM( K, B, num_iters=40, rho=1.0, gamma=0.1 ):
# store the image size
dim = B.shape

# pad out the kernel if its the wrong shape
if K.shape != dim:
raise ValueError('B and K must have same shape')

# define the two derivative operators
Dx = Kernel2D( [ 0, 0], [-1, 0], [-1.0, 1.0] )
Dy = Kernel2D( [-1, 0], [ 0, 0], [-1.0, 1.0] )

# define an initial solution estimate
I = numpy.zeros( dim )

# define the two splitting variables and lagrangr multipliers
Zx = Dx.mul( I )
Zy = Dy.mul( I )
Ux = numpy.zeros( Zx.shape )
Uy = numpy.zeros( Zy.shape )

# cache the necessary terms for the I update, need to circularly
# shift the kernel so it's DC spot lies at the corner
fK  = numpy.fft.fft2( numpy.roll( K/numpy.sum(K), (dim//2,dim//2), axis=(0,1) ) )
fB  = numpy.fft.fft2( B )
fDx = Dx.spectrum( dim )
fDy = Dy.spectrum( dim )

# build the numerator and denominator
num_init = numpy.conj( fK )*fB
den      = numpy.conj( fK )*fK + rho*( numpy.conj(fDx)*fDx + numpy.conj(fDy)*fDy )

# define the L1 soft-thresholding operator
soft_threshold = lambda q: numpy.sign(q)*numpy.maximum( numpy.abs( q ) - gamma/rho, 0.0 )

# main solver loop
for iter in range( num_iters ):

# I-update
V = rho*( Dx.mul( Zx - Ux, trans=True) + Dy.mul( Zy - Uy, trans=True ) )
I = numpy.real( numpy.fft.ifft2( (num_init + numpy.fft.fft2(V))/den ) )

tmp_x = Dx.mul( I )
tmp_y = Dy.mul( I )
Zx = soft_threshold( tmp_x + Ux )
Zy = soft_threshold( tmp_y + Uy )

# multiplier update
Ux += tmp_x - Zx
Uy += tmp_y - Zy

# return reconstructed result
return I


That is it.

## Deconvolution via ADMM in Python - Part 1: Math

Deconvolution is the process of removing blur from a signal. For images, there are two main types:

• Blind deconvolution removes blur without specific knowledge of what the exact blur is. It generally requires estimating the blur kernel.
• Non-blind deconvolution removes blur but is provided the blur kernel directly. It is an easier problem than blind deconvolution. Non-blind deconvolution is often used as a component of blind deconvolution algorithms.

Here I am focusing on non-blind deconvolution.

## Image Formation Model

When the blur kernel is the same over the entire image, blurry images can be modeled mathematically as a convolution of the blur kernel with an unknown sharp image, i.e.:

\begin{equation*} {\bf B} = {\bf K} \otimes {\bf I} + \epsilon \end{equation*}

The blur kernel $${\bf K}$$ is the image that is produced in the blurry image by a single point (pixel) of the unknown sharp image $${\bf I}$$. It's often called the point spread function and, with the blurry image $${\bf B}$$, is an input to the deconvolution algorithm. The last term $$\epsilon$$ models noise in the captured signal and is often assumed to be Gaussian, although in practice is often not.

The convolution operation is linear, so in an ideal world we would just apply it's inverse to recover the sharp image. What makes deconvolution challenging is that the blur kernel usually destroys at least some information. This means that the inverse does not exist. Consequently, solving a deconvolution problems actually means finding one of the infinitely many possible sharp images that, convolved with the blur kernel, would reproduce the measured blurry image.

## Optimizing for $${\bf I}$$

Making the typical assumption of $$\epsilon$$ being Gaussian noise, a solution for the sharp image $${\bf I}$$ can be found by looking for an underlying sharp image that best explains, in a least-squares sense, the measured data $${\bf B}$$.

\begin{equation*} {\bf I} = \mbox{argmin}_{\bf I} \frac{1}{2} \| {\bf K} \otimes {\bf I} - {\bf B} \|_2^2 \end{equation*}

Denoting $$\mathcal{F}(\bf K)$$ and $$\mathcal{F}^{-1}(\bf K)$$ as the forward and inverse Fourier transform of $${\bf K}$$ and $$*$$ as complex conjugation, this equation can be solved very quickly in the Fourier domain:

\begin{equation*} {\bf I} = \mathcal{F}^{-1}\left[ \frac{\mathcal{F}({\bf K})^*\mathcal{F}(\bf B)}{\mathcal{F}({\bf K})^2} \right] \end{equation*}

Unfortunately the results are garbage. Zero or very low magnitude values of the Fourier transform of $${\bf K}$$ cause divide-by-zero issues and amplify noise. These tiny values are due to convolution by $${\bf K}$$ attenuating certain frequency bands to nothing. It makes sense intuitively since blurry images lose the sharp edges and fine details of focused images so the Fourier modes representing these are lost forever. Trying to undo the process is poorly defined and so the solve above just finds anything that minimizes the objective, whether it looks like an image or not.

It's possible to stabilize the above a bit by adding fudge factors here and there but the results are not very compelling. A better approach is to add regularizers to the optimization that favor specific types of underlying sharp images. One very effective set is the total variation which is incorporated below:

\begin{equation*} {\bf I} = \mbox{argmin}_{\bf I} \frac{1}{2} \| {\bf K} \otimes {\bf I} - {\bf B} \|_2^2 + \gamma \| \frac{\partial}{\partial x} {\bf I} \|_1 + \gamma \| \frac{\partial}{\partial y} {\bf I} \|_1 \end{equation*}

The two extra terms penalize the gradients of the image and reach a zero value only for images that are constant. Using a 1-norm makes them less sensitive to large jumps in value than a 2-norm would be which makes these priors favor piecewise constant reconstructions. This is qualitatively similar to natural images that feature piecewise smooth features.

The only problem is that these 1-norm terms make the optimization much more difficult and costly to solve. However, the effort is worth it.

## Formulating the Optimization with ADMM

The problem with solving the optimization above is that the 1-norm terms are non-smooth. This is challenging for optimization methods. Fortunately, there are some very effective tools for solving 1-norm problems in isolation and a framework call the Alternating Direction Method of Multipliers can be used to transform intractible monolithic objective functions (such as the one above) into several coupled simpler problems.

By being careful about which terms go where, this can result in very efficient solves for problems involving complex priors. What follows is only one option for the above problem.

The overall goal is to split the $$L_1$$ solves into separate pixel-wise solves. This is done by introducing new variables and constraints that make the problem look more complex but actually help. The data term will stay as a coupled $$L_2$$ problem and will absorb the coupling terms as well (which are also $$L_2$$) and serve to stabilize the solve.

The first step is to isolate the $$L_1$$ terms. The differential operators in the priors have the effect of coupling all pixels to each other, but the fast solvers for $$L_1$$ problems expect terms that look like $$\|{\bf A}\|_1$$ for some image $${\bf A}$$. This can be accomplished by introducing splitting variables $${\bf Z}_x,{\bf Z}_y$$ with appropriate constraints.

\begin{align*} {\bf I} = \mbox{argmin} & & \frac{1}{2} \| {\bf K} \otimes {\bf I} - {\bf B} \|_2^2 + \gamma \| {\bf Z}_x \|_1 + \gamma \| {\bf Z}_y \|_1 \\ \mbox{subject to} & & \frac{\partial}{\partial x} {\bf I} - {\bf Z}_x = 0 \\ & & \frac{\partial}{\partial y} {\bf I} - {\bf Z}_y = 0 \end{align*}

It is also important to make sure that the $${\bf Z}_x, {\bf Z}_y$$ terms appear without complex linear operators in the constraints. This ensures that the terms will diagonalize to a large set of pixelwise 1D optimizations rather than couple together.

With the splitting variables introduced, the constraints can be incorporated into the objective via penalty terms with (scaled) Lagrange multipliers $${\bf U}_x,{\bf U}_y$$. The multipliers keep the constraints stiff. Without the multipliers, the high weights needed on the penalty terms to enforce the constraints can dominate the solves.

Applying this step transform back from a constrained problem to a (much more complicated) unconstrained problem again:

\begin{equation*} {\bf I} = \mbox{argmin} \frac{1}{2} \| {\bf K} \otimes {\bf I} - {\bf B} \|_2^2 + \frac{\rho}{2} \| \frac{\partial}{\partial x} {\bf I} - {\bf Z}_x + {\bf U}_x \|_2^2 + \frac{\rho}{2} \| \frac{\partial}{\partial y} {\bf I} - {\bf Z}_y + {\bf U}_y \|_2^2 + \gamma \| {\bf Z}_x \|_1 + \gamma \| {\bf Z}_y \|_1 \end{equation*}

This looks like a mess, but is just the original data term, the two added $$L_1$$ terms and two additional $$L_2$$ coupling terms. Calling the whole thing $$F({\bf I},{\bf Z}_x, {\bf Z}_y, {\bf U}_x, {\bf U}_y)$$ the ADMM algorithm solves this by the following steps:

• Updates $${\bf I}$$ by solving $${\bf I} = \mbox{argmin}_{\bf I} F({\bf I},{\bf Z}_x, {\bf Z}_y, {\bf U}_x, {\bf U}_y)$$
• Updates $${\bf Z}_x$$ by solving $${\bf Z}_x = \mbox{argmin}_{{\bf Z}_x} F({\bf I},{\bf Z}_x, {\bf Z}_y, {\bf U}_x, {\bf U}_y)$$
• Updates $${\bf U}_x$$ by setting $${\bf U}_x = {\bf U}_x + \frac{\partial}{\partial x} {\bf I} - {\bf Z}_x$$
• Repeats the last two steps, appropriately modified, for $${\bf Z}_y$$ and $${\bf U}_y$$ respectively.

### The $${\bf I}$$ Update

The first step of ADMM is to update $${\bf I}$$. This is a straightforward $$L_2$$ problem since the only terms that $${\bf I}$$ appears is in terms with a 2-norm. Denoting $$\frac{\partial}{\partial x}{\bf I}$$ as $${\bf D}_x \otimes {\bf I}$$ and likewise for $$\frac{\partial}{\partial y}$$, the solve ends up being:

\begin{equation*} {\bf I} = \mbox{argmin} \frac{1}{2} \| {\bf K} \otimes {\bf I} - {\bf B} \|_2^2 + \frac{\rho}{2} \| {\bf D}_x \otimes {\bf I} - {\bf Z}_x + {\bf U}_x \|_2^2 + \frac{\rho}{2} \| {\bf D}_y \otimes {\bf I} - {\bf Z}_y + {\bf U}_y \|_2^2 \end{equation*}

All the terms are effectively the same: a minimization over the residual of a kernel convolved the the target image with respect to a measured value. The minimum is found by expanding out the term and finding the target image that sets the gradient of the expanded term to zero.

The solution for the first term in the Fourier domain is earlier in the post as:

\begin{equation*} {\bf I} = \mathcal{F}^{-1}\left[ \frac{\mathcal{F}({\bf K})^*\mathcal{F}(\bf B)}{\mathcal{F}({\bf K})^2} \right] \end{equation*}

The remaining terms follow the same pattern and sum in the numerator and denominator of the solve:

\begin{equation*} {\bf I} = \mathcal{F}^{-1}\left[ \frac{ \mathcal{F}({\bf K})^*\mathcal{F}(\bf B) +\rho \mathcal{F}({\bf D}_x)^*\mathcal{F}({\bf Z}_x - {\bf U}_x) +\rho \mathcal{F}({\bf D}_y)^*\mathcal{F}({\bf Z}_y - {\bf U}_y) }{\mathcal{F}({\bf K})^2 + \rho \mathcal{F}({\bf D}_x)^2 + \rho \mathcal{F}({\bf D}_y)^2} \right] \end{equation*}

This involves 9 Fourier transforms, but only the terms containing $${\bf Z}_x,{\bf U}_x$$ actually change during the solve (similarly for the y subscripts). By caching the first term of the numerator, the entire denominator, as well as $$\mathcal{F}({\bf D}_x)\mbox{ & }\mathcal{F}({\bf D}_y)$$ this can be reduced to three transforms. It can further be reduced by evaluating the derivative filters $${\bf D}_x\mbox{ & }{\bf D}_y$$ in the spatial domain, since they are very inexpensive finite difference filters. The following gets the $${\bf I}$$ update down to one forward and one inverse transform once contants are cached:

\begin{align*} {\bf V} = \rho \left( {\bf D}^T_x \otimes ({\bf Z}_x - {\bf U}_x) + {\bf D}^T_y \otimes ({\bf Z}_y - {\bf U}_y)\right) \\ {\bf I} = \mathcal{F}^{-1}\left[ \frac{ \mathcal{F}({\bf K})^*\mathcal{F}(\bf B) + \mathcal{F}({\bf V}) }{\mathcal{F}({\bf K})^2 + \rho \mathcal{F}({\bf D}_x)^2 + \rho \mathcal{F}({\bf D}_y)^2} \right] \end{align*}

Important note: the $${\bf D}^T_x \otimes$$ terms in $${\bf V}$$ use the transposed filters (mixing notation terribly). This is the complex conjugate of the filter in the Fourier domain version, or matrix transpose if the convolution operation were represented in matrix form. Expressed as spatial domain convolution, the transpose simply requires mirroring the kernels horizontally and vertically about the central pixel.

### The $${\bf Z}$$ Updates

The next step of ADMM is to update the splitting variable $${\bf Z}_x\mbox{ & }{\bf Z}_y$$. These terms only appear in one coupling terms and one $$L_1$$ term each. The updates for the two $${\bf Z}$$ and two $${\bf U}$$ variables are the same except the specific filter that is used, so I'll only cover one. Here's the update needed for $${\bf Z}_x$$:

\begin{equation*} {\bf Z}_x = \mbox{argmin}_{{\bf Z}_x} \frac{\rho}{2} \| {\bf D}_x \otimes {\bf I} - {\bf Z}_x + {\bf U}_x \|_2^2 + \gamma \| {\bf Z}_x \|_1 \end{equation*}

It's important to note that there are no operators applied to $${\bf Z}_x$$, meaning that each pixel can be processed independently. The terms can be rearranged and the weight factors collected onto the $$L_1$$ term to give the equivalent minimization:

\begin{equation*} {\bf Z}_x = \mbox{argmin}_{{\bf Z}_x} \frac{\gamma}{\rho} \| {\bf Z}_x \|_1 + \frac{1}{2} \| {\bf Z}_x - ({\bf D}_x \otimes {\bf I} + {\bf U}_x ) \|_2^2 \end{equation*}

However, a complication is (still) the $$L_1$$ term. This is non-smooth and tricky to deal with but the form above allows a non-smooth optimization method known as proximal operators to be used. The proximal operator of a scaled function $$\lambda f(q)$$ is defined as:

\begin{equation*} \mbox{prox}_{\lambda f}({\bf q}) = \mbox{argmin}_{\bf p} f({\bf p}) + \frac{1}{2\lambda} \| {\bf p} - {\bf q} \|_2^2 \end{equation*}

The similarities with the $${\bf Z}_x$$ minimization are strong. By setting $$f({\bf Z}_x)=\|{\bf Z}_x\|_1\mbox{ & }\lambda = \frac{\gamma}{\rho}\mbox{ & }{\bf q} = {\bf D}_x \otimes {\bf I} + {\bf U}_x$$ the proximal operator is recovered. This is great because the closed form solution for $$\mbox{prox}_{\lambda\|{\bf p}\|_1}(\bf{q})$$ is known and trivial to implement:

\begin{equation*} \mbox{prox}_{\lambda\|{\bf p}\|_1}({\bf q}) = \mbox{sign}({\bf q}) \mbox{max}( 0, |{\bf q}| - \lambda) \end{equation*}

This is known as the soft-thresholding operator, or $$L_1$$ shrinkage operator and is used everywhere. It can be implemented in python as:

def soft_threshold( q, lam ):
return numpy.sign(q)*numpy.maximum( numpy.abs(q) - lam, 0.0 )


In my view, it's the most potent line of code that exists in signal processing.

### The $${\bf U}$$ Updates

The $${\bf U}$$ updates simply add the current constraint violation to the Lagrange multipliers $${\bf U}$$. It's kind of like the integral term in a PID controller.

The update was actually given already in the overview above but is repeated here for completeness:

\begin{align*} {\bf U}_x = {\bf U}_x + {\bf D}_x \otimes {\bf I} - {\bf Z}_x \\ {\bf U}_y = {\bf U}_y + {\bf D}_x \otimes {\bf I} - {\bf Z}_y \end{align*}

This is identical to the formula ealier, I've just used the $${\bf D}_x \otimes {\bf I}$$ notation rather than $$\frac{\partial}{\partial x} {\bf I}$$.

## Summary

That's all the math that's needed. It looks like a lot but ends up being surprisingly little code. In the next part I work through the implementation.

← Previous Page 2 of 2