Dormant Point refers to source points not contributing to the optimization. In particular, we analyze the pointwise contribution from the following aspects:
As a result, our new metric remains precise and consistent over environmental and parameter changes.
Consider the following Gaussian-Newton (GN) increment:
For the i-th residual, we evaluate the smoothness by the effective ratio (ER):
When e ≈ 0, the local Jacobian is smooth along the direction of the GN increment.
Points lost momentum to increase NDT scores and started to follow geometry constraints passively.
Terminates when ER converges to 0!
Points are non-contributive, if they lose momentum (e ≈ 0) before reaching the correct voxel (high score).
Hence, we define dormant points as points satisying:
We certify the alignment based on the ratio of dormant points.
The below figure shows that the dormant ratio can reflect correctness better than NDT scores.
Dormant ratios behave consistently in the bottom-left corner, where NDT scores are volatile and unreliable.
The tops and bottoms are identical clouds, where the color codes are:
If NDT succeeds, low-scored points will take over the ending iterations.
They will dominate the GN increment, while the high-scored ones begin to follow passive constraints.
As a result, the score and ER distribution will contrast each other.
In successful cases (score-ER complementation), low-scored points can claim a better score due to their high ERs.
On the other hand, in failed cases, the number of low-scored points remains high throughout the optimization without a decent ER to overcome the local minimum.
Hence, these points are likely to stay dormant the entire time if NDT fails.
NDT success case
NDT failed case
To meet the efficiency demand in real-time applications, we propose two innovations:
Our work significantly outperforms all state-of-the-art methods in execution time, while remaining a comparable level of accuracy.
Consider a planar equation:
For the i-th transformed point, define the i-th residual:
PFT contains merely a 3D inner product and a scalar deduction.
PFT's 1D residual makes Jacobian & Hessian accumulation fast and light-weighted.
The pointwise Jacobian is almost independent of T.
This allows our PFT to run efficiently with GN (LM not needed).
Consider the following sum-of-square objective function:
Direct neighbour searches robustify the optimizer by rewriting:
Increased Complexity: direct N makes the solver N-time slower!
Octomerge is designed to mitigate the N-time complexity of direct neighbour searches.
It uses finer voxels for point-dense regions and a coarser resolution for sparsed ones.
The coarse voxels help enlarge the voxel region, allowing us to stick with the one-voxel-per-point policy for acceleration.
The figure shows the mean error trend w.r.t. iterations, where:
According to experiments, octomerge yields better iteration convergence than direct methods do.
PFT significantly outperforms all other solvers in efficiency.
In TUM-RGBD (plane abundant), PFT even yields the best accuracy.
For further experiments, please refer to the paper (preprint).
The project uses a special form of Rao-Blackwellized Particle Filters (RBPF) to tight-couple GMapping and ORB-SLAM.
As a result, this framework is able to actively drift detection based on survival rate between two-source particles, and achieved cross-sensor verification for trajectory consistency.
To achieve front-end fusion, the system updates half (randomly chosen) of the particles with ORB-SLAM and the rest with wheel odometry.
When a laser scan arrives, particles will be resampled based on their 2d scan-matching scores (equivalent to GMapping).
If the survival rate of ORB-updated (green) particles is low, the system believes that ORB-SLAM is drifting.
In that case, the following Trajectory Rectification will be triggered.
.svg)
If triggered, the system will request ORB-SLAM to conduct global bundle adjustment with RBPF trajectory constraints.
.svg)