Solve $A^{-1} B A = C$ for orthogonal matrices

Question

I need to solve $A^{-1} B A = C$ for $A$. All matrices are affine 3D transforms (represented as homogeneous matrices) with an orthogonal linear part. I.e. they are 4 x 4 matrices, the last row is $(0, 0, 0, 1)$ and the upper left 3 x 3 block is orthogonal. In case there is no orthogonal matrix $A$ that satisfies this equation, I want the orthogonal matrix that yields the smallest error $(A^{-1} B A - C)^2$.

I tried to solve this without the orthogonality constraint and enforcing it afterwards. For that, I solved a linear system (similar to this answer). Then I projected the linear part of the solution back into the space of orthogonal matrices by $M'=UV^T$, where $U$ and $V$ are the according factors of the solution $M$'s singular value decomposition.

However, the solution I get is far from what I want. The intermediate solution $M$ actually fulfills the initial equation but it is not near to orthogonal. Especially, the translation part is all zero, which does not make sense.

So basically, I am looking for a way to solve the equation that utilizes the orthogonality properties of all involved matrices. I know that I can represent the matrices differently (e.g. as Euler angles and a translation vector) and solve for those. But this becomes non-linear and cannot be solved directly.

Btw, I will have multiple of these equations in the form

$$ A^{-1}B_1 A=C_1\\ A^{-1}B_2 A=C_2\\ A^{-1}B_3 A=C_3\\ ... $$ And I want to solve these in a least-squares sense. But this will probably be easy once I have a reliable way to solve a single equation.

Update

Here is an example:

$$ B= \begin{bmatrix} 0.975509 & -0.0993911 & -0.196226 & -85.0091 \\ 0.0976203 & 0.995048 & -0.0187001 & 34.7605 \\ 0.197113 & -0.000913531 & 0.98038 & 93.3659 \\ 0 & 0 & 0 & 1 \end{bmatrix}\\ C= \begin{bmatrix} 0.973421 & 0.0742983 & -0.216638 & -6.63937 \\ -0.0674149 & 0.996963 & 0.0389824 & -21.1132 \\ 0.218876 & 0.0233417 & 0.975473 & 49.6126\\ 0 & 0 & 0 & 1 \end{bmatrix} $$ I know that the solution must be close to the following matrix: $$ \begin{bmatrix} 0.871881 & 0.0524241 & -0.486903 & -258.214\\ 0.328096 & 0.67559 & 0.660251 & -201.229 \\ 0.36356 & -0.735412 & 0.571834 & -143.373 \\ \end{bmatrix} $$ (This is actually the matrix that I want to correct slightly in my application)

To solve for $A$ with the notation of the Kronecker product, I got the following system:

$$ \begin{bmatrix} -0.0020879999999999788 & 0.0993911 & 0.196226 & -0.0674149 & 0. & 0. & 0.218876 & 0. & 0. & 0. & 0. & 0.\\-0.0976203 & -0.021627000000000063 & 0.0187001 & 0. & -0.0674149 & 0. & 0. & 0.218876 & 0. & 0. & 0. & 0.\\-0.197113 & 0.000913531 & -0.0069590000000000485 & 0. & 0. & -0.0674149 & 0. & 0. & 0.218876 & 0. & 0. & 0.\\0.0742983 & 0. & 0. & 0.021454000000000084 & 0.0993911 & 0.196226 & 0.0233417 & 0. & 0. & 0. & 0. & 0.\\0. & 0.0742983 & 0. & -0.0976203 & 0.001915 & 0.0187001 & 0. & 0.0233417 & 0. & 0. & 0. & 0.\\0. & 0. & 0.0742983 & -0.197113 & 0.000913531 & 0.016583000000000014 & 0. & 0. & 0.0233417 & 0. & 0. & 0.\\-0.216638 & 0. & 0. & 0.0389824 & 0. & 0. & -0.00003599999999992498 & 0.0993911 & 0.196226 & 0. & 0. & 0.\\0. & -0.216638 & 0. & 0. & 0.0389824 & 0. & -0.0976203 & -0.01957500000000001 & 0.0187001 & 0. & 0. & 0.\\0. & 0. & -0.216638 & 0. & 0. & 0.0389824 & -0.197113 & 0.000913531 & -0.004906999999999995 & 0. & 0. & 0.\\-6.63937 & 0. & 0. & -21.1132 & 0. & 0. & 49.6126 & 0. & 0. & 0.02449100000000004 & 0.0993911 & 0.196226\\0. & -6.63937 & 0. & 0. & -21.1132 & 0. & 0. & 49.6126 & 0. & -0.0976203 & 0.0049519999999999564 & 0.0187001\\0. & 0. & -6.63937 & 0. & 0. & -21.1132 & 0. & 0. & 49.6126 & -0.197113 & 0.000913531 & 0.01961999999999997 \end{bmatrix} vec(A_{12})= \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ -85.0091 \\ 34.7605 \\ 93.3659 \end{bmatrix} $$ $vec(A_{12})$ is the vector of the entries of $A$ (columns stacked on top of each other) with the last row removed.

However, this system has no solution - probably due to numerical issues. The least-squares solution is:

$$ \begin{bmatrix} 2.38109*10^{-8} & -3.49507*10^{-9} & 8.80459*10^{-9} & 3.08121*10^6 \\ -6.91407*10^{-9} & -2.75114*10^{-8} & 2.16676*10^{-10} & -6.8149*10^7 \\ -7.46204*10^{-9} & 5.65462*10^{-9} & 2.58108*10^{-8} & 3.41334*10^7 \\ 0 & 0 & 0 & 1 \end{bmatrix} $$ This is obviously not even close to being orthogonal.

Answer 1

Solving the linear equations $A C = BA$ and $A_{41}=0, \ldots, A_{44} = 1$. You may get a unique solution (in which case you check that the top left $3 \times 3$ matrix is orthogonal), or a parametric solution with some arbitrary variables (in which case you check what values the parameters must have to make the top left $3 \times 3$ matrix orthogonal). The latter will require solving some quadratic equations in the parameters: easy if there is just one parameter, more complicated if there are several.

EDIT: In your example, using Maple's LSSolve function with your given approximate solution as initial point, what I get as the best least-squares solution for the linear equations, using the quadratics as constraints, is

$$ \left[ \begin {array}{cccc} 0.922106169050555202& 0.0259062112679868568&- 0.386068752889484001&- 259.015722238283104 \\ 0.240075198859380001& 0.744181318602853081& 0.623344258082939584&- 197.239155098949084\\ 0.303453641653002204&- 0.667475118124205569& 0.679994746043087850&- 169.389845060954997\\ 0&0&0&1\end {array} \right] $$

It makes $$AC - BA = \left[ \begin {array}{cccc} - 0.00476617429607195664& 0.00304423895353944651&- 0.00335193095893870918&- 0.00000404358121386394487\\ - 0.00326758507934044040& 0.0188013774691542235& 0.0152027162800097981& 0.0000894215323796743178\\ 0.0101807673199222348& 0.0229229648659853291&- 0.0184274918922858744&- 0.0000447884023486722072\\ 0.0& 0.0& 0.0& 0.0 \end {array} \right] $$