-
Notifications
You must be signed in to change notification settings - Fork 544
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Fix 4DoF PointToPlane error minimizer crash #492
Conversation
Can one of the admins verify this patch? |
ok to test |
@kubelvla can you have a look? |
Thanks for catching this bug, I'll check if it were better to change the Gamma matrix to 4x4 homogeneous version, or whether to use the sub-block. But I definitely need to re-check the sizes of the matrices, my bad. |
@pomerlef I've checked the fix, the proposed one is the best solution. So far, I've been lucky that Eigen somehow multiplies the wrong sized matrices correctly, but apparently not always. The proposed fix does not change the numerical values of the cross variable, and the mapper works the same (without the crashes, of course). I have verified it on our darpa data. You can merge the pull request. |
Ok to test |
Also LGTM. @kubelvla a proposal from my side would be to generalize the approach to optimize ICP in a sub-space with a more flexible method, e.g. http://www.seas.ucla.edu/~vandenbe/133A/lectures/cls.pdf, slide 2. With this, the DoF to optimize could be a dynamic parameter of the Error Minimizer. A constraint built in this way could be more natural / flexible to implement. What do you think? |
@YoshuaNava Hi, good idea. I'll give it a look, although I'm mostly concerned with changing diapers and driving a baby carriage these months, so the results my take some time :D |
@kubelvla No worries, all the best and thank you for looking into it. The 4DOF implementation is very useful 💪 |
I encountered a bug where PointToPlane error minimizer crashes when using
force4DOF
is enabled and the point clouds have very little overlap. Using Debug version of libpointmatcher the crash happens with all point clouds.I traced the issue down to this matrix multiplication
matrixGamma*mPts.reading.features
, wherematrixGamma
andmPts.reading.features
are 3x3 and 4xN matrices, respectively. Using inhomogeneous coordinates for the points fixed the issue.stacktrace: