1
$\begingroup$

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?

|improve this question
$\endgroup$
  • $\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$ – ZachMcDargh Jan 4 '18 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 Jan 4 '18 at 18:40
5
$\begingroup$

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

SeedRandom[2018]
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]]]
|improve this answer
$\endgroup$
  • $\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$ – ZachMcDargh Jan 4 '18 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 Jan 4 '18 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$ – Ulrich Neumann Jan 4 '18 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 Jan 4 '18 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$ – J. M. will be back soon♦ Mar 20 '18 at 2:58
0
$\begingroup$

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 

S=(R-Transpose[R])/2
(*{{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}*)
|improve this answer
$\endgroup$
  • $\begingroup$ But my rotations are not small at all, in fact they span the entire unit circle. $\endgroup$ – ZachMcDargh Jan 5 '18 at 16:13
  • $\begingroup$ In my answer there is no assumption about small angles! $\endgroup$ – Ulrich Neumann Jan 5 '18 at 16:48

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

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