Official implementations from the paper "Informed, Constrained, Aligned: A Field Analysis on Degeneracy-aware Point Cloud Registration in the Wild" arxiv link.
This repository contains the implementation of all the methods compared and analyized in this work, including the degeneracy detection method of X-ICP.
The work investigates different degeneracy mitigation methods for the problem of degenerate point cloud registration. To achieve this, this work relies on many open-sourced libraries. These libraries / packages are subject to their licenses.
- This work builds on the open-source version of Open3D SLAM.
- This work uses libpointmatcher point cloud registration library in the backend. The locally available (dont need to clone) libpointmatcher library is based on this version.
- The locally available (dont need to clone)
pointmatcher_rospackage is based on this version. - For NL-Reg. and NL-Solver methods, this work depends on Ceres. Version 2.1.0
- For the Ineq. Con. method, this work relies on an old version of QPmad quadratic problem library link.
- To replicate the result of the work of Petracek et al. (RMS), the authors forked the original work. Please also install the dependencies of this repository.
- Ubuntu 20.04 (expecting PCL 1.10)
- ROS Noetic
- CMake > 3.18 (tested 3.25)
cmake --versionIf NOT install the cmake from https://cmake.org/download download tar. Unpack tar and
cd cmake-<version>
./configure
make -j<num_of_processes>
sudo make install- Open3D, automatically installed by
open3d_catkinpackage (this take 6-7gb of space). - libnabo (dependency of libpointmatcher) link
- install dependencies for
open3d_slam(sudo apt install libgoogle-glog-dev libglfw3 libglfw3-dev liblua5.2-dev) - message_logger repository link
- QPmad fork. +
catkin build qpmad - Ceres (tested with 2.1.0). Ceres
- RMS fork and its dependencies.
To build this repository, you need to:
Clone the repository to:
cd catkin_ws/src
git clone [email protected]:leggedrobotics/open3d_slam_private.git.
cd ..
catkin init
catkin config -DCMAKE_BUILD_TYPE=Release
catkin build open3d_slam_rosStep by step building could have debugging installation process.
catkin build libpointmatchercatkin build pointmatcher_roscatkin build open3d_slamcatkin build open3d_slam_ros
Here, we describe the process for 1 dataset but this process apply to all datasets.
- Get the
.bagfile for the ANYmal forest experiment
roslaunch open3d_slam_ros replay_forest.launch rosbag_filepath:=/path/to/your_file.bag- An RVIZ window pop-up and replay the rosbag as fast as possible.
- The results of the test will be saved to the pre-defined folder in the launch file.
Running Ulmberg Tunnel experiment
roslaunch open3d_slam_ros replay_tunnel.launch rosbag_filepath:=/path/to/your_file.bagRunning HEAP Excavation experiment
roslaunch open3d_slam_ros replay_excavator.launch rosbag_filepath:=/path/to/your_file.bagRunning ANYmal simulation experiment
roslaunch open3d_slam_ros replay_sim.launch rosbag_filepath:=/path/to/your_file.bagAll the degeneracy point cloud registration method parameters are located in (open3d_slam_rsl/ros/open3d_slam_ros/param/icp.yaml). Unfortunately, the parameters are not nicely separated however, below you can find the parameter sets that exactly correspond to the methods described in the work.
P2Plane
readingDataPointsFilters:
# No specific filters.
referenceDataPointsFilters:
# No specific filters.
matcher:
KDTreeMatcher:
knn: 1
maxDist: 0.5
epsilon: 0.01
outlierFilters:
- TrimmedDistOutlierFilter:
ratio: 0.95
- SurfaceNormalOutlierFilter:
maxAngle: 1.57
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
regularization:
Disabled: # "Enabled" or "Disabled.
regularizationWeight: 440.0
# Enables the RMS filtering by Petracek et al.
enableRMSfiltering:
Disabled: # "Enabled" or "Disabled
rmsLambda: 0.0036 # Set to 0.0042 for Ouster based experiments.
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
ceresDegeneracyAnalysis:
CeresDegeneracyState:
isActive: 0 # "Enabled -> 1" or "Disabled -> 0. This is the NL-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" for correct detection.
usePointToPointCost: 0
usePointToPlaneCost: 1
useBoundConstraints: 1 # This is the transition from NL-Reg. to NL-Solver. 1: NL-Reg. 0: NL-Solver.
useSixDofRegularization: 0
useThreeDofRegularization: 1
regularizationWeight: 675.0 # The weight of NL-Reg.
degeneracyAwareness:
None:
transformationCheckers:
- DifferentialTransformationChecker:
minDiffRotErr: 0.001
minDiffTransErr: 0.01
smoothLength: 3
- CounterTransformationChecker:
maxIterationCount: 30
inspector:
NullInspector
logger:
NullLogger
errorMinimizer:
PointToPlaneErrorMinimizer
# Leave it enabled, this enforces the XICP detection.
forceXICPdetection:
Enabled: # "Enabled" or "Disabled
degeneracyDebug:
Enabled: # "Enabled" or "Disabled.
printingDegeneracy:
Enabled: # "Enabled" or "Disabled.
Eq. Con.
readingDataPointsFilters:
# No specific filters.
referenceDataPointsFilters:
# No specific filters.
matcher:
KDTreeMatcher:
knn: 1
maxDist: 0.5
epsilon: 0.01
outlierFilters:
- TrimmedDistOutlierFilter:
ratio: 0.95
- SurfaceNormalOutlierFilter:
maxAngle: 1.57
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
regularization:
Disabled: # "Enabled" or "Disabled.
regularizationWeight: 440.0
# Enables the RMS filtering by Petracek et al.
enableRMSfiltering:
Disabled: # "Enabled" or "Disabled
rmsLambda: 0.0036 # Set to 0.0042 for Ouster based experiments.
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
ceresDegeneracyAnalysis:
CeresDegeneracyState:
isActive: 0 # "Enabled -> 1" or "Disabled -> 0. This is the NL-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" for correct detection.
usePointToPointCost: 0
usePointToPlaneCost: 1
useBoundConstraints: 1 # This is the transition from NL-Reg. to NL-Solver. 1: NL-Reg. 0: NL-Solver.
useSixDofRegularization: 0
useThreeDofRegularization: 1
regularizationWeight: 675.0 # The weight of NL-Reg.
degeneracyAwareness:
OptimizedEqualityConstraints:
enoughInformationThreshold: 300
insufficientInformationThreshold: 150
point2NormalMinimalAlignmentAngleThreshold: 80
point2NormalStrongAlignmentAngleThreshold: 45
transformationCheckers:
- DifferentialTransformationChecker:
minDiffRotErr: 0.001
minDiffTransErr: 0.01
smoothLength: 3
- CounterTransformationChecker:
maxIterationCount: 30
inspector:
NullInspector
logger:
NullLogger
errorMinimizer:
PointToPlaneErrorMinimizer
# Leave it enabled, this enforces the XICP detection.
forceXICPdetection:
Enabled: # "Enabled" or "Disabled
degeneracyDebug:
Enabled: # "Enabled" or "Disabled.
printingDegeneracy:
Enabled: # "Enabled" or "Disabled.
Ineq. Con.
readingDataPointsFilters:
# No specific filters.
referenceDataPointsFilters:
# No specific filters.
matcher:
KDTreeMatcher:
knn: 1
maxDist: 0.5
epsilon: 0.01
outlierFilters:
- TrimmedDistOutlierFilter:
ratio: 0.95
- SurfaceNormalOutlierFilter:
maxAngle: 1.57
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
regularization:
Disabled: # "Enabled" or "Disabled.
regularizationWeight: 440.0
# Enables the RMS filtering by Petracek et al.
enableRMSfiltering:
Disabled: # "Enabled" or "Disabled
rmsLambda: 0.0036 # Set to 0.0042 for Ouster based experiments.
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
ceresDegeneracyAnalysis:
CeresDegeneracyState:
isActive: 0 # "Enabled -> 1" or "Disabled -> 0. This is the NL-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" for correct detection.
usePointToPointCost: 0
usePointToPlaneCost: 1
useBoundConstraints: 1 # This is the transition from NL-Reg. to NL-Solver. 1: NL-Reg. 0: NL-Solver.
useSixDofRegularization: 0
useThreeDofRegularization: 1
regularizationWeight: 675.0 # The weight of NL-Reg.
degeneracyAwareness:
InequalityConstraints:
highInformationThreshold: 300
enoughInformationThreshold: 300
insufficientInformationThreshold: 150
point2NormalMinimalAlignmentAngleThreshold: 80
point2NormalStrongAlignmentAngleThreshold: 45
inequalityboundmultiplier: 0.0014
transformationCheckers:
- DifferentialTransformationChecker:
minDiffRotErr: 0.001
minDiffTransErr: 0.01
smoothLength: 3
- CounterTransformationChecker:
maxIterationCount: 30
inspector:
NullInspector
logger:
NullLogger
errorMinimizer:
PointToPlaneErrorMinimizer
# Leave it enabled, this enforces the XICP detection.
forceXICPdetection:
Enabled: # "Enabled" or "Disabled
degeneracyDebug:
Enabled: # "Enabled" or "Disabled.
printingDegeneracy:
Enabled: # "Enabled" or "Disabled.
L-Reg.
readingDataPointsFilters:
# No specific filters.
referenceDataPointsFilters:
# No specific filters.
matcher:
KDTreeMatcher:
knn: 1
maxDist: 0.5
epsilon: 0.01
outlierFilters:
- TrimmedDistOutlierFilter:
ratio: 0.95
- SurfaceNormalOutlierFilter:
maxAngle: 1.57
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
regularization:
Enabled: # "Enabled" or "Disabled.
regularizationWeight: 440.0
# Enables the RMS filtering by Petracek et al.
enableRMSfiltering:
Disabled: # "Enabled" or "Disabled
rmsLambda: 0.0036 # Set to 0.0042 for Ouster based experiments.
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
ceresDegeneracyAnalysis:
CeresDegeneracyState:
isActive: 0 # "Enabled -> 1" or "Disabled -> 0. This is the NL-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" for correct detection.
usePointToPointCost: 0
usePointToPlaneCost: 1
useBoundConstraints: 1 # This is the transition from NL-Reg. to NL-Solver. 1: NL-Reg. 0: NL-Solver.
useSixDofRegularization: 0
useThreeDofRegularization: 1
regularizationWeight: 675.0 # The weight of NL-Reg.
degeneracyAwareness:
OptimizedEqualityConstraints:
enoughInformationThreshold: 300
insufficientInformationThreshold: 150
point2NormalMinimalAlignmentAngleThreshold: 80
point2NormalStrongAlignmentAngleThreshold: 45
transformationCheckers:
- DifferentialTransformationChecker:
minDiffRotErr: 0.001
minDiffTransErr: 0.01
smoothLength: 3
- CounterTransformationChecker:
maxIterationCount: 30
inspector:
NullInspector
logger:
NullLogger
errorMinimizer:
PointToPlaneErrorMinimizer
# Leave it enabled, this enforces the XICP detection.
forceXICPdetection:
Enabled: # "Enabled" or "Disabled
degeneracyDebug:
Enabled: # "Enabled" or "Disabled.
printingDegeneracy:
Enabled: # "Enabled" or "Disabled.
NL-Reg.
readingDataPointsFilters:
# No specific filters.
referenceDataPointsFilters:
# No specific filters.
matcher:
KDTreeMatcher:
knn: 1
maxDist: 0.5
epsilon: 0.01
outlierFilters:
- TrimmedDistOutlierFilter:
ratio: 0.95
- SurfaceNormalOutlierFilter:
maxAngle: 1.57
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
regularization:
Disabled: # "Enabled" or "Disabled.
regularizationWeight: 440.0
# Enables the RMS filtering by Petracek et al.
enableRMSfiltering:
Disabled: # "Enabled" or "Disabled
rmsLambda: 0.0036 # Set to 0.0042 for Ouster based experiments.
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
ceresDegeneracyAnalysis:
CeresDegeneracyState:
isActive: 1 # "Enabled -> 1" or "Disabled -> 0. This is the NL-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" for correct detection.
usePointToPointCost: 0
usePointToPlaneCost: 1
useBoundConstraints: 1 # This is the transition from NL-Reg. to NL-Solver. 1: NL-Reg. 0: NL-Solver.
useSixDofRegularization: 0
useThreeDofRegularization: 1
regularizationWeight: 675.0 # The weight of NL-Reg.
degeneracyAwareness:
OptimizedEqualityConstraints:
enoughInformationThreshold: 300
insufficientInformationThreshold: 150
point2NormalMinimalAlignmentAngleThreshold: 80
point2NormalStrongAlignmentAngleThreshold: 45
transformationCheckers:
- DifferentialTransformationChecker:
minDiffRotErr: 0.001
minDiffTransErr: 0.01
smoothLength: 3
- CounterTransformationChecker:
maxIterationCount: 30
inspector:
NullInspector
logger:
NullLogger
errorMinimizer:
PointToPlaneErrorMinimizer
# Leave it enabled, this enforces the XICP detection.
forceXICPdetection:
Enabled: # "Enabled" or "Disabled
degeneracyDebug:
Enabled: # "Enabled" or "Disabled.
printingDegeneracy:
Enabled: # "Enabled" or "Disabled.
Cauchy
readingDataPointsFilters:
# No specific filters.
referenceDataPointsFilters:
# No specific filters.
matcher:
KDTreeMatcher:
knn: 1
maxDist: 0.5
epsilon: 0.01
outlierFilters:
- RobustOutlierFilter: # Cauchy method.
robustFct: cauchy
scaleEstimator: mad
tuning: 1.0
- TrimmedDistOutlierFilter:
ratio: 0.95
- SurfaceNormalOutlierFilter:
maxAngle: 1.57
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
regularization:
Disabled: # "Enabled" or "Disabled.
regularizationWeight: 440.0
# Enables the RMS filtering by Petracek et al.
enableRMSfiltering:
Disabled: # "Enabled" or "Disabled
rmsLambda: 0.0036 # Set to 0.0042 for Ouster based experiments.
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
ceresDegeneracyAnalysis:
CeresDegeneracyState:
isActive: 0 # "Enabled -> 1" or "Disabled -> 0. This is the NL-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" for correct detection.
usePointToPointCost: 0
usePointToPlaneCost: 1
useBoundConstraints: 1 # This is the transition from NL-Reg. to NL-Solver. 1: NL-Reg. 0: NL-Solver.
useSixDofRegularization: 0
useThreeDofRegularization: 1
regularizationWeight: 675.0 # The weight of NL-Reg.
degeneracyAwareness:
None:
transformationCheckers:
- DifferentialTransformationChecker:
minDiffRotErr: 0.001
minDiffTransErr: 0.01
smoothLength: 3
- CounterTransformationChecker:
maxIterationCount: 30
inspector:
NullInspector
logger:
NullLogger
errorMinimizer:
PointToPlaneErrorMinimizer
# Leave it enabled, this enforces the XICP detection.
forceXICPdetection:
Enabled: # "Enabled" or "Disabled
degeneracyDebug:
Enabled: # "Enabled" or "Disabled.
printingDegeneracy:
Enabled: # "Enabled" or "Disabled.
TSVD
readingDataPointsFilters:
# No specific filters.
referenceDataPointsFilters:
# No specific filters.
matcher:
KDTreeMatcher:
knn: 1
maxDist: 0.5
epsilon: 0.01
outlierFilters:
- TrimmedDistOutlierFilter:
ratio: 0.95
- SurfaceNormalOutlierFilter:
maxAngle: 1.57
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
regularization:
Disabled: # "Enabled" or "Disabled.
regularizationWeight: 440.0
# Enables the RMS filtering by Petracek et al.
enableRMSfiltering:
Disabled: # "Enabled" or "Disabled
rmsLambda: 0.0036 # Set to 0.0042 for Ouster based experiments.
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
ceresDegeneracyAnalysis:
CeresDegeneracyState:
isActive: 0 # "Enabled -> 1" or "Disabled -> 0. This is the NL-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" for correct detection.
usePointToPointCost: 0
usePointToPlaneCost: 1
useBoundConstraints: 1 # This is the transition from NL-Reg. to NL-Solver. 1: NL-Reg. 0: NL-Solver.
useSixDofRegularization: 0
useThreeDofRegularization: 1
regularizationWeight: 675.0 # The weight of NL-Reg.
degeneracyAwareness:
SolutionRemapping:
threshold: 100
use2019: 0
useTruncatedSVD: 1 # Enables TSVD
skipRegistration: 0
#
# Hacky solution to use X-ICP degeneracy detection for Zhang et al., and TSVD.
enoughInformationThreshold: 300
insufficientInformationThreshold: 150
point2NormalMinimalAlignmentAngleThreshold: 80
point2NormalStrongAlignmentAngleThreshold: 45
transformationCheckers:
- DifferentialTransformationChecker:
minDiffRotErr: 0.001
minDiffTransErr: 0.01
smoothLength: 3
- CounterTransformationChecker:
maxIterationCount: 30
inspector:
NullInspector
logger:
NullLogger
errorMinimizer:
PointToPlaneErrorMinimizer
# Leave it enabled, this enforces the XICP detection.
forceXICPdetection:
Enabled: # "Enabled" or "Disabled
degeneracyDebug:
Enabled: # "Enabled" or "Disabled.
printingDegeneracy:
Enabled: # "Enabled" or "Disabled.
Zhang et al.
readingDataPointsFilters:
# No specific filters.
referenceDataPointsFilters:
# No specific filters.
matcher:
KDTreeMatcher:
knn: 1
maxDist: 0.5
epsilon: 0.01
outlierFilters:
- TrimmedDistOutlierFilter:
ratio: 0.95
- SurfaceNormalOutlierFilter:
maxAngle: 1.57
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
regularization:
Disabled: # "Enabled" or "Disabled.
regularizationWeight: 440.0
# Enables the RMS filtering by Petracek et al.
enableRMSfiltering:
Disabled: # "Enabled" or "Disabled
rmsLambda: 0.0036 # Set to 0.0042 for Ouster based experiments.
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
ceresDegeneracyAnalysis:
CeresDegeneracyState:
isActive: 0 # "Enabled -> 1" or "Disabled -> 0. This is the NL-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" for correct detection.
usePointToPointCost: 0
usePointToPlaneCost: 1
useBoundConstraints: 1 # This is the transition from NL-Reg. to NL-Solver. 1: NL-Reg. 0: NL-Solver.
useSixDofRegularization: 0
useThreeDofRegularization: 1
regularizationWeight: 675.0 # The weight of NL-Reg.
degeneracyAwareness:
SolutionRemapping:
threshold: 100
use2019: 0
useTruncatedSVD: 0 # Enables TSVD
skipRegistration: 0
#
# Hacky solution to use X-ICP degeneracy detection for Zhang et al., and TSVD.
enoughInformationThreshold: 300
insufficientInformationThreshold: 150
point2NormalMinimalAlignmentAngleThreshold: 80
point2NormalStrongAlignmentAngleThreshold: 45
transformationCheckers:
- DifferentialTransformationChecker:
minDiffRotErr: 0.001
minDiffTransErr: 0.01
smoothLength: 3
- CounterTransformationChecker:
maxIterationCount: 30
inspector:
NullInspector
logger:
NullLogger
errorMinimizer:
PointToPlaneErrorMinimizer
# Leave it enabled, this enforces the XICP detection.
forceXICPdetection:
Enabled: # "Enabled" or "Disabled
degeneracyDebug:
Enabled: # "Enabled" or "Disabled.
printingDegeneracy:
Enabled: # "Enabled" or "Disabled.
RMS(Petracek et al.)
readingDataPointsFilters:
# No specific filters.
referenceDataPointsFilters:
# No specific filters.
matcher:
KDTreeMatcher:
knn: 1
maxDist: 0.5
epsilon: 0.01
outlierFilters:
- TrimmedDistOutlierFilter:
ratio: 0.95
- SurfaceNormalOutlierFilter:
maxAngle: 1.57
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
regularization:
Disabled: # "Enabled" or "Disabled.
regularizationWeight: 440.0
# Enables the RMS filtering by Petracek et al.
enableRMSfiltering:
Enabled: # "Enabled" or "Disabled
rmsLambda: 0.0036 # Set to 0.0042 for Ouster based experiments.
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
ceresDegeneracyAnalysis:
CeresDegeneracyState:
isActive: 0 # "Enabled -> 1" or "Disabled -> 0. This is the NL-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" for correct detection.
usePointToPointCost: 0
usePointToPlaneCost: 1
useBoundConstraints: 1 # This is the transition from NL-Reg. to NL-Solver. 1: NL-Reg. 0: NL-Solver.
useSixDofRegularization: 0
useThreeDofRegularization: 1
regularizationWeight: 675.0 # The weight of NL-Reg.
degeneracyAwareness:
None:
transformationCheckers:
- DifferentialTransformationChecker:
minDiffRotErr: 0.001
minDiffTransErr: 0.01
smoothLength: 3
- CounterTransformationChecker:
maxIterationCount: 30
inspector:
NullInspector
logger:
NullLogger
errorMinimizer:
PointToPlaneErrorMinimizer
# Leave it enabled, this enforces the XICP detection.
forceXICPdetection:
Enabled: # "Enabled" or "Disabled
degeneracyDebug:
Enabled: # "Enabled" or "Disabled.
printingDegeneracy:
Enabled: # "Enabled" or "Disabled.
NL-Solver
readingDataPointsFilters:
# No specific filters.
referenceDataPointsFilters:
# No specific filters.
matcher:
KDTreeMatcher:
knn: 1
maxDist: 0.5
epsilon: 0.01
outlierFilters:
- TrimmedDistOutlierFilter:
ratio: 0.95
- SurfaceNormalOutlierFilter:
maxAngle: 1.57
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
regularization:
Disabled: # "Enabled" or "Disabled.
regularizationWeight: 440.0
# Enables the RMS filtering by Petracek et al.
enableRMSfiltering:
Disabled: # "Enabled" or "Disabled
rmsLambda: 0.0036 # Set to 0.0042 for Ouster based experiments.
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
ceresDegeneracyAnalysis:
CeresDegeneracyState:
isActive: 1 # "Enabled -> 1" or "Disabled -> 0. This is the NL-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" for correct detection.
usePointToPointCost: 0
usePointToPlaneCost: 1
useBoundConstraints: 1 # This is the transition from NL-Reg. to NL-Solver. 1: NL-Reg. 0: NL-Solver.
useSixDofRegularization: 0
useThreeDofRegularization: 0
regularizationWeight: 675.0 # The weight of NL-Reg.
degeneracyAwareness:
OptimizedEqualityConstraints:
enoughInformationThreshold: 300
insufficientInformationThreshold: 150
point2NormalMinimalAlignmentAngleThreshold: 80
point2NormalStrongAlignmentAngleThreshold: 45
transformationCheckers:
- DifferentialTransformationChecker:
minDiffRotErr: 0.001
minDiffTransErr: 0.01
smoothLength: 3
- CounterTransformationChecker:
maxIterationCount: 30
inspector:
NullInspector
logger:
NullLogger
errorMinimizer:
PointToPlaneErrorMinimizer
# Leave it enabled, this enforces the XICP detection.
forceXICPdetection:
Enabled: # "Enabled" or "Disabled
degeneracyDebug:
Enabled: # "Enabled" or "Disabled.
printingDegeneracy:
Enabled: # "Enabled" or "Disabled.
Gelfand et al.
readingDataPointsFilters:
- CovarianceSamplingDataPointsFilter: # Gelfand et al.
nbSample: 50000000 # In the filter, ratio based max number is applied
referenceDataPointsFilters:
# No specific filters.
matcher:
KDTreeMatcher:
knn: 1
maxDist: 0.5
epsilon: 0.01
outlierFilters:
- TrimmedDistOutlierFilter:
ratio: 0.95
- SurfaceNormalOutlierFilter:
maxAngle: 1.57
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
regularization:
Disabled: # "Enabled" or "Disabled.
regularizationWeight: 440.0
# Enables the RMS filtering by Petracek et al.
enableRMSfiltering:
Disabled: # "Enabled" or "Disabled
rmsLambda: 0.0036 # Set to 0.0042 for Ouster based experiments.
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
ceresDegeneracyAnalysis:
CeresDegeneracyState:
isActive: 0 # "Enabled -> 1" or "Disabled -> 0. This is the NL-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" for correct detection.
usePointToPointCost: 0
usePointToPlaneCost: 1
useBoundConstraints: 1 # This is the transition from NL-Reg. to NL-Solver. 1: NL-Reg. 0: NL-Solver.
useSixDofRegularization: 0
useThreeDofRegularization: 1
regularizationWeight: 675.0 # The weight of NL-Reg.
degeneracyAwareness:
None:
transformationCheckers:
- DifferentialTransformationChecker:
minDiffRotErr: 0.001
minDiffTransErr: 0.01
smoothLength: 3
- CounterTransformationChecker:
maxIterationCount: 30
inspector:
NullInspector
logger:
NullLogger
errorMinimizer:
PointToPlaneErrorMinimizer
# Leave it enabled, this enforces the XICP detection.
forceXICPdetection:
Enabled: # "Enabled" or "Disabled
degeneracyDebug:
Enabled: # "Enabled" or "Disabled.
printingDegeneracy:
Enabled: # "Enabled" or "Disabled.
Prior Only
readingDataPointsFilters:
# No specific filters.
referenceDataPointsFilters:
# No specific filters.
matcher:
KDTreeMatcher:
knn: 1
maxDist: 0.5
epsilon: 0.01
outlierFilters:
- TrimmedDistOutlierFilter:
ratio: 0.95
- SurfaceNormalOutlierFilter:
maxAngle: 1.57
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
regularization:
Disabled: # "Enabled" or "Disabled.
regularizationWeight: 440.0
# Enables the RMS filtering by Petracek et al.
enableRMSfiltering:
Disabled: # "Enabled" or "Disabled
rmsLambda: 0.0036 # Set to 0.0042 for Ouster based experiments.
# This enables L-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" degeneracyAwareness method
ceresDegeneracyAnalysis:
CeresDegeneracyState:
isActive: 0 # "Enabled -> 1" or "Disabled -> 0. This is the NL-Reg. Method. Needs to be run together with "OptimizedEqualityConstraints:" for correct detection.
usePointToPointCost: 0
usePointToPlaneCost: 1
useBoundConstraints: 1 # This is the transition from NL-Reg. to NL-Solver. 1: NL-Reg. 0: NL-Solver.
useSixDofRegularization: 0
useThreeDofRegularization: 1
regularizationWeight: 675.0 # The weight of NL-Reg.
degeneracyAwareness:
SolutionRemapping:
threshold: 100
use2019: 0
useTruncatedSVD: 0 # Enables TSVD
skipRegistration: 1
#
# Hacky solution to use X-ICP degeneracy detection for Zhang et al., and TSVD.
enoughInformationThreshold: 300
insufficientInformationThreshold: 150
point2NormalMinimalAlignmentAngleThreshold: 80
point2NormalStrongAlignmentAngleThreshold: 45
transformationCheckers:
- DifferentialTransformationChecker:
minDiffRotErr: 0.001
minDiffTransErr: 0.01
smoothLength: 3
- CounterTransformationChecker:
maxIterationCount: 30
inspector:
NullInspector
logger:
NullLogger
errorMinimizer:
PointToPlaneErrorMinimizer
# Leave it enabled, this enforces the XICP detection.
forceXICPdetection:
Enabled: # "Enabled" or "Disabled
degeneracyDebug:
Enabled: # "Enabled" or "Disabled.
printingDegeneracy:
Enabled: # "Enabled" or "Disabled.
We would like to thank all the researchers who made their work open-source to the community, which allowed us to compose this work.
All the 3rd party libraries and repositories are subject to the license of their own.
CMake Error at /usr/local/share/cmake-3.25/Modules/CMakeFindDependencyMacro.cmake:47 (find_package):
find_package called with invalid argument ".."
/usr/local/lib/cmake/Ceres/CeresConfig.cmake:182 (find_dependency)
CatkinBuild.cmake:27 (find_package)
CMakeLists.txt:81 (include)
On certain Ceres / Cmake versions this error prevents correct build. In that case, just go to the errorous file (/usr/local/lib/cmake/Ceres/CeresConfig.cmake) and remove .. which is out of date. find_dependency(CXSparse .. ) -> find_dependency(CXSparse)