# Closed Form Solution for Optimization Problem over the Space of Rigid Transforms

Is there a closed form solution to this constrained optimization problem:

$$$$\min_{R \in SO(3),\, \mathbf t \in \mathbb R^3} = \sum_{i = 1}^N \| M_i(R \mathbf p_i + \mathbf t) \|^2,$$$$

where $$M_i$$ are given $$3 \times 3$$ matrices and $$\mathbf p_i \in \mathbb R^3$$ are given vectors.

To put it in words, I am interested in the rigid transform, given by a $$3 \times 3$$ rotation matrix $$R$$ and a translation vector $$\mathbf t \in \mathbb R^3$$, which minimize the above expression.

It is also fine to minimize the norms, instead of the squared norms, if it makes it any easier.

• This looks a bit like a Lagrange multiplier type problem no? – DCM Oct 18 '18 at 20:49
• To clarify what I mean above: I'd be inclined (perhaps unhelpfully) to think of this as minimising $f:\mathbb{R}^{4 \times 4}\to \mathbb{R}, A\mapsto \sum_i\left|\left( \begin{array}{cc} M_i & 0 \\ 0 & 1 \end{array}\right) A \left( \begin{array}{c} p_i \\ 1 \end{array}\right)\right|^2$ subject to some constraints which keep $A$ on the manifold of rigid motions. I don't claim to have any expertise in this area, but I like the question so I thought I'd post in case it helps (sorry if you've already tried something like this). – DCM Oct 18 '18 at 21:05
• Thanks for the comment, but I do not see how this can help... The constraints for keeping A on the manifold of rigid motions seem to be the hard part. – user3749105 Oct 19 '18 at 17:21
• Re. constraints, we do have the coordinate functions $x_{ij}: \mathbb{R}^{4\times 4} \to \mathbb{R}, A\mapsto A_{ij}$ to help out with these, so I'd be surprised if it's really that bad (the equations you get out of the Lagrange multiplier method might be horrible of course). Alternatively, there is a (I think surjective) exponential map $\mathbb{R}^3 \times \mathbb{R}^3 \to SE(3)$ available; it's use would make this an unconstrained problem on $\mathbb{R}^3 \times \mathbb{R}^3$. I'll stop being so lazy and try some of this tomorrow :) – DCM Oct 19 '18 at 22:56
• Recall that I am interested in a closed form solution :) But in any case, let us know about your results. – user3749105 Oct 20 '18 at 0:26

1. Find the derivative of the function $$f: \mathbb{R}^{4\times 4}\to \mathbb{R}\;,\; A\mapsto \sum_i\left|\left( \begin{array}{cc} M_i & 0 \\ 0 & 1 \end{array}\right) A \left( \begin{array}{c} p_i \\ 1 \end{array}\right)\right|^2$$. I think this is given by $$Df(A)B = 2\sum_i \left( \left( \begin{array}{cc} M_i & 0 \\ 0 & 1 \end{array}\right) A \left( \begin{array}{c} p_i \\ 1 \end{array}\right), \left( \begin{array}{cc} M_i & 0 \\ 0 & 1 \end{array}\right) B \left( \begin{array}{c} p_i \\ 1 \end{array}\right) \right)$$ for $$A,B\in \mathbb{R}^{4\times 4}$$, where $$(.,.)$$ is the usual dot product on $$\mathbb{R}^4$$.
2. Identify the tangent spaces $$(T_AX)_{A\in X}$$ of the manifold $$X$$ over which we're optimising. In our case, $$X = SE(3)$$, and (I think): $$T_AX = \left\{ A \left( \begin{array}{cc} \widehat{\omega} & \tau \\ 0 & 0 \end{array}\right) : \omega, \tau \in \mathbb{R}^3\right\}\;\;\mbox{with}\;\; \widehat{\omega} = \left( \begin{array}{ccc} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{array} \right).$$
3. Observe (or believe, or neither) that if $$A\in X$$ is a maximum or minimum of $$f$$ then $$Df(A)[T_AX]=\{0\}$$. Letting $$df_A$$ be the restriction of $$Df(A)$$ to $$T_AX$$, this is the same as having $$df_A = 0$$ at extrema (I mention this just because derivatives being zero at extrema is a familiar idea from single variable calculus).
4. Combine items 1., 2. and 3. to see that, at an extremum $$A$$ of $$f$$ on $$X$$, one must have $$0=\sum_i \left( \left( \begin{array}{cc} M_i & 0 \\ 0 & 1 \end{array}\right) A \left( \begin{array}{c} p_i \\ 1 \end{array}\right), \left( \begin{array}{cc} M_i & 0 \\ 0 & 1 \end{array}\right) A \left( \begin{array}{c} (\omega \times p_i) +\tau \\ 0 \end{array}\right) \right)$$ for all $$\omega, \tau \in \mathbb{R}^3$$, where $$\times$$ is the usual cross product in $$\mathbb{R}^3$$.
5. Substitute $$A=\left( \begin{array}{cc} R & t \\ 0 & 1 \end{array}\right)$$ to get an equation satisfied by $$R$$ and $$t$$ at extrema. Unless I've bodged the algebra, I think this ends up being $$\sum_i M_i^TM_i (Rp_i +t ) = 0$$
6. Given the special form of your $$M_i (= \widehat{u_i}^2)$$, we have $$M_i^TM_i = -M_i$$, so this is just the same as $$\sum_i M_i(Rp_i + t) = 0$$ at any minimising/maximising $$R$$ and $$t$$. This condition is necessary but not sufficient (again, assuming there isn't a mistake in my algebra). There may be cases where this condition restricts your choice of $$R$$ and $$t$$ to just one, but I don't think this is the usual case. It seems like there may even be no minimising $$R$$ and $$t$$ for some choices of $$p_i$$ and $$u_i$$ if the calculations above are correct ($$SE(3)$$ isn't compact, so this isn't too much of a red flag).