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

Question

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

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

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.

Answer 1

This is a bit too long for a comment, so I'll add this as an `answer'; you can decide for yourself whether this does anything towards answering your question. I should reiterate that I am by no means an expert in this area (so do call me out if you think I've made a mistake), but I think the steps here are:

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).

Do you have a reason to believe that your minimum can always be attained?

If you do decide to go down the numerical route, you might want to look at the paper Minimization on the Lie Group SO(3) and Related Manifolds of Taylor and Kreigman; in my experience, using local exponential parameterisations of your manifold is almost always better than coordinate-free methods (accumulation of numerical errors can mean your matrices stop being in the group you want after a few million SLERPs). What experience I have in this area comes from the direction of minimum-volume bounding box computation; if you're able to give a bit more background to your problem, I might be able to suggest some more bespoke references (obviously don't reveal anything which could be considered 'commercially sensitive').