Shortest Path and Minimum Curvature Path - implementation

Solution 1:

Equation 6

By (5) you have formulas for the displacement in $x$ and in $y$ direction. These are simply numbers. The squared length is the sum of the squares of these displacements, by Pythagoras. That squared length is then written as a function of a two-element parameter vector $\alpha_i$. It is quadratic in $\alpha$, so it can be split into a matrix which gets multiplied by $\alpha_i$ from both sides, a vector which gets multiplied by $\alpha_i$ in what is essentially a dot product, and a constant part which does not depend on $\alpha_i$ at all. This separation is what (6) does.

Let's have a closer look. Remember that the form of your $x$ displacement was this:

$$ \Delta P_{x,i} = \begin{pmatrix}\Delta x_{i+1}\\-\Delta x_i\end{pmatrix}^{\mathrm T}\alpha_i + \Delta x_{i,0} = \alpha_i^{\mathrm T}\begin{pmatrix}\Delta x_{i+1}\\-\Delta x_i\end{pmatrix} + \Delta x_{i,0} $$

Lets introduce some abbreviations for the reoccurring vectors here:

$$ \beta_i := \begin{pmatrix}\Delta x_{i+1}\\-\Delta x_i\end{pmatrix} \\ \gamma_i := \begin{pmatrix}\Delta y_{i+1}\\-\Delta y_i\end{pmatrix} $$

Now you can write

$$ \Delta P_{x,i} = \beta_i^{\mathrm T}\alpha_i + \Delta x_{i,0} \\ \Delta P_{y,i} = \gamma_i^{\mathrm T}\alpha_i + \Delta y_{i,0} $$

and with these you can compute the squared length as the sum of the squared displacements in the two coordinate directions like this:

\begin{align*} \Delta P_{x,i}^2 + \Delta P_{y,i}^2 &= \left(\beta_i^{\mathrm T}\alpha_i + \Delta x_{i,0}\right)^2 + \left(\gamma_i^{\mathrm T}\alpha_i + \Delta y_{i,0}\right)^2 \\&= \left(\beta_i^{\mathrm T}\alpha_i\right)^2 + 2\left(\beta_i^{\mathrm T}\alpha_i\right)\Delta x_{i,0} + \Delta x_{i,0}^2 + \left(\gamma_i^{\mathrm T}\alpha_i\right)^2 + 2\left(\gamma_i^{\mathrm T}\alpha_i\right)\Delta y_{i,0} + \Delta y_{i,0}^2 \\&= \alpha_i^{\mathrm T}\beta_i\beta_i^{\mathrm T}\alpha_i + 2\Delta x_{i,0}\beta_i^{\mathrm T}\alpha_i + \Delta x_{i,0}^2 + \alpha_i^{\mathrm T}\gamma_i\gamma_i^{\mathrm T}\alpha_i + 2\Delta y_{i,0}\gamma_i^{\mathrm T}\alpha_i + \Delta y_{i,0}^2 \\&= \alpha_i^{\mathrm T}\left( \beta_i\beta_i^{\mathrm T} + \gamma_i\gamma_i^{\mathrm T} \right)\alpha_i + \left( 2\Delta x_{i,0}\beta_i^{\mathrm T} + 2\Delta y_{i,0}\gamma_i^{\mathrm T} \right)\alpha_i + \left( \Delta x_{i,0}^2 + \Delta y_{i,0}^2 \right) \end{align*}

So here you see that $H_{S,i}$ is the matrix

$$ H_{S,i}:=\beta_i\beta_i^{\mathrm T} + \gamma_i\gamma_i^{\mathrm T} $$

and $B_{S,i}$ is the row vector

$$ B_{S,i}:= 2\Delta x_{i,0}\beta_i^{\mathrm T} + 2\Delta y_{i,0}\gamma_i^{\mathrm T} $$

while the constant term is the last parenthesis and irrelevant for the subsequent optimization.

Equation 7

Based on this, (7) expresses $\alpha_i$ as the matrix-times-vector product

$$\alpha_i = E_i\alpha = \begin{pmatrix}\alpha_{i+1}\\\alpha_i\end{pmatrix}$$

The matrix $E_i$ here simply has two rows and $n$ columns. The first row has a $1$ in column $i+1$ and the second a $1$ in column $i$. This matrix $E_i$ can be combined with the matrices of the previous step in order to obtain an expression in $\alpha$, without the need to select elements based on $i$. With this formulation, the sum of products with matrices can be turned into a product with sums of matrices, using

\begin{align*} H_S &:= \sum_{i=1}^n E_i^{\mathrm T}H_{S,i}E_i \\ B_S &:= \sum_{i=1}^n B_{S,i}E_i \end{align*}

Minimum curvature

Explaining pretty much every equation in the MCP case is beyond the scope of this post. The above should give you an idea about the kind of transformations required. If you need more help, please ask more specific questions separately.