Comments (7)
(I also reported this issue in the repo of libpointmatcher, the library that I'm the most familiar with and that kickstarted my investigation. Issue link -> norlab-ulaval/libpointmatcher#507)
from cilantro.
Thanks for your interest in cilantro. These are interesting research questions and I'll answer briefly from my own limited experience, but would be happy to discuss this into further detail.
- Numerically, it makes a lot of sense to center the clouds. Perhaps even scaling could be meaningful to improve numerical stability of the ICP algorithm. From experience I can confirm that this makes a difference. You could evaluate the condition number of the linear system of equations (Ax = b) at each ICP step. Based on your question in libpointmatcher, it seems you notice a difference in the basin of attraction depending on whether you center the clouds. AFAICS, this can only be explained by numerical conditioning as theoretically the ICP steps should be identical.
- There are many, many different ICP approaches of which symmetric Point-To-Plane ICP is only one. I've implemented this alongside the already existing ICP approaches in cilantro for two reasons. 1) It uses normals from both clouds. Other point-to-plane approaches typically only use one of the two normals. This intuitively introduces asymmetry to the solutions and ignores data that is anyways there. 2) It has proven better convergence properties than regular ICP. Check the publication for more details.
I have positive experiences with other libraries and each of the ICP methods has its own merits and drawbacks. I can only recommend experimenting and see what suits your application best. - I've been using cilantro and symmetric ICP regularly with success (and stability) but not (yet) in SLAM.
Additionally, note that ICP is a local optimization, assuming an already reasonable initial guess. For a large part, the basin of attraction is also determined by the clouds and not the algorithm, e.g. a geometry with a dominant plane may have a worse basin of attraction for point-to-plane ICP as the objective will be close to zero once the dominant face is aligned regardless of the rotation around the normal of the dominant face.
from cilantro.
Dear @wannesvanloock,
Thank you for your reply and suggestions 🙏
Topic 1 - ICP Conditioning
Numerically, it makes a lot of sense to center the clouds.
I resonate with your observation about conditioning. My hope is that improving this leads to more uniform precision, independent of the location of reference and reading point clouds.
Furthermore, during my experiments I observed cases where the bad conditioning fooled the optimization to slide 🏂 into a wrong local minimum. I wonder if bad conditioning could be blinding ICP to misalignments, and if by improving the conditioning of ICP, we could enable it to better detect mis-registrations (there is a ton of work like CorAl which is trying to address that outside ICP).
Perhaps even scaling could be meaningful to improve numerical stability of the ICP algorithm.
I found that Trimesh2 from Rusinkiewicz (the author of Symmetric Point-To-Plane) takes scaling into account. I linked part of the formulation in the libpointmatcher issue and the implementation from this fork of Trimesh2.
For a large part, the basin of attraction is also determined by the clouds and not the algorithm, e.g. a geometry with a dominant plane may have a worse basin of attraction for point-to-plane ICP as the objective will be close to zero once the dominant face is aligned regardless of the rotation around the normal of the dominant face.
What you mention is spot-on 🤘. In robot localization we encounter this problem in areas like long corridors, wide-open spaces, where the P2P cost function is blind due to lack of features in certain degrees of freedom. Rusinkiewicz studied this problem and identified some geometric primitives that lead to ill-formulated ICP:
(Image taken from this paper)
In a collaboration between ANYbotics (my employer) & ETH-Zurich's Robotics Systems Lab we developed a way to measure the "degree of Localizability' based on the rotational and translational components of the Point-To-Plane error, and proposed constrained optimization as a way to limit registration to the observable sub-space. We released this paper describing the approach.
Topic 2 - Alternative methods
I follow the introduction of Symmetric Point-To-Plane ICP with interest. A reason I haven't yet tried to integrate it is because for live robot localization we tend to get reading and reference point clouds with different sampling properties, which influence the quality of surface normals significantly.
While a reference point cloud map can provide enough geometric support to extrude good normals, the point clouds coming from spinning lidar sensors may offer little support to extrude normals in certain areas. See for example the concentric rings falling on the floor, where extruding a normal would require using a large radius for finding neighbors, or some prior knowledge (e.g. a local map):
Moving from regular to symmetric P2P for robot localization may require to A) improve the quality of surface normals in reading point clouds, B) be more selective when matching, or C) both. A paper that I often come back to, which predates Symmetric P2P, but contains many insights on how to sample surface normals for SLAM is NOctoSLAM.
Topic 3 - Applications of Symmetric Point-To-Plane ICP
I see. I haven't yet used Cilantro for robot localization, but I will try it. The depth and breadth of the algorithms in Cilantro is awesome. I got to know the library thanks to a colleague, @fevb.
Actions moving forward
The actions mentioned in norlab-ulaval/libpointmatcher#507 (comment).
Additionally, I could keep you updated about the outcome of my investigation in this issue or the libpointmatcher one.
(Sorry for the long reply)
from cilantro.
Hi @wannesvanloock,
I read through the Cilantro code and noticed that when you compute the means of the point clouds to register, you don't take into account the weights from the point-to-plane correlator.
The impact I could see in that is that even if your correlator rejects a point pair, it would still be used in the estimation.
Is this something you had considered in the past?
Thank you!
from cilantro.
Cannot say I have considered this :) It does make sense though!
from cilantro.
Hi @wannesvanloock,
I've been doing some experiments, and got Point-To-Plane ICP (libpointmatcher's) to reach 1e-6 to 1e-8 precision with single-precision floating point. I was also to boost Point-To-Point accuracy a bit, although it doesn't reach the same level. In my path I found that Eigen has a super nice implementation of Umeyama with solid conditioning.
I implemented multiple modules (first one submitted) to be able to unit test the code (synthetic data generation, computing the precision over multiple test cases). I could support doing the same for cilantro.
On the other hand, I wanted to ask you about this line:
Is there any specific reason why you multiple by the cosine of the angle of rotation?
Thank you!
Yoshua
from cilantro.
Is there any specific reason why you multiple by the cosine of the angle of rotation?
See equation (11) in Szymon Rusinkiewicz. "A Symmetric Objective Function for ICP." ACM Transactions on Graphics (Proc. SIGGRAPH) 38(4), Article 85, July 2019.
In my path I found that Eigen has a super nice implementation of Umeyama with solid conditioning.
I'm aware of the Umeyama implementation in Eigen. I've explored similar paths in which we don't compute AtA
but rather formulate the problem as Ax = b
and solve it with Eigen in LSQ sense. IIRC, I found that the computational performance was dramatically worsened. However, I do think it will benefit the overall quality of the solution.
Perhaps you can conclude ICP with a few of such steps, or perform these when the condition number degrades?
from cilantro.
Related Issues (20)
- how to use this library in my project? HOT 5
- A error in non_rigid_icp.cpp for Example 2: Compute a densely supported warp field HOT 1
- Can be used to create a surface from a point cloud? HOT 1
- Radius search using kdtree always returns sorted result. HOT 4
- complie smooth in ubuntu18.04,but not in win10 with VS2017 HOT 2
- transform rgbd image to point cloud
- have c# library? HOT 2
- TypeTraits.h HOT 1
- The old versions of ffmpeg libraries are not available on for Ubuntu20
- non-rigid registration speed
- Compilation issues in Win + VS HOT 3
- Questions about using this library HOT 6
- Releases
- ExternalProject and Linking, is Cilantro header-only library? HOT 8
- Add comparison to existing C++ libraries
- Interfacing to ROS
- Point Cloud Streams... OpenNI or something else? HOT 5
- Issues when trying to use cilantro in a different project HOT 3
- Minimal way to include cilantro in other project? HOT 4
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from cilantro.