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.
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?)
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.)
The method looks cool - in particular since it allows to exchange the point sets. Regarding the static asserts I think the only thing we can do is something like this EIGEN_STATIC_ASSERT(srcDerived::RowsAtCompileTime == Dynamic || srcDerived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); EIGEN_STATIC_ASSERT(dstDerived::RowsAtCompileTime == Dynamic || dstDerived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); EIGEN_STATIC_ASSERT(wgtDerived::RowsAtCompileTime == Dynamic || wgtDerived::RowsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE); 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 types. 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...
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.
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)
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 SVD. I still need to write a test suite for this function. Feedback is welcome in the meantime.
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.
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 alignPoints<m>(x,y, allow_translation,allow_scaling,allow_reflection,&E) alignPoints<m>(x,y,w, allow_translation,allow_scaling,allow_reflection,&E) Thus, the user gets to make 5 binary choices when calling alignPoints supply weight allow_translation allow_scaling allow_reflection 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.
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.
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.
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 fit).
Sorry for having this left unattended for so long. Some comments: * Try to avoid the EIGEN_ALIGNPOINTS_DISABLE_Q_METHOD_SPECIALIZATION macro. 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.
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.
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,
(In reply to Charles Karney from comment #14) Yes, I would like to second @Semen Bug 247 , weighting would still be very much appreciated!
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.
-- GitLab Migration Automatic Message -- This bug has been migrated to gitlab.com'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: https://gitlab.com/libeigen/eigen/issues/771.