Skip to content

Instantly share code, notes, and snippets.

@rygorous
Created February 6, 2012 06:12
Show Gist options
  • Save rygorous/1750166 to your computer and use it in GitHub Desktop.
Save rygorous/1750166 to your computer and use it in GitHub Desktop.
This program estimates a rotation under constraints that are naturally stated in angles.
The original version works directly on Euler angles: first an initial solution is
determined using an iterative algorithm, then positions of other local optima are
determined (this is the part that works in angles), then the candidate solutions are
evaluated and passed through the iterative algorithm again to polish the results. Here's
the result of solving a few instances of the problem. Note that the initial solution is
always found as one of the valid solutions (as it should be), but the constrained
minimization part sometimes produces significant amount of error that requires a lot of
extra iterations to correct:
initial it=57 err=0.000095
refine solution 0 it=17 err=0.003206
refine solution 1 it=26 err=0.000095
initial it=413 err=0.000034
refine solution 0 it=1 err=0.000034
initial it=97 err=0.000083
refine solution 0 it=31 err=0.002072
refine solution 1 it=1 err=0.000083
initial it=275 err=0.000018
refine solution 0 it=500 err=0.000136
refine solution 1 it=117 err=0.000018
initial it=291 err=0.000055
refine solution 0 it=201 err=0.000327
refine solution 1 it=146 err=0.000055
initial it=92 err=0.000013
refine solution 0 it=15 err=0.002535
refine solution 1 it=1 err=0.000013
initial it=75 err=0.000010
refine solution 0 it=19 err=0.002742
refine solution 1 it=38 err=0.000010
initial it=99 err=0.000004
refine solution 0 it=26 err=0.002084
refine solution 1 it=52 err=0.000004
initial it=163 err=0.000048
refine solution 0 it=82 err=0.001408
refine solution 1 it=76 err=0.000048
initial it=110 err=0.000003
refine solution 0 it=7 err=0.002033
refine solution 1 it=1 err=0.000003
initial it=71 err=0.000082
refine solution 0 it=22 err=0.003111
refine solution 1 it=33 err=0.000082
initial it=46 err=0.000096
refine solution 0 it=13 err=0.003676
refine solution 1 it=21 err=0.000096
initial it=36 err=0.000145
refine solution 0 it=11 err=0.003889
refine solution 1 it=16 err=0.000145
The improved version doesn't parametrize the rotation in terms of angles, but instead
represents rotations in the corresponding planes as (unit) complex numbers. The (simple)
constraints are solved using projections. Everything else is the same; the main
difference is avoiding an (unnecessary) path that first takes an atan2(y, x) only to
determine the sine and cosine of the corresponding angle. Here's the results with the
new version. Note how the original solutions immediately stop without further iterations;
this is because the adjusted solution produces an error below the set error threshold of
1e-5. The original implementation perturbed solutions significantly.
initial it= 57 err=0.000095
refine solution 0 it= 11 err=0.003206
refine solution 1 it= 1 err=0.000095
initial it=413 err=0.000034
refine solution 0 it= 1 err=0.000034
initial it= 97 err=0.000083
refine solution 0 it= 31 err=0.002072
refine solution 1 it= 1 err=0.000083
initial it=275 err=0.000018
refine solution 0 it= 1 err=0.000018
initial it=291 err=0.000055
refine solution 0 it= 52 err=0.000327
refine solution 1 it= 1 err=0.000055
initial it= 92 err=0.000013
refine solution 0 it= 15 err=0.002535
refine solution 1 it= 1 err=0.000013
initial it= 75 err=0.000010
refine solution 0 it= 6 err=0.002742
refine solution 1 it= 1 err=0.000010
initial it= 99 err=0.000004
refine solution 0 it= 5 err=0.002084
refine solution 1 it= 1 err=0.000004
initial it=163 err=0.000048
refine solution 0 it= 52 err=0.001408
refine solution 1 it= 1 err=0.000048
initial it=110 err=0.000003
refine solution 0 it= 7 err=0.002033
refine solution 1 it= 1 err=0.000003
initial it= 71 err=0.000082
refine solution 0 it= 9 err=0.003111
refine solution 1 it= 1 err=0.000082
initial it= 46 err=0.000096
refine solution 0 it= 1 err=0.003676
refine solution 1 it= 1 err=0.000096
initial it= 36 err=0.000145
refine solution 0 it= 6 err=0.003889
refine solution 1 it= 1 err=0.000145
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment