Fitting a rotation matrix to corresponding points

When tracking a rigid body by following a bunch of points on the body, we need to be able to find the rotation matrix and translation vector that tell us the new orientation and position of the body relative to some canonical orientation and position.

Consider a collection of  $n$ points $A = [a_1 \cdots a_n]$ in $\mathbb{R}^m$ (where, it's likely that $m=3$).  If we assume that $A$ has been rotated by $R$, translated by $t$, and corrupted by Gaussian noise to produce a corresponding collection of points $B = [b_1 \cdots b_n]$ s.t. $b_i=Ra_i+t+w$ where $w\sim \mathcal{N}_m(0,cI)$.  If we want to recover $R$ and $t$, we can use Umeyama's paper to approximately do so.  For those not interested in the proofs, the key equations are numbers 34-42.

It boils down to this:

1. Find the mean and variance around the mean of both point sets (eqs. 34-37).
2. Find the covariance of the mean-centered point sets (eq. 38).
3. Take the singular value decomposition of the the covariance.
4. Replace the diagonal matrix of singular values with an identity matrix, negating the last value if the determinant of the covariance matrix is less-than zero (eq. 39).
5. Multiply the matrix back out and you have the rotation matrix, $R$ (eq. 40).
6. Rotate the mean of the first set and scale by the trace of the singular value matrix (with the same negation as in step 4).  Subtract this from the mean of the second set and you have the translation $t$ (eqs. 41-42). 

Step 4 will probably not be necessary in most cases for the purpose of tracking  a rigid body with motion capture markers. It is intended to prevent the algorithm from finding a mirrored copy of the rotation matrix in cases where the noise is particularly severe.  The scaling in step 6 should also be small if we're dealing with an actual rigid body; so we might optionally skip that part as well.

In pretty equations:

$\bar{a} =  \frac{1}{n}\sum_{i=1}^n a_i $
$\sigma_a^2 = \frac{1}{n}\sum_{i=1}^n \left\| a_i - \bar{a}\right\|^2$  note that this is not the covariance of the points around the mean, but rather the variance of the scalar distance from the mean.
$\Sigma_{ab} = \frac{1}{n}\sum_{i=1}^n (b_i - \bar{b})(a_i - \bar{a})^\text{T}$

$UDV^\text{T} = \text{SVD}(\Sigma_{ab})$
$S = I$ --- If $\text{det}(\Sigma_{ab})<0$ then $S_{mm}=-1$.
$c = \frac{1}{\sigma_a^2}\text{tr}(DS)$

We then find:
$R = USV^\text{T}$
$t = \bar{b} - cR\bar{a}$

In matlab, we have:

m = size(a,1);
n = size(a,2); 
abar = mean(a,2);
bbar = mean(b,2);
aa = a - repmat(abar, 1,n);
bb = b - repmat(bbar,1,n);
va = mean(sum(aa.*aa),2);
sig = bb*aa'*(1.0/n);
[U D V] = svd(sig);
S = eye(m);
if prod(diag(D))<0)
  S(m,m)=-1;
end
R = U*S*V';
c = trace(D*S)/va;
t = bbar - c*R*abar;

This seems to do the trick, and with a little understanding of the singular value decomposition, it's not too unintuitive.  Many thanks to Shinji Umeyama.

Open Dynamics Math

I found that I needed a better understanding of exactly what is going on in the Open Dynamics Engine physics simulator.  So I dove into the code.   Most of what's going on is pretty clear, but some of it is confusing and not precisely documented.  I decided to write about it to clarify things in my own mind.

Dynamic State

Given $n$ 3d bodies, the dynamic state of the $i^{\text{th}}$ body at time $t$ is its position, orientation, linear velocity, and angular velocity: $\left[\begin{array}{cccc}x_{it}&R_{it}&v_{it}&\omega_{it}\end{array}\right]$.  The body also has mass $m_i$ and inertia tensor $\mathcal{I}_i$.  The body also has geometry for the purposes of collision, but we won't worry about that here.  We will assume that all of these values are framed in the global coordinate system (this means that $\mathcal{I}$ is actually a function of time since it's affected by $R$, but we'll assume that's taken care of).

Integration Step

The dynamics equations start-out straightforward: $f = ma$.  A single 3d rigid body has six degrees of freedom; so we get $a = M^{-1}\left[\begin{array}{c} f\\ \tau \end{array}\right]$ where $M=\left[\begin{array}{cc}mI&0\\0&\mathcal{I}\end{array}\right]$ and $f$ and $\tau$ are force and torque.  After a time step of $h$ using semi-implicit Euler integration (and lumping gyroscopic effects into $\tau$), the future velocity is the current velocity plus the acceleration, scaled by time: $\left[\begin{array}{c}v_{t+h}\\ \omega_{t+h}\end{array}\right] = \left[\begin{array}{c}v_{t} \\ \omega_{t}\end{array}\right] + ha$.  Then the future position is the current position plus the future velocity scaled by time: $x_{t+h} = x_t + hv_{t+h}$ and $R_{t+h} = \mathcal{R}(h\omega_{t+h})R_t$ where $\mathcal{R}()$ is a function that makes a rotation matrix out of time-scaled angular velocity (when using ODE's FiniteRotationMode, otherwise, the orientation, in quaternion form, is updated the same way as position and then re-normalized).

Constraint Equation

So far, so good.  When a rigid body is constrained in some way, we need an extra step to figure out the forces resulting from constraints.  If our $n$ bodies are all part of a single interacting system and we constrain $k$ degrees of freedom, then we need to create a matrix $J$ of size $\left\{k \times 6n\right\}$.  Each row of $J$ describes a linear relationship between all of these bodies.

The ODE document on creating new joints does a good job describing how to compute individual rows of $J$.  Russell Smith (original author of ODE) expanded on this in "Constraints in rigid body dynamics" (Game Programming Gems, 2004).  Erin Catto also put together a nice paper that covers forming and resolving constraints.  This paper also describes the iterative Projected Gauss-Seidel algorithm for solving the constraint equations. The ODE function, dWorldQuickStep(), uses this method.  For my work, I prefer the dWorldStep() solver which uses a slower, but more accurate algorithm sometimes called Lemke's algorithm and sometimes called the Dantzig-Cottle simplex algorithm.  David Baraff seems to get credit for introducing this algorithm as a solver for physical simulation with contacts and friction.

In ODE, constraints start out being applied to velocities, but then are actually solved as accelerations.  A single "constraint row" applies to a single degree of freedom.  We frame a constraint stating that a body can't move along the $x$ axis as $[\begin{array}{cccccc}1&0&0&0&0&0\end{array}]\left[\begin{array}{c}v_{t} \\ \omega_{t}\end{array}\right] = 0$.  Solving this constraint is easy.  We just need a force that cancels out whatever forces and momentum exist along that axis.  This leads to zero acceleration (or acceleration canceling our current velocity) for a final velocity of zero. 

If we have many such constraints, it still boils down to solving a linear system.  When solved as a purely linear system, however, if we aren't careful and accidentally over-constrain the system, things break.  This is why ODE applies constraint-force mixing (CFM) component which, as discussed in previous posts, allows constraints to slip proportional to the force required to satisfy them.  If we turn to the ODE manual we see the $\text{CFM}$ augmented constraint framed as, $J\left[\begin{array}{c}v_{t+h} \\ \omega_{t+h}\end{array}\right] = c + \text{CFM}\lambda$.  The right hand side should actually read, $c - \text{CFM}\lambda$, because that is how it's used in the code.

The $j^{\text{th}}$ row of $J$ looks like this:
$\left[\begin{array}{ccccc}p_{j1}&q_{j1}&\cdots&p_{jn}&q_{jn}\end{array}\right]$
where $p_{ji}$ is applied to $v_i$ and $q_{ji}$ is applied to $\omega_i$.  Good examples exist in the sources cited above.  Each row of $J$ also has associated elements that define limits on the force ($\text{lo}_j$ and $\text{hi}_j$), springiness of each degree of freedom ($\text{cfm}_j$), the desired relative velocity along the constraint ($c_j$), and a reference to another row ($\text{findex}_j$) that modulates $\text{lo}_j$ and $\text{hi}_j$ for friction constraints (to make friction proportional to normal force). 

So we have our desired constraint relationships: $J\left[\begin{array}{ccccc}v_1&\omega_1&\cdots&v_n&\omega_n \end{array}\right]^\intercal = c - \text{CFM}\lambda$.  In ODE, the rows of $J$ are restricted to dealing with two or fewer bodies, with all other columns set to zero.  That simplifies the code, but there are interesting constraints that involve three or more bodies and you can't easily create those.

Final Equation in Terms of Acceleration

For the solver, we re-frame the system as constraints on acceleration.  When we project the velocity into acceleration, we divide both sides by time and then replace the left hand-side (acceleration) with inverse-mass times force: $JM^{-1}J^\intercal\lambda$ (here $M$ represents the diagonal mass and the rotated inertia tensor together).  Grouping terms involving $\lambda$, we get $\left(JM^{-1}J^\intercal + \frac{1}{h}\text{CFM}\right)\lambda =  \frac{1}{h}c$.  Since $c$ is the desired velocity, converting it into an acceleration this way assumes that the starting velocity is zero.  To correct for that, we subtract out the current velocity, projected onto the constraint space.  We also need to subtract out the effects of external forces, $F$ (gravity, gyroscopic torques, and any forces the user may have applied) since they affect the bodies' velocities.  This gives us the final equation:
$\left(JM^{-1}J^\intercal + \frac{1}{h}\text{CFM}\right)\lambda =  \frac{1}{h}c -J\left(M^{-1}F+\frac{1}{h}v_t\right)$.
where $\text{CFM}$ is a diagonal matrix and $F$ is the vector of externally applied forces and torques.

For convenience, we group the pieces of this equation into three variables: $A\lambda = b$. In spirit, this equation is still $m^{-1}f = a$, and we need to solve for $f$.  The solver finds $\lambda$ that satisfies the above system subject to constraints $\text{lo}\leq\lambda\leq\text{hi}$.  The solver algorithms are quite interesting, but are a topic for another day.  Once we have solved for $\lambda$, we project out of constraint space back into force space and add it to the other forces, $F_{\text{new}} = F_{\text{old}} + J^\intercal\lambda$, and we integrate to find our new velocities and positions.

Gyroscopic forces in ODE

Conservation of angular momentum is a bit questionable in ODE.  If your rigid bodies have inertial tensors whose principal axes are all equal, everything is fine.  Otherwise, you will get into trouble.  A freely rotating body will tend to gain energy rather rapidly, eventually spinning fast enough to crash the simulator.  This was discussed somewhat recently on the ODE mailing list. I have generally worked around this problem by either adding global angular "friction" to the system or applying a maximum threshold to angular velocity.  This is fine but a little ugly.  You can also disable the gyroscopic term for each body which will result in conservation of angular velocity instead of angular momentum: a spinning figure skater pulling her arms in won't experience any acceleration.

The newsgroup discussion referenced a paper by Claude Lacoursière that addresses this problem (building on a paper by Anitescu and Potra).  The essence of the paper seems to be that you can achieve stability by manipulating the inertia/mass matrix to account for the gyroscopic torques.

It seems worthwhile to start with the computations affecting angular velocity in ODE's time step.  I'll use the notation in Lacoursière's paper.  Before resolving constraints for each time step of $h$ seconds, ODE first computes the inverse inertia matrix for time $n$ in the global frame: $\mathcal{I}^{-1}_{n} = R_n\mathcal{I}^{-1}_{0}R^{\intercal}_n$.  If gyroscope mode is enabled, it then computes the extra gyroscopic torques for the time step: $\tilde{\tau}_n = [{\mathcal{I}_n}\omega_n]\times\omega_n + \tau_n$.  Gravitational forces are also computed at this time, but they're not relevant for this discussion.

After computing the constraint Jacobian, ODE prepares a system for the LCP solver.  The solution is effectively found in "acceleration space".  Essentially, we start with $F=ma$ and then find the $F$ that satisfies $m^{-1}F=a$ where $a$ is the acceleration necessary to satisfy our constraints.  For rotation, the conserved value will be computed as $\mathcal{I}^{-1}_{n}\tilde{\tau}_n+\frac{\omega_n}{h}.$  Once the solver finds the constraint forces, the new angular velocity will be computed:

$\omega_{n+1} =\omega_{n}+h\mathcal{I}^{-1}_{n}\tilde{\tau}^*_n$ (where $\tilde{\tau}^*_n$ includes the constraint forces).
Ignoring the constraint forces, this expands to the explicit time step:
$\omega_{n+1} = \omega_n+h\mathcal{I}^{-1}_{n}[\hat{L}_n\omega_n+\tau_n]$ where $\hat{L}_n$ is the angular momentum, $\mathcal{I}_n\omega_n$, in "cross-product-matrix" form.  As mentioned before, this explicit form gains energy much like an explicit spring.  That is bad.

The paper goes on to compute the implicit form in body-local coordinates and finds that it's equivalent to solving the explicit form with a different inertia matrix: $\tilde{\mathcal{I'}}_n=\mathcal{I}_0-h\hat{L'}_n$.  According to the paper, this "is rather bizarre since then, the modified inertia tensor is non-symmetric" (pg. 8).  The paper shrugs at this form, stating that it would cause problems for most multibody frameworks and is therefore not very interesting, but they still give a function for the stepper:
$\omega_{n+1}'=[I+h\alpha C_n +h^2\alpha C^2_n]\omega_n' + h[I+h\alpha C_n + h^2\alpha C^2_n]\mathcal{I}^{-1}_0\tau_n'$
where
$\alpha = 1/(1+h^2\kappa)$
$\kappa = \det(\mathcal{I}^{-1}_0)\hat{L'}^{\intercal}_n\mathcal{I}_0\hat{L'}_n$
and
$C_n = \mathcal{I}^{-1}_0\hat{L'}_n$

I derived things a little differently.  ODE solves things in global space, rather than local space.  If we start with eq. 5.2 from the paper: $\tilde{\mathcal{I'}}_n\omega'_{n+1}=\mathcal{I}_0\omega'_n+h\tau'$ and move things into global space, we get the following:
$\omega_{n+1}=\tilde{\mathcal{I}}^{-1}_n\mathcal{I}_n\omega_n+h\tilde{\mathcal{I}}^{-1}_n\tau_n$
If we set the right-hand side to be equal to the explicit computation and then solve for $\tilde{\tau}_n$, we get $\tilde{\tau}_n=\frac{1}{h}\mathcal{I}_n[\tilde{\mathcal{I}}^{-1}_n\mathcal{I}_n\omega_n+h\tilde{\mathcal{I}}^{-1}_n\tau_n-\omega_n]$.
This is a lot uglier than $\tilde{\tau}_n = [{\mathcal{I}_n}\omega_n]\times\omega_n + \tau_n$, but as long as $\tilde{\mathcal{I}}_n$ is invertible, ODE shouldn't have much trouble with it.  The LCP solver never needs to see the funky mass matrix.  If we group terms a little better, we can see that there's not all that much extra to compute:
$\tilde{\tau}_n=\mathcal{I}_n\tilde{\mathcal{I}}^{-1}_n (\frac{1}{h}L_n)+\mathcal{I}_n\tilde{\mathcal{I}}^{-1}_n\tau_n-(\frac{1}{h}L_n)$
We already need to compute $\mathcal{I}_n=R_n\mathcal{I}_{0}R^{\intercal}_n$.
$L_n = \mathcal{I}_n\omega_n$ is cheap.
$\tilde{\mathcal{I}}_n=\mathcal{I}_n-h\hat{L}_n$ is cheaper.
I believe that inverting a 3x3 matrix can be done without too much cost and my feeling is that this matrix should always be invertible, although the matter merits additional attention.

I tested a straightforward implementation in ODE and found that it takes 3 to 7 times longer than the simple matrix-multiply and cross-product of the explicit method, but it's stable instead of exploding and that computation is still pretty small compared to the constraint solver or collision detection.

Local angular velocity of a single spinning body in ODE.  Inertia tensor is diag(10,4,2).  Initial angular velocity is (1,2,3).  Time step is h=1/60.  With gyroscope disabled, the body experiences conservation of angular velocity.  With explicit gyroscopic torques, it explodes.  With implicit computations, it converges.  It ought to maintain a closed fixed orbit that looks like the explicit one.

If we plot the local angular velocity of a spinning body, we get something that looks a lot like Fig. 8.1. from Lacoursière's paper (but cooler with animation).

Interestingly, the paper's justification for their inertia tensor of choice is

...it has three different values but is not an extreme case of a long thin object.  This illustrates many of the difficulties that can be encountered in practice. (pg. 11)

 However, when I went to visualize the geometry of an object with such an inertia tensor, I found that I couldn't do it.  It's impossible to create a box or ellipsoid of uniform density with that tensor unless you allow it to have imaginary dimensions.  My intuition says that you can't create any object with that tensor.  Stack-exchange verifies that the principal axes of an inertia tensor need to satisfy the triangle equality. 

    If, instead, we create a box with the density of water using the same ratio for dimensions: $[1,.4,.2]\text{m}$, then we get an $80\text{kg}$ box with inertia tensor $I_0\approx\text{diag}(1.33,6.93,7.73)$.  The end effect with this mass moment is pretty similar: the implicit method converges to a stable point and the explicit explodes.

I tried averaging the two methods or alternating between methods on successive passes, but, at least for this set of conditions, the trajectory was almost the same as the implicit trajectory.  The paper discusses other approaches that are "symplectic and time-reversible" and are much better at approximating the correct behavior, but these seems to use completely different integration methods and might not be amenable to implementation with popular rigid-body physics engines.

One remaining issue that should be addressed is the fact that I used a matrix inverse in my computation.  As I have experimented with it, I've found that there's a danger of encountering numeric instabilities with regular masses.