# Davenport's Q-method (Finding an orientation matching a set of point samples)

I have an initial set of 3D positions that form a shape. After letting them move independently, my goal is to find the best rotation of the original configuration to try to match the current state. This is for a soft body physics simulation, the idea being that if I can construct an optimal 'rigid' frame for the deformed shape then I can apply a shape matching constraint that removes deformation without introducing energy.

Existing solutions tend to find the optimal linear transformation representing the deformation, and then use various methods to decompose the matrix into rotation and scale/shear components. However, I found the orientations provided by such methods tended to not be very stable. After significant searching I discovered that my problem was identical to a problem solved by NASA to determine satellite orientations. When I implemented their solution my simulation was remarkably stable. I want to gain a better understanding of why it works.

Details of Davenport's Q-method are here. Somehow, after taking a bunch of outer, cross and dot products of the original and deformed samples, jamming them into a symmetric 4x4 matrix, and then computing the eigenbasis for that matrix, the eigenvector corresponding to the largest eigenvalue can be reinterpreted as a quaternion that is the best orientation to use. The author of the linked paper claims this result is easy to prove, but I guess easy is relative. Can anyone walk me through why this works?

Since nobody has answered this yet and its been more than a year I'll take a stab. I'll apologize for my engineery answer from the start.

# Problem Description

The Davenport Q-Method Solution is a solution to what is referred to as Wahba's Problem, which was proposed by Grace Wahba in 1965(Wahba Paper). Wahba's problem is to find the rotation matrix that minimizes the cost function $$\min_{\mathbf{T}} J(\mathbf{T})=\frac{1}{2}\sum_{i}{w_i}\left\|\mathbf{b}_i-\mathbf{T}\mathbf{a}_i\right\|^2$$ where $\mathbf{a}_i$ are a set of unit vectors expressed in frame $A$, $\mathbf{b}_i$ are the same set of unit vectors expressed in frame $B$, $\mathbf{T}$ is the rotation matrix to transform from frame $A$ to $B$, and $w_i$ is some weight corresponding to each vector pair (usually set to be the inverse of the variance of the measurement that your vectors are generated from). Note that the $1/2$ comes from the maximum likelihood estimate (MLE) formulation of Wahba's problem. Since it is just a constant multiplier, it will have no effect on the minimization problem so we can ignore it in future steps. This is a constrained minimization problem, with the constraint that $$\mathbf{T}^{-1}=\mathbf{T}^T,\qquad\left\|\mathbf{T}\right\|=1$$

# The Q-Method Solution

## Linear Algebra Transformations

In 1968, Davenport came up with a solution to Wahba's problem using attitude quaternions (Davenport Paper). To get to Davenport's solution we need to manipulate the cost function. First, express the vector norm as an inner product $$\min_T J(\mathbf{T}) = \sum_i{w_i(\mathbf{b}_i-\mathbf{T}\mathbf{a}_i)^T(\mathbf{b}_i-\mathbf{T}\mathbf{a}_i)}$$ Distributing the multiplication and recalling (a) that $\mathbf{a}_i$ and $\mathbf{b}_i$ are unit vectors (and thus their inner product with themselves is 1), (b) the constraint that $\mathbf{T}^T\mathbf{T}=\mathbf{I}$, and that an inner product is a scalar and thus symmetric (that is $\mathbf{a}_i^T\mathbf{b}_i=\mathbf{b}_i^T\mathbf{a}_i$) the minimization problem can be written as $$\min_{\mathbf{T}}J(\mathbf{T})=\sum_i{2w_i(1-\mathbf{b}_i^T\mathbf{T}\mathbf{a}_i})$$ Dropping the constant multiplier 2, and recognizing that $\sum{w_i}$ will have no effect on the minimization problem we can further write this as $$\min_{\mathbf{T}}J(\mathbf{T})=-\sum_i{w_i\mathbf{b}_i^T\mathbf{T}\mathbf{a}_i}$$

Now, making use of the fact that the trace operator is a linear operator, and that the trace of a scalar is the scalar, we can write $$\min_{\mathbf{T}}J(\mathbf{T})=-\text{Tr}\left[\sum_i{w_i\mathbf{b}_i^T\mathbf{T}\mathbf{a}_i}\right]$$ which, using the cyclic property of the trace can be written as $$\min_{\mathbf{T}}J(\mathbf{T})=-\text{Tr}\left[\mathbf{T}\sum_i{w_i\mathbf{a}_i\mathbf{b}_i^T}\right]=\text{Tr}\left[\mathbf{T}\mathbf{B}^T\right]$$ where $\mathbf{B}=\sum_i{w_i\mathbf{b}_i\mathbf{a}_i^T}$ is known as the attitude profile matrix.

We can now use the equation to convert an attitude quaternion to a rotation matrix $$\mathbf{T}=(q_s^2-\mathbf{q}_v^T\mathbf{q}_v)\mathbf{I}+2\mathbf{q}_v\mathbf{q}_v^T-2q_s\left[\mathbf{q}_v\times\right],$$ where $$\left[\mathbf{a}\times\right]=\left[\begin{array}{rrr}0 & -\mathbf{a}(3) & \mathbf{a}(2) \\ \mathbf{a}(3) & 0 & -\mathbf{a}(1) \\ -\mathbf{a}(2) & \mathbf{a}(1) & 0\end{array}\right]$$ is the skew-symmetric cross product matrix, $\mathbf{q}_v$ is the vector portion of the attitude quaternion, and $q_s$ is the scalar portion of the attitude quaternion, to substitute in for $\mathbf{T}$ $$\min_{\mathbf{q}}J(\mathbf{q})=-\text{Tr}\left[\left((q_s^2-\mathbf{q}_v^T\mathbf{q}_v)\mathbf{I}+2\mathbf{q}_v\mathbf{q}_v^T-2q_s\left[\mathbf{q}_v\times\right]\right)\mathbf{B}^T\right]$$ Distributing $\mathbf{B}^T$ and the trace operator leaves us with $$\min_{\mathbf{q}}J(\mathbf{q})=-(q_s^2-\mathbf{q}_v^T\mathbf{q}_v)\text{Tr}\left[\mathbf{B}^T\right]-2\text{Tr}\left[\mathbf{q}_v\mathbf{q}_v^T\mathbf{B}^T\right]+2q_s\text{Tr}\left[\left[\mathbf{q}_v\times\right]\mathbf{B}^T\right].$$

At this point it again becomes necessary to make use of trace properties (the cyclic property, the scalar property $\text{Tr}\left[a\right]=a$, and the transpose property $\text{Tr}\left[\mathbf{A}^T\right]=\text{Tr}\left[\mathbf{A}\right]$). Applying these properties we can simplify to $$\min_{\mathbf{q}}J(\mathbf{q})=-(q_s^2-\mathbf{q}_v^T\mathbf{q}_v)\text{Tr}\left[\mathbf{B}\right]-2\mathbf{q}_v^T\mathbf{B}\mathbf{q}_v+2q_s\text{Tr}\left[\left[\mathbf{q}_v\times\right]\mathbf{B}^T\right].$$ Further, recognizing that $2\mathbf{a}^T\mathbf{A}\mathbf{a}=\mathbf{a}^T(\mathbf{A}+\mathbf{A}^T)\mathbf{a}$ we can reduce this to $$\min_{\mathbf{q}}J(\mathbf{q})=-(q_s^2-\mathbf{q}_v^T\mathbf{q}_v)\text{Tr}\left[\mathbf{B}\right]-\mathbf{q}_v^T(\mathbf{B}+\mathbf{B}^T)\mathbf{q}_v+2q_s\text{Tr}\left[\left[\mathbf{q}_v\times\right]\mathbf{B}^T\right].$$

Examine the term $\text{Tr}\left[\left[\mathbf{q}_v\times\right]\mathbf{B}^T\right]$. Applying the operators it can be seen that $$\text{Tr}\left[\left[\mathbf{q}_v\times\right]\mathbf{B}^T\right]=\mathbf{q}_v(1)(\mathbf{B}(3, 2)-\mathbf{B}(2, 3))+\mathbf{q}_v(2)(\mathbf{B}(1, 3)-\mathbf{B}(3, 1))+\mathbf{q}_v(3)(\mathbf{B}(2, 1)-\mathbf{B}(1, 2))$$ Defining $$\mathbf{z}=\left[\begin{array}{ccc} \mathbf{B}(2, 3)-\mathbf{B}(3, 2) \\ \mathbf{B}(3, 1) - \mathbf{B}(1, 3) \\ \mathbf{B}(1, 2)-\mathbf{B}(2, 1)\end{array}\right]$$ (which implies that $\left[\mathbf{z}\times\right]=\mathbf{B}^T-\mathbf{B}$), then we can write $$\text{Tr}\left[\left[\mathbf{q}_v\times\right]\mathbf{B}^T\right]=-\mathbf{z}^T\mathbf{q}_v$$ and our minimization problem becomes $$\min_{\mathbf{q}}J(\mathbf{q})=-(q_s^2-\mathbf{q}_v^T\mathbf{q}_v)\text{Tr}\left[\mathbf{B}\right]-\mathbf{q}_v^T(\mathbf{B}+\mathbf{B}^T)\mathbf{q}_v-2q_s\mathbf{z}^T\mathbf{q}_v.$$

Defining $\mathbf{S}=\mathbf{B}+\mathbf{B}^T$ and $\mu=\text{Tr}\left[\mathbf{B}\right]$ and simplifying gives $$\min_{\mathbf{q}}J(\mathbf{q})=-\left(\mathbf{q}_v^T(\mathbf{S}-\mu\mathbf{I})\mathbf{q}_v+q_s\mathbf{z}^T\mathbf{q}_v+q_s\mathbf{q}_v^T\mathbf{z}+q_s^2\mu\right).$$ This can equivalently be written as an inner product $$\min_{\mathbf{q}}J(\mathbf{q})=-\left[\begin{array}{cc} \mathbf{q}_v^T(\mathbf{S}-\mu\mathbf{I})+q_s\mathbf{z}^T & \mathbf{q}_v^T\mathbf{z}+q_s\mu\end{array}\right]\left[\begin{array}{c} \mathbf{q}_v \\ q_s \end{array}\right].$$ Finally, we can write this as $$\min_{\mathbf{q}}J(\mathbf{q})=-\left[\begin{array}{cc} \mathbf{q}_v^T & q_s \end{array}\right]\left[\begin{array}{cc} \mathbf{S}-\mu\mathbf{I} & \mathbf{z} \\ \mathbf{z}^T & \mu\end{array}\right]\left[\begin{array}{c} \mathbf{q}_v \\ q_s \end{array}\right]=-\mathbf{q}^T\mathbf{K}\mathbf{q}$$ where $\mathbf{q}$ is the attitude quaternion (vector first), and $\mathbf{K}$ is the Davenport matrix.

## Optimization Problem

Having finally sufficiently simplified the cost function, we can now perfom a constrained minimization using a Lagrange multiplier to enforce the constraint that $\mathbf{q}^T\mathbf{q}=1$ $$\min_{\mathbf{q}\text{, }\lambda} J(\mathbf{q}\text{, }\lambda) =-\mathbf{q}^T\mathbf{K}\mathbf{q}+\lambda(\mathbf{q}^T\mathbf{q}-1)$$ Applying the first differential condition to this results in $$\mathbf{K}\mathbf{q}=\lambda\mathbf{q}$$ which is a 4x4 eigenvalue/eigenvector problem. The attitude quaternion that minimizes the cost function in the unit eigenvector corresponding to the largest (most positive eigenvalue). To understand this remember that the function we are minimizing is $$\min_{\mathbf{q}\text{, }\lambda} J(\mathbf{q}\text{, }\lambda) =-\mathbf{q}^T\mathbf{K}\mathbf{q}+\lambda(\mathbf{q}^T\mathbf{q}-1)$$ which, when $\mathbf{q}$ is a unit eigenvector of $\mathbf{K}$, simplifies to $$-\mathbf{q}^T\mathbf{K}\mathbf{q}+\lambda(\mathbf{q}^T\mathbf{q}-1)=-\lambda$$ since $\mathbf{q}^T\mathbf{K}\mathbf{q}=\lambda$ when $\mathbf{q}$ is an eigenvector of $\mathbf{K}$.

So overall, yes the derivation is simple in that it only requires relatively basic linear algebra/optimization but it is complex in that it requires a good deal of creativity in order to get everything into the proper form.

• Thanks for the breakdown, looking forward to going through it when I have a bit more time. I have since developed a bit more intuition about why this method works in terms of geometric algebra - thought process here: docs.google.com/document/d/… Major insight for me was that each point sample can be equally well explained by a complete circle of attitude quaternions, which is why methods which just contribute the shortest angular paths to rotate each point are less robust. – Jason Hise May 13 '17 at 3:49