# What would be a good approach to solving this large data non-linear least squares optimisation

## Introduction to Problem

I'm using a Truncated Signed Distance Function to perform 3D reconstruction from depth images.

Essentially I have a large voxel grid where each voxel contains the signed distance to an implicit surface. The surface can be extracted form the Voxel Grid using a marching cubes algorithm to generate a mesh. The vertices of this generated mesh lie on the edges of the cubes formed by joining the central points of 8 adjacent voxels.

Now the volume that I'm reconstructing is moving in a non-rigid way. I am able to track the motion of points on the surface of the object. In this way I can determine how a particular point on the mesh moved between one frame and another. I want to use this information to generate the effective transformation (translation) of each voxel in the voxel grid which would lead to the deformation of the implicit mesh which most closely matches the observed scene.

I have the situation described below: $v_0, v_1, \cdots, v_m$ are points in 3D space. They initially lie on a regular lattice with width $w$, height $h$ and depth $d$ so that $m = whd$.

Other than at the boundaries of the grid, each $v_x$ is connected to 6 neighbouring points:

$v_{x-1}, v_{x+1}, v_{x-wh}, v_{x+wh}, v_{x+h}$ and $v_{x-w}$.

The set of points $p_{a,b}$ lie on the edges connecting pairs of vertices $v_a$ and $v_b$. I have at most one point per edge that I care about but for some edges, I have no point. For example, in the image, the edges connecting $v_{wh}$ to $v_{wh+1}$ and $v_{wh}$ to $v_{2wh}$ have no point defined.

For each defined point, we can define $\alpha$ (green in the image), which is the proportion of the way along the edge from the first vertex to the second. So for example, $p_{0,1}$ has $\alpha$ equal to

$$\alpha = \frac{|v_0 p_{0,1}|}{|v_0 v_1|}$$

## The problem

The grid as shown represents the situation at time 0.

At time 1, I require each of the each of the points $p_{a,b}$ to be in a new location, $p'_{a,b}$ (known) which requires each of these points to undergo a translation. The overall transformation of the point cloud is not rigid, i.e. each point will be translated by a different amount.

I can only translate the points $p_{a,b}$ by moving the vertices $v_x$. When I do this, the distances between neighbouring vertices can change arbitrarily however the values of $\alpha$ computed after remain constant, i.e. the point $p_{a,b}$ still divides the edge $v_{a} v_{b}$ in the ratio $\alpha:(1 - \alpha)$.

I want to work out the ‘best' translations for $v_0, \cdots, v_m$ which will :

• Get each of the points $p$ as close as possible to its desired position; and
• Minimise the differences between the translations applied to neighbouring $v$s

## What I think is true

I need to construct an energy equation comprising a data term and a regularisation term and then minimise this equation.

The data term, $E_\text{data}$ minimises the squared difference between the desired coordinates of $p'_{a,b}$ and the computed values of $p'_{a,b}$ which are given by $(1 - \alpha) v_a + \alpha v_b$

$$E_\text{data} = \sum_{a, b} (((1 - \alpha) v_a + \alpha v_b) - p'_{a,b})^2$$

The sum is across all edges and so each $v'_a$ and $v' _b$ may be involved in multiple terms.

The regularisation term $E_\text{reg}$ will aim to minimise the differences between vertex transforms and will be

$$E_\text{reg} = \sum_{i=0}^m \sum_{j\in N(i)} (|t_i| - |t_j|)^2$$

Where $N(i)$ is the neighbourhood of i and comprises all of the adjacent vertices.

Note that there are more regularisation terms ($m$) than there are data terms (because there’s not one $p$ per $v$).

## My questions

I’m thinking of using non-linear least squares to solve this. I’ve written code which does this for a simpler problem in MATLAB and it works fine. The simpler problem uses :

• Only 2 dimensions
• A very small grid
• The built in MATLAB lsqnonlin function.

Now I want to build my own code which can solve this for the full scale problem in 3D but I have some concerns:

• It seems like using Newton’s method should work however to compute the gradient vector Hessian matrix it feels like I need the same number of data and regularisation terms so I need to express $E_\text{data}$ in terms of $v_0, \cdots, v_m$ but am unsure how to do this. Is this required ?
• Given that $m$ is of the order of tens, if not hundreds of thousands, computing the Hessian and it’s inverse seems like it will take a very long time. The Hessian should be sparse but is there a better way to solve this ?
• It feels like avoiding constructing matrices altogether would be beneficial but is there a simple (to understand) way of achieving this ?
• I feel that using CUDA to parallelize the problem would help with performance but again, I'm not sure how I would go about crafting the algorithm to use.

Can anyone point me at appropriate literature or suggest a good starting point for research ?

## References

1. Curless, Brian, and Marc Levoy. "A volumetric method for building complex models from range images." Proceedings of the 23rd annual conference on Computer graphics and interactive techniques. ACM, 1996.
• Hi Scoobadoob, I edited your post to include the image inline as you wanted to do but couldn't due to reputation. I hope you don't mind. Oct 1, 2016 at 7:09
• Could you add a short introduction explaining the big picture of what you are trying to accomplish (with only words, no equations)? There is a lot of notation and details here (which is good), but it is hard for me to understand what is going on without first having a basic idea of what the problem is. Oct 1, 2016 at 7:15
• Thanks Nick for your edit and your feedback. Sorry if this is too long winded, I wanted to be complete in my formulations of the problem I'm facing. Oct 1, 2016 at 11:47
• You can use MathJax to format your equations using LaTeX syntax. I reformatted the equations, but a couple of them had mismatched parenthesis, so you would like to double check them. Oct 1, 2016 at 17:57

I don't know much about tracking implicit surfaces, so I'm just going to start with the optimization problem and go from there.

The optimization problem is, at the core, nonlinear least squares, and can therefore be solved efficiently by the Gauss-Newton method, wherein at each step one linearizes the problem at the current guess, solves the linear least squares problem for this linearization, then uses the solution as the initial guess for the next step.

At each step, the linear least squares problem can be recast as solving one large sparse linear system. Figuring out what the coefficient matrix is for this linear system is a little mindbending, but doable with some matrix vectorization tricks (explanation below).

Once the matrix is constructed, the linear system can be solved efficiently with any good sparse-direct factorization technique up to quite large problem sizes. Furthermore, this matrix is physically analogous to the matrices that arise when trying to find the minimum energy configuration of a spring network, and so if the problem is extremely huge (too big for a sparse-direct factorization to fit in memory) it should still be amenable to multigrid.

This optimization problem has the interesting property that the data misfit term is linear (and therefore stays the same regardless of the iteration), whereas the regularization term is nonlinear (and therefore changes from iteration to iteration). Usually it is exactly the opposite, where the data misfit is nonlinear but the regularization is linear.

## Optimization problem

In what follows, let $N$ be the total number of grid vertices, and let $M$ be the total number of target points $p'$.

If I read everything correctly, the optimization problem can be recast as:

$$\min_{V'} \frac{1}{2}||\hat{Q}'(V') - P'||^2_\text{fro} + \frac{1}{2}||D~m(V'-V)||^2_\text{fro}$$

In this formula,

• $||\cdot||_\text{fro}$ is the Frobenius matrix norm (square root of sum of squares of all the entries of the matrix),
• $V$ is the $3$-by-$N$ matrix where each column is one of the vertices $v$ in the initial configuration.
• $V'$ is the $3$-by-$N$ matrix where each column is one of the vertices $v'$ of the moved configuration.
• $P'$ is the $3$-by-$M$ matrix where each column is one of the target points $p'$.
• $\hat{Q}'(V')$ is the $3$-by-$M$ matrix of approximate target points, $q' \approx p'$, based on a given choice of moved vertices $V'$.
• $D$ is the $M$-by-$N$ graph finite difference matrix that takes in a vector of values on the nodes of a graph, and outputs a list of differences across the edges. The $j$'th entry of the output is the difference between the value at the head vertex and the value at the tail vertex for edge $e_j$. I.e., the $i$'th row of this matrix has $1$ in the column corresponding to the "head" vertex for edge $e_i$, $-1$ in the column corresponding to the "tail" vertex for edge $e_i$, and zeros elsewhere.
• $m:\mathbb{R}^{3\times N}\rightarrow \mathbb{R^N}$ is the columnwise length measuring function. I.e, if $X$ is a matrix with columns $x_i$, then the $i$'th element of $m(X)$ is $||x_i||$.

## Data misfit term

Suppose the $j$'th approximate vertex, $q_j$, lies on an edge, where the "tail" of the edge is a moved vertex $v'_\text{tail}$, and the "head" of the edge is a moved vertex $v'_\text{head}$. Then from the problem statement, $$q = (1-\alpha_j)v'_\text{head} + \alpha_j v'_\text{tail}.$$

Then, applying this transformation to all vertices at once, we have: $$Q'(V') = (1 - A).*(V' E_\text{heads}) + A.*(V' E_\text{tails}),$$ where

• "$.*$" is MATLAB-style notation for the elementwise (Hadamard) matrix product.
• $A$ is the matrix where each column is constant, and the $j$'th column contains copies of $\alpha_j$ repeated vertically,
• $1$ represents the matrix of all ones, of the same size as $A$ (not the identity matrix),
• $E_\text{heads}$ and $E_\text{tails}$ are the column selection matrices, that, when acting on the matrix $V'$ from the right, generate the matrices whose $j$'th columns are the "head" and "tail" vectors for $q_j$, respectively. More details on these column selection matrices later..

Element-wise multiplication of matrices is the same as multipying the vectorized form of one of the matrices by a big diagonal matrix whose diagonal entries come from the other matrix. Thus if we vectorizes (unrolls) the matrix $Q'(V')$ into one long vector, "$\text{vec}(Q'(V')$", we can write it as follows: $$\text{vec}(Q'(V')) = (I_{3M}-D)~\text{vec}(V' E_\text{heads}) + D~\text{vec}(V' E_\text{tails}),$$ where $D:=\text{diag}(\text{vec}(A))$ is a very big diagonal matrix whose diagonal entries are the entries of $A$, and the notation $I_{k}$ denotes a $k$-by-$k$ identity matrix.

Furthermore, recall the following relationship between vectorization $\text{vec}(\cdot)$ and Kronecker products $\otimes$: $$\text{vec}(ABC) = (C^T \otimes A)\text{vec}(B).$$ Applying this identity to our expression above, we have: $$\text{vec}(Q'(V')) = (I_{3M}-D)~(E_\text{heads}^T \otimes I_3)~\text{vec}(V') + D~(E_\text{tails}^T \otimes I_3)~\text{vec}(V'),$$ or $$\text{vec}(Q'(V')) = J~ \text{vec}(V'),$$ where $$\boxed{J:=(I_{3M}-D)~(E_\text{heads}^T \otimes I_3) + D~(E_\text{tails}^T \otimes I_3)}$$ is the matrix that describes the vectorized transformation from all moved $v'$ vertices to all approximate target points $q'$. This matrix is extremely large, but also extremely sparse. (Also very easy to construct in MATLAB from built-in functions)

## Regularization term

Recall that the regularization term is: $$\frac{1}{2}||D~m(V'-V)||^2_\text{fro},$$ where, $m(\cdot)$ is the function that takes in a $3$-by-$N$ matrix, and outputs the vector containing the lengths of each column of the input matrix. I.e., $$m(X) = \begin{bmatrix} ||x_1|| \\ ||x_2|| \\ \vdots \\ ||x_N|| \\ \end{bmatrix}$$ We seek to linearize this term about some initial guess, $V'_0$, for the moved point $V'$.

Using the formula for the derivative of the norm, we can compute the the derivative of $m$ at point $Y$ in direction $\delta X$ as follows: \begin{align} \left.\frac{dm}{dX}\right|_{Y}~\delta X &= \begin{bmatrix} \frac{y_1^T \delta x_1}{||y_1||} \\ \frac{y_2^T \delta x_2}{||y_2||} \\ \vdots \\ \frac{y_N^T \delta x_N}{||y_N||} \\ \end{bmatrix} \\ &= \begin{bmatrix} \frac{y_1^T}{||y_1||} \\ & \frac{y_2^T}{||y_2||} \\ & & \ddots \\ & & & \frac{y_N^T}{||y_N||} \end{bmatrix} \begin{bmatrix} \delta x_1 \\ \delta x_2 \\ \vdots \\ \delta x_N \end{bmatrix} \\ &= M(Y)~\text{vec}(\delta X), \end{align} where $M(Y)$ is the big $N$-by-$3N$ matrix shown in the middle formula. I.e., $$M(Y)=\text{blockdiag}\left(\frac{y_1^T}{||y_1||}, \frac{y_2^T}{||y_2||}, \dots, \frac{y_N^T}{||y_N||}\right).$$

Hence, the regularization function can be linearized about an initial point $V'_0 \approx V'$ as follows, $$D~m(V'-V) = D~m(V_0' - V) + D~M(V_0' - V)~\text{vec}(V' - V'_0) + O(||V' - V'_0||^2),$$ Defining $$\boxed{R(V, V_0') := D~M(V_0' - V)}$$ and $$\boxed{r_0(V, V'_0) := D~m(V_0' - V) - R(V, V_0')~\text{vec}(V'_0),}$$ the linearization of the regularization term can be written as follows: $$D~m(V'-V) = r_0(V, V_0') + R(V, V_0')~\text{vec}(V') + O(||V' - V'_0||^2).$$

## Linearized optimization problem

Hence, the original optimization problem, when the regularization is linearized about a point $V'_0$, can be recast in vectorized form as follows: $$\min_{\text{vec}(V')} \frac{1}{2}||J~\text{vec}(V') - \text{vec}(P')||^2 + \frac{1}{2}||R~\text{vec}(V') + r_0||^2,$$ where we abbreviate $R:=R(V, V_0')$, and $r_0:=r_0(V, V_0')$. This linear least squares problem can be solved by solving the normal equations system: $$\boxed{(J^TJ + R^TR)~\text{vec}(V') = J^T~\text{vec}(P') - R^T~r_0.}$$ So, you can form solve this sparse system for $\text{vec}(V')$, then reshape the result from a vector into a matrix, and then extract the columns of this matrix as the desired vertices $v'$.

This will give you the optimal moved vertices based on the linearization of the regularization about point $V'_0$. To solve the problem with general nonlinear regularization, you can iterate: linearize about an initial guess, solve the linearized problem, use the solution as the next point to linearize about, solve that problem, and repeat until convergence. This is the Gauss-Newton method.

A side note of interest is that the graph Laplacian $D^TD$ appears within the core of the $R^TR$, so this regularization could be interpreted as some form of grouped discrete smoothing regularization.

## Heads and tails selection matrices

Above we glossed over the construction of the "heads" and "tails" selection matrices $E_\text{heads}$ and $E_\text{tails}$, so more details are provided here.

Let $e_\text{heads}$ and $e_\text{tails}$ be length-$M$ vectors representing the "head-for-$p$" map and "tail-for-$p$" maps. That is, if $p_j$ lies on the edge going from vertex $v_a$ to vertex $v_b$, then the $j$'th entry of $e_\text{heads}$ is the index $b$, and the $j$'th entry of $e_\text{tails}$ is the index $a$.

For example, suppose the first point lies on the edge going from vertex $v_3$ to $v_8$, the second point lies on the edge going from vertex $v_5$ to $v_6$, and the third point lies on the edge going from $v_9$ to $v_5$ (this is a made up example to illustrate the point). Schematically, \begin{align} v_3 &\overset{p_1}{\rightarrow} v_8 \\ v_5 &\overset{p_2}{\rightarrow} v_6 \\ v_9 &\overset{p_3}{\rightarrow} v_5. \end{align} Then the "edge-to-head" and "edge-to-tail" vectors would start: \begin{align} e_\text{tails}=&\begin{bmatrix}8, & 6, & 5, & \dots\end{bmatrix} \\ e_\text{heads}=&\begin{bmatrix}3, & 5, & 9, & \dots\end{bmatrix}. \end{align}

We want to find the selection matrices $E_\text{heads}$ and $E_\text{tails}$ such that: $$\begin{bmatrix}v_1 & v_2 & \dots & v_N\end{bmatrix} E_\text{heads} = \begin{bmatrix}v_{e_\text{heads}(1)} & v_{e_\text{heads}(2)} & \dots & v_{e_\text{heads}(M)}\end{bmatrix},$$ and $$\begin{bmatrix}v_1 & v_2 & \dots & v_N\end{bmatrix} E_\text{tails} = \begin{bmatrix}v_{e_\text{tails}(1)} & v_{e_\text{tails}(2)} & \dots & v_{e_\text{tails}(M)}\end{bmatrix}.$$

The matrix $E_\text{heads}$ that does this is the sparse $N$-by-$M$ matrix, where the $j$'th column has a $1$ in row $e_\text{heads}(j)$ (and likewise for $E_\text{tails}$, except with the row indices drawn from $e_\text{tails}$).

For example, consider the following product of a matrix whose columns are the vectors $x_1$, $x_2$, and $x_3$, with a selection matrix: $$\begin{bmatrix} x_1 & x_2 & x_3 \end{bmatrix} \begin{bmatrix} & & 1\\ & & & 1 \\ 1 & 1 & & \end{bmatrix} = \begin{bmatrix} x_3 & x_3 & x_1 & x_2 \end{bmatrix}.$$

• Thanks Nick for the extremely detailed response and for the time spent composing it. I certainly would not have got through the vectorisation. This does seem to make life much easier. I'll report back if as and when I get through implementing. In the meantime, can I check that E_heads is M-by-N? It seems as though it should be N-by-M as we are right multiplying it by a vector [v_1 v_2 ... v_N] Oct 5, 2016 at 10:35
• Also, and this may be confusion on my part, in the definition of J, the term I appears twice. It seems as though these are different I's. Am I right in thinking that the first I has dimensions equal to length vec(V'E_head) which is 3M whilst the second I (introduced to allow the Kronecker products is 3x3 as it must be able to premultiply V' which is 3xN) ? Or am I misinterpreting something here ? Oct 5, 2016 at 11:28
• @Scoobadood Yes on all points. The same identity symbol was used for both the $3M$-by-$3M$ identity and the $3$-by-$3$ identity, and E_heads should be N-by-M. Oct 5, 2016 at 11:44
• Ok, I edited the post to fix those points. However, looking at the post with fresh eyes, I actually don't think the regularization term in your original question is the same as the identity regularization I wrote here. So, this post is incomplete, as it only correctly addresses the data misfit term in the objective function. Linearizing and vectorizing the regularization should be possible with similar techniques and vectorization tricks (I think?), but it would take quite a bit of pencil and paper work to figure it out. Oct 5, 2016 at 11:57
• @Scoobadood Ok, I wrote up how to linearize and vectorize the correct regularization term. Oct 6, 2016 at 1:00