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.

  • $\begingroup$ This looks a bit like a Lagrange multiplier type problem no? $\endgroup$
    – DCM
    Oct 18, 2018 at 20:49
  • $\begingroup$ 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). $\endgroup$
    – DCM
    Oct 18, 2018 at 21:05
  • $\begingroup$ 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. $\endgroup$ Oct 19, 2018 at 17:21
  • $\begingroup$ 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 :) $\endgroup$
    – DCM
    Oct 19, 2018 at 22:56
  • $\begingroup$ Recall that I am interested in a closed form solution :) But in any case, let us know about your results. $\endgroup$ Oct 20, 2018 at 0:26

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


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.