Fatal error "optimizedposes.find"

[olofmat]

Hi, I use the l515 with icprgbd_odometry and the quality of the map is generally very good! But occasionally when running rosbags I get this error:

[FATAL] (2023-07-10 14:59:03.581) Rtabmap.cpp:3615::process() Condition (_optimizedPoses.find(signature->id()) != _optimizedPoses.end()) not met!
terminate called after throwing an instance of 'UException'
  what():  [FATAL] (2023-07-10 14:59:03.581) Rtabmap.cpp:3615::process() Condition (_optimizedPoses.find(signature->id()) != _optimizedPoses.end()) not met!

Signal 6 caught...

And RTABMAP is shut down while odometry keeps on running. The issue mostly happens in difficult parts of the rosbag but sometimes seems to occur when it otherwise has no issues. After running "rtabmap-recovery rtabmap.db" I get:

Processed 223/229 nodes...
Processed 224/229 nodes...
[FATAL] (2023-07-10 14:59:03.581) Rtabmap.cpp:3615::process() Condition (_optimizedPoses.find(signature->id()) != _optimizedPoses.end()) not met!
terminate called after throwing an instance of 'UException'
  what():  [FATAL] (2023-07-10 14:59:03.581) Rtabmap.cpp:3615::process() Condition (_optimizedPoses.find(signature->id()) != _optimizedPoses.end()) not met!

Signal 6 caught...

I use these to launch:

roslaunch rtabmap_ros rtabmap.launch     rgbdicp_odometry:=true     scan_cloud_topic:=/cloud     subscribe_scan_cloud:=true     depth_topic:=/camera/aligned_depth_to_color/image_raw     rgb_topic:=/camera/color/image_raw     camera_info_topic:=/camera/color/camera_info     approx_sync:=false     wait_imu_to_init:=true     imu_topic:=/rtabmap/imu use_sim_time:=true rtabmapviz:=false  rtabmap_args:="--delete_db_on_start" cfg:=~/Documents/RTAB-Map/foricp.ini

rosrun nodelet nodelet standalone rtabmap_ros/point_cloud_xyz     _approx_sync:=false      /depth/image:=/camera/depth/image_rect_raw     /depth/camera_info:=/camera/depth/camera_info     _decimation:=4

rosrun imu_filter_madgwick imu_filter_node     _use_mag:=false     _publish_tf:=false     _world_frame:="enu"     /imu/data_raw:=/camera/imu     /imu/data:=/rtabmap/imu

And this is my config file:

[Core]
Version = 0.20.23
BRIEF\Bytes = 32
BRISK\Octaves = 3
BRISK\PatternScale = 1
BRISK\Thresh = 30
Bayes\FullPredictionUpdate = false
Bayes\PredictionLC = 0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23
Bayes\VirtualPlacePriorThr = 0.9
Db\TargetVersion =
DbSqlite3\CacheSize = 10000
DbSqlite3\InMemory = false
DbSqlite3\JournalMode = 3
DbSqlite3\Synchronous = 0
DbSqlite3\TempStore = 2
FAST\CV = 0
FAST\Gpu = false
FAST\GpuKeypointsRatio = 0.05
FAST\GridCols = 0
FAST\GridRows = 0
FAST\MaxThreshold = 200
FAST\MinThreshold = 7
FAST\NonmaxSuppression = true
FAST\Threshold = 20
FREAK\NOctaves = 4
FREAK\OrientationNormalized = true
FREAK\PatternScale = 22
FREAK\ScaleNormalized = true
GFTT\BlockSize = 3
GFTT\K = 0.04
GFTT\MinDistance = 7
GFTT\QualityLevel = 0.001
GFTT\UseHarrisDetector = false
GMS\ThresholdFactor = 6.0
GMS\WithRotation = false
GMS\WithScale = false
GTSAM\Optimizer = 1
Grid\3D = true
Grid\CellSize = 0.05
Grid\ClusterRadius = 0.1
Grid\DepthDecimation = 4
Grid\DepthRoiRatios = 0.0 0.0 0.0 0.0
Grid\FlatObstacleDetected = true
Grid\FootprintHeight = 0.0
Grid\FootprintLength = 0.0
Grid\FootprintWidth = 0.0
Grid\GroundIsObstacle = false
Grid\MapFrameProjection = false
Grid\MaxGroundAngle = 45
Grid\MaxGroundHeight = 0.0
Grid\MaxObstacleHeight = 0.0
Grid\MinClusterSize = 10
Grid\MinGroundHeight = 0.0
Grid\NoiseFilteringMinNeighbors = 5
Grid\NoiseFilteringRadius = 0.0
Grid\NormalK = 20
Grid\NormalsSegmentation = true
Grid\PreVoxelFiltering = true
Grid\RangeMax = 0
Grid\RangeMin = 0.0
Grid\RayTracing = false
Grid\Scan2dUnknownSpaceFilled = false
Grid\ScanDecimation = 1
Grid\Sensor = 0
GridGlobal\AltitudeDelta = 0
GridGlobal\Eroded = false
GridGlobal\FloodFillDepth = 0
GridGlobal\FootprintRadius = 0.0
GridGlobal\FullUpdate = true
GridGlobal\MaxNodes = 0
GridGlobal\MinSize = 0.0
GridGlobal\OccupancyThr = 0.5
GridGlobal\ProbClampingMax = 0.971
GridGlobal\ProbClampingMin = 0.1192
GridGlobal\ProbHit = 0.7
GridGlobal\ProbMiss = 0.4
GridGlobal\UpdateError = 0.01
Icp\CCFilterOutFarthestPoints = false
Icp\CCMaxFinalRMS = 0.2
Icp\CCSamplingLimit = 50000
Icp\CorrespondenceRatio = 0.2
Icp\DebugExportFormat =
Icp\DownsamplingStep = 1
Icp\Epsilon = 0.005
Icp\Force4DoF = false
Icp\Iterations = 30
Icp\MaxCorrespondenceDistance = 0.1
Icp\MaxRotation = 0.78
Icp\MaxTranslation = 0.2
Icp\OutlierRatio = 0.65
Icp\PMConfig =
Icp\PMMatcherEpsilon = 0.0
Icp\PMMatcherIntensity = false
Icp\PMMatcherKnn = 1
Icp\PointToPlane = true
Icp\PointToPlaneGroundNormalsUp = 0.0
Icp\PointToPlaneK = 20
Icp\PointToPlaneLowComplexityStrategy = 1
Icp\PointToPlaneMinComplexity = 0
Icp\PointToPlaneRadius = 0
Icp\RangeMax = 0
Icp\RangeMin = 0
Icp\Strategy = 1
Icp\VoxelSize = 0.05
ImuFilter\ComplementaryBiasAlpha = 0.01
ImuFilter\ComplementaryDoAdpativeGain = true
ImuFilter\ComplementaryDoBiasEstimation = true
ImuFilter\ComplementaryGainAcc = 0.01
ImuFilter\MadgwickGain = 0.1
ImuFilter\MadgwickZeta = 0.0
KAZE\Diffusivity = 1
KAZE\Extended = false
KAZE\NOctaveLayers = 4
KAZE\NOctaves = 4
KAZE\Threshold = 0.001
KAZE\Upright = false
Kp\BadSignRatio = 0.5
Kp\ByteToFloat = false
Kp\DetectorStrategy = 8
Kp\DictionaryPath =
Kp\FlannRebalancingFactor = 2.0
Kp\GridCols = 1
Kp\GridRows = 1
Kp\IncrementalDictionary = true
Kp\IncrementalFlann = true
Kp\MaxDepth = 0
Kp\MaxFeatures = 1000
Kp\MinDepth = 0
Kp\NNStrategy = 1
Kp\NewWordsComparedTogether = true
Kp\NndrRatio = 0.8
Kp\Parallelized = true
Kp\RoiRatios = 0.0 0.0 0.0 0.0
Kp\SubPixEps = 0.02
Kp\SubPixIterations = 0
Kp\SubPixWinSize = 3
Kp\TfIdfLikelihoodUsed = true
Marker\CornerRefinementMethod = 0
Marker\Dictionary = 0
Marker\Length = 0.18
Marker\MaxDepthError = 0.01
Marker\MaxRange = 0.0
Marker\MinRange = 0.0
Marker\Priors = 4 0 0 0 0 0 0
Marker\PriorsVarianceAngular = 0.001
Marker\PriorsVarianceLinear = 0.001
Marker\VarianceAngular = 0.01
Marker\VarianceLinear = 0.001
Mem\BadSignaturesIgnored = false
Mem\BinDataKept = true
Mem\CompressionParallelized = true
Mem\CovOffDiagIgnored = true
Mem\DepthAsMask = true
Mem\GenerateIds = true
Mem\ImageCompressionFormat = .jpg
Mem\ImageKept = false
Mem\ImagePostDecimation = 1
Mem\ImagePreDecimation = 1
Mem\IncrementalMemory = true
Mem\InitWMWithAllNodes = false
Mem\IntermediateNodeDataKept = false
Mem\LaserScanDownsampleStepSize = 1
Mem\LaserScanNormalK = 0
Mem\LaserScanNormalRadius = 0.0
Mem\LaserScanVoxelSize = 0.0
Mem\LocalizationDataSaved = false
Mem\MapLabelsAdded = true
Mem\NotLinkedNodesKept = true
Mem\RawDescriptorsKept = true
Mem\RecentWmRatio = 0.2
Mem\ReduceGraph = false
Mem\RehearsalIdUpdatedToNewOne = false
Mem\RehearsalSimilarity = 0.6
Mem\RehearsalWeightIgnoredWhileMoving = false
Mem\STMSize = 10
Mem\SaveDepth16Format = false
Mem\StereoFromMotion = false
Mem\TransferSortingByWeightId = false
Mem\UseOdomFeatures = true
Mem\UseOdomGravity = false
ORB\EdgeThreshold = 19
ORB\FirstLevel = 0
ORB\Gpu = false
ORB\NLevels = 5
ORB\PatchSize = 19
ORB\ScaleFactor = 1.2
ORB\ScoreType = 0
ORB\WTA_K = 2
Odom\ScanKeyFrameThr = 0.7
Odom\Strategy = 0
Odom\ResetCountdown = 5
OdomF2M\ScanMaxSize = 15000
OdomF2M\BundleAdjustment = 0
Optimizer\Epsilon = 0.0
Optimizer\GravitySigma = 0.3
Optimizer\Iterations = 20
Optimizer\LandmarksIgnored = true
Optimizer\PriorsIgnored = true
Optimizer\Robust = false
Optimizer\Strategy = 1
Optimizer\VarianceIgnored = false
PyDetector\Cuda = true
PyDetector\Path =
PyMatcher\Cuda = true
PyMatcher\Iterations = 20
PyMatcher\Model = indoor
PyMatcher\Path =
PyMatcher\Threshold = 0.2
RGBD\AngularSpeedUpdate = 0.0
RGBD\AngularUpdate = 0.1
RGBD\CreateOccupancyGrid = false
RGBD\Enabled = true
RGBD\GoalReachedRadius = 0.5
RGBD\GoalsSavedInUserData = false
RGBD\InvertedReg = false
RGBD\LinearSpeedUpdate = 0.0
RGBD\LinearUpdate = 0.1
RGBD\LocalBundleOnLoopClosure = false
RGBD\LocalImmunizationRatio = 0.25
RGBD\LocalRadius = 10
RGBD\LoopClosureIdentityGuess = false
RGBD\LoopClosureReextractFeatures = false
RGBD\LoopCovLimited = false
RGBD\MarkerDetection = true
RGBD\MaxLocalRetrieved = 2
RGBD\MaxLoopClosureDistance = 0.0
RGBD\MaxOdomCacheSize = 10
RGBD\NeighborLinkRefining = true
RGBD\NewMapOdomChangeDistance = 0
RGBD\OptimizeFromGraphEnd = false
RGBD\OptimizeMaxError = 4.0
RGBD\PlanAngularVelocity = 0
RGBD\PlanLinearVelocity = 0
RGBD\PlanStuckIterations = 0
RGBD\ProximityAngle = 45
RGBD\ProximityBySpace = true
RGBD\ProximityByTime = false
RGBD\ProximityGlobalScanMap = false
RGBD\ProximityMaxGraphDepth = 50
RGBD\ProximityMaxPaths = 3
RGBD\ProximityMergedScanCovFactor = 100.0
RGBD\ProximityOdomGuess = false
RGBD\ProximityPathFilteringRadius = 1
RGBD\ProximityPathMaxNeighbors = 1
RGBD\ProximityPathRawPosesUsed = true
RGBD\ScanMatchingIdsSavedInLinks = true
RGBD\StartAtOrigin = false
Reg\Force3DoF = false
Reg\RepeatOnce = true
Reg\Strategy = 0
Rtabmap\ComputeRMSE = true
Rtabmap\CreateIntermediateNodes = false
Rtabmap\DetectionRate = 1
Rtabmap\ImageBufferSize = 1
Rtabmap\ImagesAlreadyRectified = true
Rtabmap\LoopGPS = true
Rtabmap\LoopRatio = 0
Rtabmap\LoopThr = 0.11
Rtabmap\MaxRepublished = 2
Rtabmap\MaxRetrieved = 2
Rtabmap\MemoryThr = 0
Rtabmap\PublishLastSignature = true
Rtabmap\PublishLikelihood = true
Rtabmap\PublishPdf = true
Rtabmap\PublishRAMUsage = false
Rtabmap\PublishStats = true
Rtabmap\RectifyOnlyFeatures = false
Rtabmap\SaveWMState = false
Rtabmap\StartNewMapOnGoodSignature = false
Rtabmap\StartNewMapOnLoopClosure = false
Rtabmap\StatisticLogged = false
Rtabmap\StatisticLoggedHeaders = true
Rtabmap\StatisticLogsBufferedInRAM = true
Rtabmap\TimeThr = 750
Rtabmap\WorkingDirectory = /home/eoaltom/.ros
SIFT\ContrastThreshold = 0.04
SIFT\EdgeThreshold = 10
SIFT\NFeatures = 0
SIFT\NOctaveLayers = 3
SIFT\RootSIFT = false
SIFT\Sigma = 1.6
SURF\Extended = false
SURF\GpuKeypointsRatio = 0.01
SURF\GpuVersion = false
SURF\HessianThreshold = 500
SURF\OctaveLayers = 2
SURF\Octaves = 4
SURF\Upright = false
Stereo\DenseStrategy = 0
Stereo\Eps = 0.01
Stereo\Iterations = 30
Stereo\MaxDisparity = 128.0
Stereo\MaxLevel = 5
Stereo\MinDisparity = 0.5
Stereo\OpticalFlow = true
Stereo\SSD = true
Stereo\WinHeight = 3
Stereo\WinWidth = 15
StereoBM\BlockSize = 15
StereoBM\Disp12MaxDiff = -1
StereoBM\MinDisparity = 0
StereoBM\NumDisparities = 128
StereoBM\PreFilterCap = 31
StereoBM\PreFilterSize = 9
StereoBM\SpeckleRange = 4
StereoBM\SpeckleWindowSize = 100
StereoBM\TextureThreshold = 10
StereoBM\UniquenessRatio = 15
StereoSGBM\BlockSize = 15
StereoSGBM\Disp12MaxDiff = 1
StereoSGBM\MinDisparity = 0
StereoSGBM\Mode = 2
StereoSGBM\NumDisparities = 128
StereoSGBM\P1 = 2
StereoSGBM\P2 = 5
StereoSGBM\PreFilterCap = 31
StereoSGBM\SpeckleRange = 4
StereoSGBM\SpeckleWindowSize = 100
StereoSGBM\UniquenessRatio = 20
SuperPoint\Cuda = true
SuperPoint\ModelPath =
SuperPoint\NMS = true
SuperPoint\NMSRadius = 4
SuperPoint\Threshold = 0.010
VhEp\Enabled = false
VhEp\MatchCountMin = 8
VhEp\RansacParam1 = 3
VhEp\RansacParam2 = 0.99
Vis\BundleAdjustment = 1
Vis\CorFlowEps = 0.01
Vis\CorFlowIterations = 30
Vis\CorFlowMaxLevel = 3
Vis\CorFlowWinSize = 16
Vis\CorGuessMatchToProjection = false
Vis\CorGuessWinSize = 40
Vis\CorNNDR = 0.8
Vis\CorNNType = 1
Vis\CorType = 0
Vis\DepthAsMask = true
Vis\EpipolarGeometryVar = 0.1
Vis\EstimationType = 1
Vis\FeatureType = 8
Vis\ForwardEstOnly = true
Vis\GridCols = 1
Vis\GridRows = 1
Vis\InlierDistance = 0.1
Vis\Iterations = 300
Vis\MaxDepth = 0
Vis\MaxFeatures = 1000
Vis\MeanInliersDistance = 0.0
Vis\MinDepth = 0
Vis\MinInliers = 20
Vis\MinInliersDistribution = 0.0
Vis\PnPFlags = 0
Vis\PnPMaxVariance = 0.0
Vis\PnPRefineIterations = 0
Vis\PnPReprojError = 2
Vis\RefineIterations = 5
Vis\RoiRatios = 0.0 0.0 0.0 0.0
Vis\SubPixEps = 0.02
Vis\SubPixIterations = 0
Vis\SubPixWinSize = 3
g2o\Baseline = 0.075
g2o\Optimizer = 0
g2o\PixelVariance = 1.0
g2o\RobustKernelDelta = 8
g2o\Solver = 0

Thanks in advance!

[olofmat]

UPDATE

It seems as setting "RGBD\OptimizeFromGraphEnd" to "true" solves the issue. Other less fatal issues appear but the previous issue seems to disappear.

I do however want to keep an origin to an aruco tag so this is not a very sustainable solution for me. Could it be that when optimizing it cannot find links all the way to the origin? But on the other hand the "optimizedPoses.find" issue happens sometimes when the local map contains the same amount of nodes as the WM.

Just thought I should add this additional information if it helps clear things up.

[matlabbe]

I would be very interesting to try

olofmat wrote

After running "rtabmap-recovery rtabmap.db" I get:

Processed 223/229 nodes...
Processed 224/229 nodes...
[FATAL] (2023-07-10 14:59:03.581) Rtabmap.cpp:3615::process() Condition (_optimizedPoses.find(signature->id()) != _optimizedPoses.end()) not met!
terminate called after throwing an instance of 'UException'
  what():  [FATAL] (2023-07-10 14:59:03.581) Rtabmap.cpp:3615::process() Condition (_optimizedPoses.find(signature->id()) != _optimizedPoses.end()) not met!

Signal 6 caught...

if you can share the database, as if recovery can reproduce the error, it will be easier to debug. This should never happen, unless there is an obscure combination of parameters that would cause this.

[olofmat]

Hi,

I don't have the database saved anymore but I will get back to you if I encounter the error again. It has not happened for a while though which is nice!

Olof