Re: a rigid transformation problem

In article <44195a1c@xxxxxxxxxxxxx>, Thomas Mautsch <mautsch@xxxxxxx> wrote:
In news:<dva3jn$pks$1@xxxxxxxxxxxxxxxxxxxxxx>
schrieb Robert Israel <israel@xxxxxxxxxxx>:
<jayzhuo@xxxxxxxxx> wrote:
given two point sets {pi} {qi}, determing the rotation matrix R and
translation matrix T, so that the error E is minimized:

E = square sum of d ( R*pi + T, qi)

Do you mean sum of the squares?
Let us suppose he does.

the distance is defined as follow:

d ( p, q ) = || p - q|| if || p - q|| < M
M otherwise

where M is a constance.

That could be rather nasty, as it makes the objective function
non-differentiable.

But not badly non-differentiable - I personally would always prefer
a piecewise linear function to a fully nonlinear function like tanh
in this context.

*In theory* you could solve this optimization problem as follows:

Say the number of given pairs of points (pi,qi) is n.
For each of the 2^n subsets S of the index set {1,2,...,n},

Not very practical, of course, unless n is rather small...

....

Would a smooth cut-off such as
d(p, q) = M tanh(||p - q||/M) work for your purpose?

It might be better to take two different values for the
two values M in this formula,
maybe even different from the value of M
in the OP's definition of distance, or might it not?

how to solve this problem?

Using a numeric solver.
You have your choice of parametrizing the rotation matrix, e.g.
in terms of Euler angles, or using constraints R' R = I and
det(R) > 0.

I wonder how good numeric solvers can cope
with the fact that the gradient of tanh is so small
at larger values. - Might there not be the possibility
that the solver gets stuck in local maxima where
(most of) the points (R pi + T) and qi are far apart
from each other.

Maybe you could provide an example. -
That would be marvellous!

OK, here's an example in Maple 10.

with(LinearAlgebra): with(Optimization):

# Here is a parametrization of a 3D rotation matrix
# in terms of Euler angles a,b,c
Rabc:= Matrix([[ cos(c)*cos(b)*cos(a)-sin(c)*sin(a),
-sin(c)*cos(b)*cos(a)-cos(c)*sin(a), sin(b)*cos(a) ],
[ cos(c)*cos(b)*sin(a)+sin(c)*cos(a),
-sin(c)*cos(b)*sin(a)+cos(c)*cos(a), sin(b)*sin(a) ],
[ -cos(c)*sin(b), sin(c)*sin(b), cos(b) ]]);
V:= <v1,v2,v3>;

# Make up some data. Ten random vectors for the p_i
P:= [seq(RandomVector(3,generator=rand(-5..5)),i=1..10)];

[1] [-4] [ 0] [-5] [ 4] [-4] [ 5] [ 4] [2] [ 5]
[ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ]
P := [[4], [ 5], [-1], [ 2], [ 5], [-2], [-3], [-4], [4], [-5]]
[ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ]
[0] [-2] [ 5] [-1] [-4] [ 2] [ 3] [ 5] [3] [ 0]

# The true transformation has Euler angles 1, 2, 3 and translation
# <1,2,3>

Rt:= eval(Rabc,{a=1.0, b=2.0, c=3.0});
Tt:= <1.0,2.0,3.0>;
Q:= map(v -> Rt.v+Tt, P);

# An extra translation of <1,1,1> is added to the first three vectors in Q
# so the "cutoff" will have something to do.

for i from 1 to 3 do Q[i]:= Q[i] + <1,1,1> od:

# Now get the residuals d(p,q) = tanh ||p - q||

E:= [seq(Rabc.P[i]+V-Q[i], i=1..10)];
ds:= map(v -> tanh(sqrt(v^%T . v)), E);

# Minimize the sum of squares of residuals using the least-squares
# solver

LSSolve(ds);

[1.3090743731944, [a = 1.00382746183031957, c = 2.99466288305908712,

b = 1.99673288295575734, v1 = 1.04762504047361582,

v2 = 2.03025329876483252, v3 = 3.02999015731672605]]

It was very quick, and the result is very close to the "true"
transformation.

Robert Israel israel@xxxxxxxxxxx
Department of Mathematics http://www.math.ubc.ca/~israel
University of British Columbia Vancouver, BC, Canada
.

Relevant Pages

  • Re: a rigid transformation problem
    ... translation matrix T, so that the error E is minimized: ... Do you mean sum of the squares? ... Using a numeric solver. ...
    (sci.math)
  • Re: a rigid transformation problem
    ... Do you mean sum of the squares? ... Using a numeric solver. ... I still have doubts about the usefullness of the Optimization package. ...
    (sci.math)
  • Re: "...the sum of squares removed by fitting..."
    ... the author uses the phrase "the sum of squares removed by fitting", ... Standard regression theory shows that the sum of squares removed ... Another phrase for the "error sum of squares" is the ...
    (sci.stat.math)
  • Re: Another Error in Afonsos Confidence Interval Code
    ... soma2 is the raw sum of squares of those numbers ... of squares to the usual corrected sum of squares. ... I have tried repeatedly to get Afonso to address, ... claiming about simulating confidence intervals. ...
    (sci.stat.math)
  • Re: Another Error in Afonsos Confidence Interval Code
    ... soma2 is the raw sum of squares of those numbers ... of squares to the usual corrected sum of squares. ... I have tried repeatedly to get Afonso to address, ... IF bb> b THEN maior = bb ...
    (sci.stat.math)