I have a collection of rigid bodies, each comprising ~100 particles. Each body is defined by a list of points corresponding to the coordinates of its constituent particles. The rigid bodies are approximately identical (i.e. there should exist a rigid body motion that maps each body onto any other body), though some statistical noise in the algorithm that determined the particle positions makes them not exactly identical. I want to determine the center of mass and orientation of each rigid body.

The simplest way to do this seems to be using the function FindGeometricTransform. Using this, I have determined a translation vector and a transformation matrix $R$ to transfer one body to another. I would like to use this matrix to determine the Euler/Tait-Bryan/roll-pitch-yaw angles between each body. However, the matrix obtained by FindGeometricTransform is not exactly a rotation matrix because of the aforementioned statistical noise. Hence, RollPitchYawAngles returns an error. How can I find an approximate set of angles to transform between the different bodies?

  • $\begingroup$ One alternative method I could use is to find the center of mass and principle axes of each body directly, which I could of course use to determine an exact rotation matrix. But I'd prefer something a bit more elegant, and something that scales a bit more nicely in the event that I have to do this for many more bodies in the future. $\endgroup$ Commented Jan 4, 2018 at 17:17
  • 1
    $\begingroup$ Maybe you could try QR decomposition and use Q as the rotation matrix. But I'm not sure if it's relevant in your case (i.e. I don't know such a Q is robust to noise). $\endgroup$
    – anderstood
    Commented Jan 4, 2018 at 18:40

2 Answers 2


Orthogonalize seems to do the job when the noise is not too large. Of course if the data is very noisy, then FindGeometricTransform will not return the result you are looking for.

Below, pts0 is the original set of points, pts1 is the transformed nosiy data, rotated by an angle $0.5$ (the unknown angle we are looking for).

pts0 = RandomInteger[{-5, 5}, {10, 2}];
noiseFactor = 0.1;
pts1 = RotationMatrix[0.5].#& /@ (noiseFactor*RandomReal[{-1, 1}, {10, 2}] + pts0);
t = FindGeometricTransform[pts0, pts1];
r = t[[2, 1, ;; 2, ;; 2]];
rot = Orthogonalize@r;
ArcCos[rot[[1, 1]]] (* mind the sign *)
(* 0.497794 *)

The result is very close to $0.5$.

enter image description here

With more noise (noiseFactor = 0.5) you get 0.489369 which is not too bad.

enter image description here

Edit The above approach seems to give the same result as specifying TransformationClass -> "Rigid" in FindGeometricTransform. Thus it is even simpler to use

t = FindGeometricTransform[pts0, pts1, TransformationClass -> "Rigid"];
ArcCos[t[[2, 1, 1, 1]]]
  • $\begingroup$ Thanks much, this works perfectly. My alignment error for FindGeometricTransform was around 10^-4, so I'm well within the tolerance of this method. $\endgroup$ Commented Jan 4, 2018 at 20:54
  • $\begingroup$ @ZachMcDargh Good. Just be careful with the sign in ArcCos, I did not take into account rot[[1, 2]] for simplicity but you might want to improve that to ensure you have the right sign in the angle. $\endgroup$
    – anderstood
    Commented Jan 4, 2018 at 21:03
  • $\begingroup$ Be careful with Orthogonalize[]! The best guess for the unknown rotation vector(small rotation) is dr=(r-Transpose[r])/2 ,The angle is dr[[2,1]] $\endgroup$ Commented Jan 4, 2018 at 22:17
  • $\begingroup$ @UlrichNeumann Where does the expression for dr comes from? I found a cleaner approach, that seems to be the same as with Orthogonalize, see edit. $\endgroup$
    – anderstood
    Commented Jan 4, 2018 at 23:21
  • 1
    $\begingroup$ Instead of ArcCos[], I would probably use two-argument arctangent on the first row of the rotation matrix. Thus: {err, tf} = FindGeometricTransform[pts0, pts1, TransformationClass -> "Rigid"]; -ArcTan @@ TransformationMatrix[tf][[1, {1, 2}]] $\endgroup$ Commented Mar 20, 2018 at 2:58

The 2D-rotationmatrix can be expressed (one possibility)

\[Phi] = {{0, -\[CurlyPhi]}, {\[CurlyPhi], 0}} 
R=IdentityMatrix[2] + (Sin[\[CurlyPhi]] \[Phi])/\[CurlyPhi] + ((1 -Cos[\[CurlyPhi]]) \[Phi].\[Phi])/\[CurlyPhi]^2
(* euler angle \[CurlyPhi]*)

The difference

(*{{0, -Sin[\[CurlyPhi]]}, {Sin[\[CurlyPhi]], 0}} *)

gives a skew matrix S, containing the euler angle of the given rotation, which can be evaluated by solving the equation

 Sin[\[CurlyPhi]]== S[[2,1]] (* S=(r-r^t)/2*)
 (*{\[CurlyPhi] -> -0.510147}*)
  • $\begingroup$ But my rotations are not small at all, in fact they span the entire unit circle. $\endgroup$ Commented Jan 5, 2018 at 16:13
  • $\begingroup$ In my answer there is no assumption about small angles! $\endgroup$ Commented Jan 5, 2018 at 16:48

Your Answer

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

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