Open YoshuaNava opened 1 year ago
Hi @YoshuaNava ,
Cheers!
Hi @pomerlef,
Thank you for your prompt reply 🙂
My motivation for digging into this topic is because I noticed (empirically) that libpointmatcher's Point-To-Plane ICP implementation shows a high degree of sensitivity to the position of the map origin relative to where we are localizing.
During my investigations I implemented a rotation-only version of Point-To-Plane ICP, and I observed how, the closer to the origin (and the lower the norm of the reference CoM), the more accurate the rotations were estimated, and the larger rotations it could handle. Conversely, I also observed how, the farther we are from the origin, the higher the likelihood that the estimated rotation affects the translation component in an unexpected way.
My hypothesis is that the CoM computation could be reducing the basin of convergence of P2P ICP the farther away from the origin the CoM is.
That being said, I would like to know what the other libraries are doing.
Sure thing :metal:
I made a short summary of different approaches, from libraries and papers.
At the start I just want to get some clarity on how coordinates are/should be handled in ICP: transforms (initial guess, delta from ICP iterations, total ICP correction, corrected initial guess), frames (Reading, Reference, Reading CoM, Reference CoM).
I'll try to derive a bit the equations to understand whether the operations are running in the right coordinate frames.
I'm planning to write some unit tests that
I'll add some flags to toggle ON/OFF:
I'll validate:
Note: I used the tool UpMath to format LaTeX for displaying in Markdown
@pomerlef I implemented unit tests for ICP conditioning, as well as a fix.
In order to test the conditioning of the registration pipeline I put together new code, and I would like to submit a PR. To do that, I would like to know your opinion on where we should store new functionality. I follow the practice of 'shifting discussions to the left' (discussing design before submitting PRs, the 'what'), so that reviews can focus on the implementation (the 'how')
Some of the utilities I implemented are targeted to:
boost::filesystem
)Eigen
and cmath
),Would you like me to add these utilities? If yes, should I do so under the utest
directory, or make them generally available under the pointmatcher
directory & namespace?
Thank you in advance.
Background
I'm debugging some behavior of Point-To-Plane ICP, which seems to estimate large rotations much better when
This seems to increase the basin of convergence for rotations, and I can get up to 30-40° pure rotation estimations in one ICP call.
I checked different point cloud libraries such as PCL, Open3D, KissICP, OpenCV, as well as Cilantro, and every implementation seems to handle this aspect differently. I also digged a bit through Libpointmatcher's commit history, and found two mentions (1, 2) but no more details.
Questions
Thank you in advance 🙏
Best, Yoshua Nava