This bugzilla service is closed. All entries have been migrated to
Bug 771 - Implement quaternion fitting from the todo list
Summary: Implement quaternion fitting from the todo list
Alias: None
Product: Eigen
Classification: Unclassified
Component: Geometry (show other bugs)
Version: 3.2
Hardware: All All
: Normal Feature Request
Assignee: Nobody
Depends on:
Blocks: 3.x
  Show dependency treegraph
Reported: 2014-03-23 15:23 UTC by Charles Karney
Modified: 2019-12-04 13:08 UTC (History)
6 users (show)

Implement qMethod (8.49 KB, text/plain)
2014-03-23 15:23 UTC, Charles Karney
no flags Details
Take 2 now called alignPoints.h (15.46 KB, text/x-chdr)
2014-03-27 20:26 UTC, Charles Karney
no flags Details
alignPoints.h with some documentation fixes (16.11 KB, text/x-chdr)
2014-03-28 20:06 UTC, Charles Karney
no flags Details
alignPoints.h in close to final form (24.98 KB, text/x-chdr)
2014-04-03 20:59 UTC, Charles Karney
no flags Details
Complete implementation as a patch to changeset 5889:f3e345d4ebae (33.29 KB, patch)
2014-04-22 13:30 UTC, Charles Karney
no flags Details | Diff

Description Charles Karney 2014-03-23 15:23:06 UTC
Created attachment 438 [details]
Implement qMethod

I've implemented quaternion fitting, the q-method, of corresponding sets
of 3d points in Eigen, see attached file qMethod.h.  Of course, the
umeyama method is more general (applying to points in arbitrary
dimensions).  Reasons for including the q-method are:

* possibly more efficient

* allows weights to be assigned to the points

* returns a Transform instead of Matrix

* by including 1/sqrt(c) in the residual, the inverse transformation is
  returned if the source and destination sets are interchanged.

* As of Eigen 3.2.1, there's a bug in umeyama() which causes it to
  return incorrect results with with_scaling = false unless the residual
  is zero (see separate bug report).

An expert in Eigen internals should look at the implementation I provide
here.  In particular,

* use static asserts to verify the dimensions of the input

* optimize the type of the 3xn matrices in the version of the function
  with weights.
Comment 1 Charles Karney 2014-03-23 16:04:45 UTC
A possible generalization that might be useful would be to allow the
vector of weights to be of a different scalar type that that of the
source and destination matrices.  (For example, a few discrete weights
might be stored in a vector of unsigned chars.)  This would be easy to
accommodate, of course.  (But possibly just invoking the routine with an
actual arguemnt of wgt.cast<double>() might amount to the same thing?)
Comment 2 Charles Karney 2014-03-24 20:24:20 UTC
I'm looking into providing the facility to allow coordinate reflections.
I think this can be done at zero cost by allowing a final
allow_reflection optional argument.  (This would allow the user to match
left and right handed gloves.)  I'll let you know in a few days.  Stay
tuned!  In the meantime, I'd appreciate feedback on the version I
posted.  (And I see I left a debugging statement in this; sorry.)
Comment 3 Hauke Heibel 2014-03-24 20:38:11 UTC
The method looks cool - in particular since it allows to exchange the point

Regarding the static asserts I think the only thing we can do is something like

EIGEN_STATIC_ASSERT(srcDerived::RowsAtCompileTime == Dynamic ||
srcDerived::RowsAtCompileTime == 3,
EIGEN_STATIC_ASSERT(dstDerived::RowsAtCompileTime == Dynamic ||
dstDerived::RowsAtCompileTime == 3,
EIGEN_STATIC_ASSERT(wgtDerived::RowsAtCompileTime == Dynamic ||
wgtDerived::RowsAtCompileTime == 1,

because dynamic row matrices should be allowed.

In order to enable hand crafted scalar types, you might consider using the
sqrt() function as follows:

using std::sqrt;
Scalar c = scaling ? sqrt( dst1.array().square().sum() /
src1.array().square().sum() ) : Scalar(1);

The advantage is that this allows argument dependent look-up of sqrt which
allows users to provide their hand written sqrt function for special scalar

Affine could in theory be replaced by AffineCompact and one could declare a few
more local variables as 'const' but other than that I don't see any issues.

I would refrain from generalizing the scalar type of the weights. Casting
within the function would kill the vectorization, thus the only solution
performant would be the creation of a temporary inside the function which is as
good as passing wgt.cast<double>() as you suggested.

I've just seen that the method without the scaling is demeaning the data twice.
A temporary is probably more efficient.

That's all I have seen so far...
Comment 4 Gael Guennebaud 2014-03-24 22:52:25 UTC
Nice, I'm looking forward a short benchmark against umeyama method ;)

Maybe it's just a matter of taste, but srcmean is just a matrix vector product: src*wgt and src1 a product with a diagonal matrix:

(src.colwise() - srcmean) * wgt.asDiagonal();

Perhaps that code duplication could be avoided by calling the general version with VectorXd::Constant(1,n). It might likely be the case that products with 1 will be removed by the compiler. Have to check the generated assembly though.
Comment 5 Charles Karney 2014-03-25 01:27:28 UTC
Gag, I see I put in the weights wrong (they are getting including twice
in forming the correlation matrix).  I'll fix (w = 1 is fine, however).
In addition to allowing reflections, it's easy to return the value of
the residual.  Thus, the calling sequence might be...

  qMethod(src, dst,
  wgt = Matrix<Scalar,1,1>::Ones(), // If size is 1 assume all weights equal
  bool allow_translation = true,
  bool allow_scale = true,
  bool allow_reflection = false,
  Scalar* residual = NULL)
Comment 6 Charles Karney 2014-03-27 20:26:05 UTC
Created attachment 444 [details]
Take 2 now called alignPoints.h

Here is my take 2.  I've renamed the function alignPoints and extended
it to treat any number of dimensions m >= 2 (which must be a
compile-time constant).  The typical calling sequence is

  Transform<double,m,AffineCompact> T = 
  alignPoints<m>(x, y, w,
                 allow_translation, allow_scaling,
                 allow_reflection, &E);

For m == 3 this implements the quaternion method, otherwise it uses the

I still need to write a test suite for this function.  Feedback is
welcome in the meantime.
Comment 7 Charles Karney 2014-03-28 20:06:04 UTC
Created attachment 447 [details]
alignPoints.h with some documentation fixes

I've fixed up the documentation.  Two glaring errors have
been fixed: the factor in front of E should be 1/c and not
1/sqrt(c); R is a rotation (possibly improper) and not
a reflection.
Comment 8 Charles Karney 2014-04-03 20:59:58 UTC
Created attachment 449 [details]
alignPoints.h in close to final form

Attached is alignPoints.h in close to final form.  There is a measurable
penalty in using a weight which is RowVectorXd::Ones(n), so I decided to
make the weight argument optional.  Thus the signatures are



Thus, the user gets to make 5 binary choices when calling alignPoints

    supply weight
    return error

This flexibility will be of use to users, who, depending on the
application, will need to make any of the 2^5 choices.  In typical
applications, these choices will be known at compile time and this
allows the unused code to be eliminated.

The other thing the user *must* specify at compile time is the number of
dimensions.  I imagine that this covers most uses.  The restriction
arises from the use of a Transform as the return type.  Evidently
Dynamic-dimensional (or 0d or 1d) Transforms are not allowed.  If this
is relaxed, alignPoints can quickly accommodate.

The resulting function is basically a more flexible version of umeyama
(with perhaps a saner way of dealing defining the error when the scale
is allowed to vary).  With allow_scaling = true, the speed is more or
less the same; but the quaternion method is somewhat faster for m = 3
(especially if the number of points is less that 100 or so).  For
allow_scaling = false, alignPoints is appreciably faster (and
allow_translation = false is faster still).

I expect to supply the testing code within 2 weeks.
Comment 9 Charles Karney 2014-04-22 13:29:15 UTC
Here is the complete implementation of alignPoints.  This includes
implementation, documentation, sample code, and tests.  These are
provided as a patch to changeset 5889:f3e345d4ebae.  The documentation
compiles cleanly under Linux and the tests compile and run without
warnings on Linux and Windows.
Comment 10 Charles Karney 2014-04-22 13:30:29 UTC
Created attachment 455 [details]
Complete implementation as a patch to changeset 5889:f3e345d4ebae

Here is the complete implementation of alignPoints.  This includes
implementation, documentation, sample code, and tests.  These are
provided as a patch to changeset 5889:f3e345d4ebae.  The documentation
compiles cleanly under Linux and the tests compile and run without
warnings on Linux and Windows.
Comment 11 Charles Karney 2014-06-12 00:15:43 UTC
Any feedback on this feature?

It seems that it provides some useful additional capabilities to eigen
(e.g., fits with/without reflection), deals with scaling in a slightly
saner way that umeyama, and fulfils one feature request (for a weighted
Comment 12 Christoph Hertzberg 2014-06-13 17:56:03 UTC
Sorry for having this left unattended for so long.

Some comments:
We try to avoid introducing too many preprocessor switches which change behavior -- wouldn't it be possible to make this either a parameter or simply a different function?
* I don't really like the passing of an optional parameter via pointer (*E) in the public interface
* You specify that the weights must be non-negative, but only partially check that. Either say that the result is undefined for negative weights and avoid any checks (at least in NDEBUG mode) or make a full check right at the beginning (e.g., w.minCoeff()>=T(0))

A general thing to consider:
It may be worthwhile to implement this feature using a class where you can set options and which has a compute() method and methods to get the result (similar to how the solvers are implemented).
Your implementation using a free function is closer to the umeyama interface, but I think we barely have free functions for non-trivial things anywhere else.
Comment 13 Charles Karney 2014-06-14 22:37:46 UTC
OK, thanks for the feedback.  I'll look into implementing as a class
and addressing your other concerns.  Turn around time will be approximately
3 months.
Comment 14 Charles Karney 2016-02-16 16:51:48 UTC
Sorry, I've dropped the ball here.  Is there still interest in this?  I
can pick this up again if there is.  (I assume I should do fresh pull
from the SCM repository and create patches against this.)

Most likely I would stick with the free function method for now -- this
would be regarded as a drop in replacement for the umeyama method,
Comment 15 Mykola Dimura 2018-03-11 12:28:57 UTC
(In reply to Charles Karney from comment #14)

Yes, I would like to second @Semen Bug 247 , weighting would still be very much appreciated!
Comment 16 Charles Karney 2018-03-11 22:17:27 UTC
It doesn't look like I'll be able to pick up on this for the foreseeable future.
If someone else wants to pick up where I left off, I'll he happy to help.
Comment 17 Nobody 2019-12-04 13:08:04 UTC
-- GitLab Migration Automatic Message --

This bug has been migrated to's GitLab instance and has been closed from further activity.

You can subscribe and participate further through the new bug through this link to our GitLab instance:

Note You need to log in before you can comment on or make changes to this bug.