search

introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
945 stars 553 forks source link

Errors: [Not Supporting ROI] when superpoint is used as the feature detector #676

Closed szx0112 closed 2 years ago

szx0112 commented 2 years ago

Hi @matlabbe,

I tried to test the performance of Superpoint on my project. I followed your your comment to build rtabmap with libtorch 1.10 and cuda 11.3.

I can launch the rtabmap with superpoint enabled without errors. However, once I start streaming (e..g, RGBD image sequence from a D435i camera with resolution as 848x480 and 60fps), I got the Not Supporting ROI errors as below:

[FATAL] (2021-11-10 17:26:29.041) Features2d.cpp:2092::generateKeypointsImpl() Condition (roi.x==0 && roi.y ==0) not met! [Not supporting ROI]
terminate called after throwing an instance of 'UException'
  what():  [FATAL] (2021-11-10 17:26:29.041) Features2d.cpp:2092::generateKeypointsImpl() Condition (roi.x==0 && roi.y ==0) not met! [Not supporting ROI]

The roi is used as the input in this function sub_keypoints = this->generateKeypointsImpl(image, roi, mask) which is seemly shared by all feature types.

However, when SIFT, ORB or other hand craft feature is chose, I can run the code smoothly without any error.

Could you give me some advice how to handle this issue? Thanks.

matlabbe commented 2 years ago

The Region of Interest (ROI) seems to be set. What are the parameters you are using? Make sure those parameters are set to defaults:

Param: Kp/RoiRatios = "0.0 0.0 0.0 0.0"                    [Region of interest ratios [left, right, top, bottom].]
Param: Vis/RoiRatios = "0.0 0.0 0.0 0.0"                   [Region of interest ratios [left, right, top, bottom].]
szx0112 commented 2 years ago

@matlabbe Thanks for the reply. I manually set Roi parameters as suggested. I still receive the same error message. Below is the full return from the command line:

` started roslaunch server http://szx19:43199/

SUMMARY

PARAMETERS

NODES / rgbd_sync (nodelet/nodelet) /rtabmap/ rgbd_odometry (rtabmap_ros/rgbd_odometry) rtabmap (rtabmap_ros/rtabmap) rtabmapviz (rtabmap_ros/rtabmapviz)

ROS_MASTER_URI=http://localhost:11311

process[rgbd_sync-1]: started with pid [25809] process[rtabmap/rgbd_odometry-2]: started with pid [25811] type is rtabmap_ros/rgbd_sync process[rtabmap/rtabmap-3]: started with pid [25817] process[rtabmap/rtabmapviz-4]: started with pid [25820] [ INFO] [1636927786.061453182]: Starting node... [ INFO] [1636927786.097494879]: Initializing nodelet with 16 worker threads. [ INFO] [1636927786.138699664]: Initializing nodelet with 16 worker threads. [ INFO] [1636927786.159903023]: Starting node...

(rtabmapviz:25820): dbind-WARNING **: 16:09:46.248: Couldn't register with accessibility bus: Did not receive a reply. Possible causes include: the remote application did not send a reply, the message bus security policy blocked the reply, the reply timeout expired, or the network connection was broken. [ INFO] [1636927786.279505956]: Odometry: frame_id = base_link [ INFO] [1636927786.279564519]: Odometry: odom_frame_id = odom [ INFO] [1636927786.279589218]: Odometry: publish_tf = true [ INFO] [1636927786.279607264]: Odometry: wait_for_transform = true [ INFO] [1636927786.279638978]: Odometry: wait_for_transform_duration = 2.000000 [ INFO] [1636927786.279687396]: Odometry: initial_pose = xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000 [ INFO] [1636927786.279712315]: Odometry: ground_truth_frame_id = [ INFO] [1636927786.279733750]: Odometry: ground_truth_base_frame_id = [ INFO] [1636927786.279755159]: Odometry: config_path = [ INFO] [1636927786.279776369]: Odometry: publish_null_when_lost = true [ INFO] [1636927786.279797325]: Odometry: guess_frame_id = [ INFO] [1636927786.279819346]: Odometry: guess_min_translation = 0.000000 [ INFO] [1636927786.279840931]: Odometry: guess_min_rotation = 0.000000 [ INFO] [1636927786.279862081]: Odometry: guess_min_time = 0.000000 [ INFO] [1636927786.279883453]: Odometry: expected_update_rate = 0.000000 Hz [ INFO] [1636927786.279904240]: Odometry: max_update_rate = 0.000000 Hz [ INFO] [1636927786.279928150]: Odometry: wait_imu_toinit = false [ INFO] [1636927786.279965977]: Odometry: stereoParams=0 visParams=1 icpParams=0 [ INFO] [1636927786.281506726]: Setting odometry parameter "BRIEF/Bytes"="32" [ INFO] [1636927786.282059694]: Setting odometry parameter "BRISK/Octaves"="3" [ INFO] [1636927786.282553325]: Setting odometry parameter "BRISK/PatternScale"="1" [ INFO] [1636927786.283047241]: Setting odometry parameter "BRISK/Thresh"="30" [ INFO] [1636927786.283514635]: Setting odometry parameter "FAST/CV"="0" [ INFO] [1636927786.283979822]: Setting odometry parameter "FAST/Gpu"="false" [ INFO] [1636927786.284480226]: Setting odometry parameter "FAST/GpuKeypointsRatio"="0.05" [ INFO] [1636927786.285002252]: Setting odometry parameter "FAST/GridCols"="0" [ INFO] [1636927786.285532663]: Setting odometry parameter "FAST/GridRows"="0" [ INFO] [1636927786.286003235]: Setting odometry parameter "FAST/MaxThreshold"="200" [ INFO] [1636927786.286415274]: Setting odometry parameter "FAST/MinThreshold"="7" [ INFO] [1636927786.286808218]: Setting odometry parameter "FAST/NonmaxSuppression"="true" [ INFO] [1636927786.287195853]: Setting odometry parameter "FAST/Threshold"="20" [ INFO] [1636927786.287929493]: Setting odometry parameter "FREAK/NOctaves"="4" [ INFO] [1636927786.288645745]: Setting odometry parameter "FREAK/OrientationNormalized"="true" [ INFO] [1636927786.289343441]: Setting odometry parameter "FREAK/PatternScale"="22" [ INFO] [1636927786.290034545]: Setting odometry parameter "FREAK/ScaleNormalized"="true" [ INFO] [1636927786.290724225]: Setting odometry parameter "GFTT/BlockSize"="3" [ INFO] [1636927786.291441492]: Setting odometry parameter "GFTT/K"="0.04" [ INFO] [1636927786.292302503]: Setting odometry parameter "GFTT/MinDistance"="7" [ INFO] [1636927786.293219337]: Setting odometry parameter "GFTT/QualityLevel"="0.001" [ INFO] [1636927786.294118876]: Setting odometry parameter "GFTT/UseHarrisDetector"="false" [ INFO] [1636927786.294996670]: Setting odometry parameter "GMS/ThresholdFactor"="6.0" [ INFO] [1636927786.295622657]: Setting odometry parameter "GMS/WithRotation"="false" [ INFO] [1636927786.296233788]: Setting odometry parameter "GMS/WithScale"="false" [ INFO] [1636927786.296908411]: Setting odometry parameter "GTSAM/Optimizer"="1" [ INFO] [1636927786.297447548]: Setting odometry parameter "KAZE/Diffusivity"="1" [ INFO] [1636927786.297762582]: /rgbd_sync: approx_sync = false [ INFO] [1636927786.298005102]: Setting odometry parameter "KAZE/Extended"="false" [ INFO] [1636927786.298603445]: Setting odometry parameter "KAZE/NOctaveLayers"="4" [ INFO] [1636927786.299018308]: Setting odometry parameter "KAZE/NOctaves"="4" [ INFO] [1636927786.299381524]: /rgbd_sync: queue_size = 10 [ INFO] [1636927786.299432323]: /rgbd_sync: depth_scale = 1.000000 [ INFO] [1636927786.299467467]: /rgbd_sync: decimation = 1 [ INFO] [1636927786.299489909]: /rgbd_sync: compressed_rate = 0.000000 [ INFO] [1636927786.299638768]: Setting odometry parameter "KAZE/Threshold"="0.001" [ INFO] [1636927786.300313125]: Setting odometry parameter "KAZE/Upright"="false" [ INFO] [1636927786.300707425]: Setting odometry parameter "ORB/EdgeThreshold"="19" [ INFO] [1636927786.301328726]: Setting odometry parameter "ORB/FirstLevel"="0" [ INFO] [1636927786.301657862]: Setting odometry parameter "ORB/Gpu"="false" [ INFO] [1636927786.301961812]: Setting odometry parameter "ORB/NLevels"="3" [ INFO] [1636927786.302262288]: Setting odometry parameter "ORB/PatchSize"="31" [ INFO] [1636927786.302568560]: Setting odometry parameter "ORB/ScaleFactor"="2" [ INFO] [1636927786.302878302]: Setting odometry parameter "ORB/ScoreType"="0" [ INFO] [1636927786.303187358]: Setting odometry parameter "ORB/WTA_K"="2" [ INFO] [1636927786.303519357]: Setting odometry parameter "Odom/AlignWithGround"="false" [ INFO] [1636927786.304113207]: Setting odometry parameter "Odom/FillInfoData"="true" [ INFO] [1636927786.304667993]: Setting odometry parameter "Odom/FilteringStrategy"="0" [ INFO] [1636927786.304987508]: Setting odometry parameter "Odom/GuessMotion"="true" [ INFO] [1636927786.305289727]: Setting odometry parameter "Odom/GuessSmoothingDelay"="0" [ INFO] [1636927786.305586844]: Setting odometry parameter "Odom/Holonomic"="true" [ INFO] [1636927786.305897042]: Setting odometry parameter "Odom/ImageBufferSize"="1" [ INFO] [1636927786.306347048]: Setting odometry parameter "Odom/ImageDecimation"="1" [ INFO] [1636927786.306846204]: Setting odometry parameter "Odom/KalmanMeasurementNoise"="0.01" [ INFO] [1636927786.307332369]: Setting odometry parameter "Odom/KalmanProcessNoise"="0.001" [ INFO] [1636927786.308840076]: Setting odometry parameter "Odom/KeyFrameThr"="0.3" [ INFO] [1636927786.309783492]: Setting odometry parameter "Odom/ParticleLambdaR"="100" [ INFO] [1636927786.310440767]: Setting odometry parameter "Odom/ParticleLambdaT"="100" [ INFO] [1636927786.311252563]: Setting odometry parameter "Odom/ParticleNoiseR"="0.002" [ INFO] [1636927786.311920330]: Setting odometry parameter "Odom/ParticleNoiseT"="0.002" [ INFO] [1636927786.312408578]: Setting odometry parameter "Odom/ParticleSize"="400" [ INFO] [1636927786.313378349]: Setting odometry parameter "Odom/ResetCountdown"="0" [ INFO] [1636927786.313839116]: Setting odometry parameter "Odom/ScanKeyFrameThr"="0.9" [ INFO] [1636927786.316205795]: Setting odometry parameter "Odom/Strategy"="0" [ INFO] [1636927786.317245916]: Setting odometry parameter "Odom/VisKeyFrameThr"="150" [ INFO] [1636927786.318086219]: Setting odometry parameter "OdomF2M/BundleAdjustment"="1" [ INFO] [1636927786.320599610]: Setting odometry parameter "OdomF2M/BundleAdjustmentMaxFrames"="10" [ INFO] [1636927786.321566305]: Setting odometry parameter "OdomF2M/MaxNewFeatures"="0" [ INFO] [1636927786.322906709]: Setting odometry parameter "OdomF2M/MaxSize"="2000" [ INFO] [1636927786.324389888]: Setting odometry parameter "OdomF2M/ScanMaxSize"="2000" [ INFO] [1636927786.326040277]: Setting odometry parameter "OdomF2M/ScanRange"="0" [ INFO] [1636927786.326761974]: Setting odometry parameter "OdomF2M/ScanSubtractAngle"="45" [ INFO] [1636927786.327979778]: Setting odometry parameter "OdomF2M/ScanSubtractRadius"="0.05" [ INFO] [1636927786.329730861]: /rgbd_sync subscribed to (exact sync): /camera5/color/image_raw \ /camera5/aligned_depth_to_color/image_raw \ /camera5/color/camera_info [ INFO] [1636927786.330166489]: Setting odometry parameter "OdomF2M/ValidDepthRatio"="0.75" [ INFO] [1636927786.330860332]: Setting odometry parameter "OdomFovis/BucketHeight"="80" [ INFO] [1636927786.331531720]: Setting odometry parameter "OdomFovis/BucketWidth"="80" [ INFO] [1636927786.332256539]: Setting odometry parameter "OdomFovis/CliqueInlierThreshold"="0.1" [ INFO] [1636927786.332950059]: Setting odometry parameter "OdomFovis/FastThreshold"="20" [ INFO] [1636927786.333372592]: Setting odometry parameter "OdomFovis/FastThresholdAdaptiveGain"="0.005" [ INFO] [1636927786.334137021]: Setting odometry parameter "OdomFovis/FeatureSearchWindow"="25" [ INFO] [1636927786.334798538]: Setting odometry parameter "OdomFovis/FeatureWindowSize"="9" [ INFO] [1636927786.335523641]: Setting odometry parameter "OdomFovis/InlierMaxReprojectionError"="1.5" [ INFO] [1636927786.336250051]: Setting odometry parameter "OdomFovis/MaxKeypointsPerBucket"="25" [ INFO] [1636927786.336972369]: Setting odometry parameter "OdomFovis/MaxMeanReprojectionError"="10.0" [ INFO] [1636927786.337770665]: Setting odometry parameter "OdomFovis/MaxPyramidLevel"="3" [ INFO] [1636927786.338283247]: Setting odometry parameter "OdomFovis/MinFeaturesForEstimate"="20" [ INFO] [1636927786.338860622]: rtabmapviz: Using configuration from "/home/szx/catkin_ws/src/rtabmap_ros/launch/config/rgbd_gui.ini" [ INFO] [1636927786.339327156]: Setting odometry parameter "OdomFovis/MinPyramidLevel"="0" [ INFO] [1636927786.340151595]: Setting odometry parameter "OdomFovis/StereoMaxDisparity"="128" [ INFO] [1636927786.340744673]: Setting odometry parameter "OdomFovis/StereoMaxDistEpipolarLine"="1.5" [ INFO] [1636927786.341850364]: Setting odometry parameter "OdomFovis/StereoMaxRefinementDisplacement"="1.0" [ INFO] [1636927786.342615232]: Setting odometry parameter "OdomFovis/StereoRequireMutualMatch"="true" [ INFO] [1636927786.343366088]: Setting odometry parameter "OdomFovis/TargetPixelsPerFeature"="250" [ INFO] [1636927786.344302315]: Setting odometry parameter "OdomFovis/UpdateTargetFeaturesWithRefined"="false" [ INFO] [1636927786.345094516]: Setting odometry parameter "OdomFovis/UseAdaptiveThreshold"="true" [ INFO] [1636927786.346102553]: Setting odometry parameter "OdomFovis/UseBucketing"="true" [ INFO] [1636927786.346671951]: Setting odometry parameter "OdomFovis/UseHomographyInitialization"="true" [ INFO] [1636927786.347190552]: Setting odometry parameter "OdomFovis/UseImageNormalization"="false" [ INFO] [1636927786.347769559]: Setting odometry parameter "OdomFovis/UseSubpixelRefinement"="true" [ INFO] [1636927786.348319248]: Setting odometry parameter "OdomLOAM/AngVar"="0.01" [ INFO] [1636927786.348866284]: Setting odometry parameter "OdomLOAM/LinVar"="0.01" [ INFO] [1636927786.349395800]: Setting odometry parameter "OdomLOAM/LocalMapping"="true" [ INFO] [1636927786.349901560]: Setting odometry parameter "OdomLOAM/Resolution"="0.2" [ INFO] [1636927786.350398982]: Setting odometry parameter "OdomLOAM/ScanPeriod"="0.1" [ INFO] [1636927786.350911729]: Setting odometry parameter "OdomLOAM/Sensor"="2" [ INFO] [1636927786.351425450]: Setting odometry parameter "OdomMSCKF/FastThreshold"="10" [ INFO] [1636927786.351958002]: Setting odometry parameter "OdomMSCKF/GridCol"="5" [ INFO] [1636927786.352509301]: Setting odometry parameter "OdomMSCKF/GridMaxFeatureNum"="4" [ INFO] [1636927786.353035940]: Setting odometry parameter "OdomMSCKF/GridMinFeatureNum"="3" [ INFO] [1636927786.353534393]: Setting odometry parameter "OdomMSCKF/GridRow"="4" [ INFO] [1636927786.354024050]: Setting odometry parameter "OdomMSCKF/InitCovAccBias"="0.01" [ INFO] [1636927786.354370714]: Setting odometry parameter "OdomMSCKF/InitCovExRot"="0.00030462" [ INFO] [1636927786.354707673]: Setting odometry parameter "OdomMSCKF/InitCovExTrans"="0.000025" [ INFO] [1636927786.355051611]: Setting odometry parameter "OdomMSCKF/InitCovGyroBias"="0.01" [ INFO] [1636927786.355422194]: Setting odometry parameter "OdomMSCKF/InitCovVel"="0.25" [ INFO] [1636927786.355774692]: Setting odometry parameter "OdomMSCKF/MaxCamStateSize"="20" [ INFO] [1636927786.356117792]: Setting odometry parameter "OdomMSCKF/MaxIteration"="30" [ INFO] [1636927786.356453671]: Setting odometry parameter "OdomMSCKF/NoiseAcc"="0.05" [ INFO] [1636927786.356832477]: Setting odometry parameter "OdomMSCKF/NoiseAccBias"="0.01" [ INFO] [1636927786.357200010]: Setting odometry parameter "OdomMSCKF/NoiseFeature"="0.035" [ INFO] [1636927786.357590268]: Setting odometry parameter "OdomMSCKF/NoiseGyro"="0.005" [ INFO] [1636927786.357989468]: Setting odometry parameter "OdomMSCKF/NoiseGyroBias"="0.001" [ INFO] [1636927786.358346376]: Setting odometry parameter "OdomMSCKF/OptTranslationThreshold"="0" [ INFO] [1636927786.358697631]: Setting odometry parameter "OdomMSCKF/PatchSize"="15" [ INFO] [1636927786.359042288]: Setting odometry parameter "OdomMSCKF/PositionStdThreshold"="8.0" [ INFO] [1636927786.359424724]: Setting odometry parameter "OdomMSCKF/PyramidLevels"="3" [ INFO] [1636927786.359755551]: Setting odometry parameter "OdomMSCKF/RansacThreshold"="3" [ INFO] [1636927786.360480268]: Setting odometry parameter "OdomMSCKF/RotationThreshold"="0.2618" [ INFO] [1636927786.361141700]: Setting odometry parameter "OdomMSCKF/StereoThreshold"="5" [ INFO] [1636927786.361787849]: Setting odometry parameter "OdomMSCKF/TrackPrecision"="0.01" [ INFO] [1636927786.362390967]: Setting odometry parameter "OdomMSCKF/TrackingRateThreshold"="0.5" [ INFO] [1636927786.363072037]: Setting odometry parameter "OdomMSCKF/TranslationThreshold"="0.4" [ INFO] [1636927786.363726844]: Setting odometry parameter "OdomMono/InitMinFlow"="100" [ INFO] [1636927786.364360868]: Setting odometry parameter "OdomMono/InitMinTranslation"="0.1" [ INFO] [1636927786.364964247]: Setting odometry parameter "OdomMono/MaxVariance"="0.01" [ INFO] [1636927786.365558950]: Setting odometry parameter "OdomMono/MinTranslation"="0.02" [ INFO] [1636927786.366147008]: Setting odometry parameter "OdomOKVIS/ConfigPath"="" [ INFO] [1636927786.366750721]: Setting odometry parameter "OdomORBSLAM/Bf"="0.076" [ INFO] [1636927786.367362857]: Setting odometry parameter "OdomORBSLAM/Fps"="0.0" [ INFO] [1636927786.367959739]: Setting odometry parameter "OdomORBSLAM/MapSize"="3000" [ INFO] [1636927786.368583522]: Setting odometry parameter "OdomORBSLAM/MaxFeatures"="1000" [ INFO] [1636927786.369185153]: Setting odometry parameter "OdomORBSLAM/ThDepth"="40.0" [ INFO] [1636927786.369754880]: Setting odometry parameter "OdomORBSLAM/VocPath"="" [ INFO] [1636927786.370312664]: Setting odometry parameter "OdomVINS/ConfigPath"="" [ INFO] [1636927786.370903473]: Setting odometry parameter "OdomViso2/BucketHeight"="50" [ INFO] [1636927786.371521095]: Setting odometry parameter "OdomViso2/BucketMaxFeatures"="2" [ INFO] [1636927786.372169059]: Setting odometry parameter "OdomViso2/BucketWidth"="50" [ INFO] [1636927786.372804380]: Setting odometry parameter "OdomViso2/InlierThreshold"="2.0" [ INFO] [1636927786.373276689]: Setting odometry parameter "OdomViso2/MatchBinsize"="50" [ INFO] [1636927786.373628629]: Setting odometry parameter "OdomViso2/MatchDispTolerance"="2" [ INFO] [1636927786.373968708]: Setting odometry parameter "OdomViso2/MatchHalfResolution"="true" [ INFO] [1636927786.374307658]: Setting odometry parameter "OdomViso2/MatchMultiStage"="true" [ INFO] [1636927786.374641342]: Setting odometry parameter "OdomViso2/MatchNmsN"="3" [ INFO] [1636927786.374975074]: Setting odometry parameter "OdomViso2/MatchNmsTau"="50" [ INFO] [1636927786.375330558]: Setting odometry parameter "OdomViso2/MatchOutlierDispTolerance"="5" [ INFO] [1636927786.375677313]: Setting odometry parameter "OdomViso2/MatchOutlierFlowTolerance"="5" [ INFO] [1636927786.376019601]: Setting odometry parameter "OdomViso2/MatchRadius"="200" [ INFO] [1636927786.376354187]: Setting odometry parameter "OdomViso2/MatchRefinement"="1" [ INFO] [1636927786.376687848]: Setting odometry parameter "OdomViso2/RansacIters"="200" [ INFO] [1636927786.377023307]: Setting odometry parameter "OdomViso2/Reweighting"="true" [ INFO] [1636927786.377353700]: Setting odometry parameter "Optimizer/Epsilon"="0.00001" [ INFO] [1636927786.377684487]: Setting odometry parameter "Optimizer/GravitySigma"="0.3" [ INFO] [1636927786.378024757]: Setting odometry parameter "Optimizer/Iterations"="20" [ INFO] [1636927786.378363226]: Setting odometry parameter "Optimizer/LandmarksIgnored"="false" [ INFO] [1636927786.378697682]: Setting odometry parameter "Optimizer/PriorsIgnored"="true" [ INFO] [1636927786.379034169]: Setting odometry parameter "Optimizer/Robust"="false" [ INFO] [1636927786.379404121]: Setting odometry parameter "Optimizer/Strategy"="2" [ INFO] [1636927786.379769492]: Setting odometry parameter "Optimizer/VarianceIgnored"="false" [ INFO] [1636927786.380117426]: Setting odometry parameter "PyDetector/Cuda"="true" [ INFO] [1636927786.380462070]: Setting odometry parameter "PyDetector/Path"="" [ INFO] [1636927786.380799950]: Setting odometry parameter "PyMatcher/Cuda"="true" [ INFO] [1636927786.381140123]: Setting odometry parameter "PyMatcher/Iterations"="20" [ INFO] [1636927786.381478111]: Setting odometry parameter "PyMatcher/Model"="indoor" [ INFO] [1636927786.381813345]: Setting odometry parameter "PyMatcher/Path"="" [ INFO] [1636927786.382145590]: Setting odometry parameter "PyMatcher/Threshold"="0.2" [ INFO] [1636927786.382482465]: Setting odometry parameter "Reg/Force3DoF"="false" [ INFO] [1636927786.382823663]: Setting odometry parameter "Reg/RepeatOnce"="true" [ INFO] [1636927786.383158914]: Setting odometry parameter "Reg/Strategy"="0" [ INFO] [1636927786.383513134]: Setting odometry parameter "Rtabmap/ImagesAlreadyRectified"="true" [ INFO] [1636927786.383834780]: Setting odometry parameter "Rtabmap/PublishRAMUsage"="false" [ INFO] [1636927786.384152791]: Setting odometry parameter "SIFT/ContrastThreshold"="0.04" [ INFO] [1636927786.384466930]: Setting odometry parameter "SIFT/EdgeThreshold"="10" [ INFO] [1636927786.384781254]: Setting odometry parameter "SIFT/NFeatures"="0" [ INFO] [1636927786.385096165]: Setting odometry parameter "SIFT/NOctaveLayers"="3" [ INFO] [1636927786.385407590]: Setting odometry parameter "SIFT/RootSIFT"="false" [ INFO] [1636927786.385715943]: Setting odometry parameter "SIFT/Sigma"="1.6" [ INFO] [1636927786.386035748]: Setting odometry parameter "SURF/Extended"="false" [ INFO] [1636927786.386352167]: Setting odometry parameter "SURF/GpuKeypointsRatio"="0.01" [ INFO] [1636927786.386665973]: Setting odometry parameter "SURF/GpuVersion"="false" [ INFO] [1636927786.386988912]: Setting odometry parameter "SURF/HessianThreshold"="500" [ INFO] [1636927786.387322103]: Setting odometry parameter "SURF/OctaveLayers"="2" [ INFO] [1636927786.387645152]: Setting odometry parameter "SURF/Octaves"="4" [ INFO] [1636927786.387968362]: Setting odometry parameter "SURF/Upright"="false" [ INFO] [1636927786.388282048]: Setting odometry parameter "SuperPoint/Cuda"="true" [ INFO] [1636927786.388601121]: Setting odometry parameter "SuperPoint/ModelPath"="/home/szx/catkin_ws/src/rtabmap_ros/cfg/superpoint.pt" [ INFO] [1636927786.388917545]: Setting odometry parameter "SuperPoint/NMS"="true" [ INFO] [1636927786.389228221]: Setting odometry parameter "SuperPoint/NMSRadius"="4" [ INFO] [1636927786.389539350]: Setting odometry parameter "SuperPoint/Threshold"="0.010" [ INFO] [1636927786.390415832]: Setting odometry parameter "Vis/BundleAdjustment"="1" [ INFO] [1636927786.390742329]: Setting odometry parameter "Vis/CorFlowEps"="0.01" [ INFO] [1636927786.391056776]: Setting odometry parameter "Vis/CorFlowIterations"="30" [ INFO] [1636927786.391388932]: Setting odometry parameter "Vis/CorFlowMaxLevel"="3" [ INFO] [1636927786.391715771]: Setting odometry parameter "Vis/CorFlowWinSize"="16" [ INFO] [1636927786.392030704]: Setting odometry parameter "Vis/CorGuessMatchToProjection"="false" [ INFO] [1636927786.392890842]: Setting odometry parameter "Vis/CorGuessWinSize"="20" [ INFO] [1636927786.393216674]: Setting odometry parameter "Vis/CorNNDR"="0.8" [ INFO] [1636927786.394109247]: Setting odometry parameter "Vis/CorNNType"="1" [ INFO] [1636927786.395004525]: Setting odometry parameter "Vis/CorType"="0" [ INFO] [1636927786.395359635]: Setting odometry parameter "Vis/DepthAsMask"="true" [ INFO] [1636927786.395733016]: Setting odometry parameter "Vis/EpipolarGeometryVar"="0.1" [ INFO] [1636927786.396754239]: Setting odometry parameter "Vis/EstimationType"="1" [ INFO] [1636927786.397706035]: Setting odometry parameter "Vis/FeatureType"="11" [ INFO] [1636927786.398075335]: Setting odometry parameter "Vis/ForwardEstOnly"="true" [ INFO] [1636927786.399028386]: Setting odometry parameter "Vis/GridCols"="20" [ INFO] [1636927786.400009847]: Setting odometry parameter "Vis/GridRows"="20" [ INFO] [1636927786.400985137]: Setting odometry parameter "Vis/InlierDistance"="0.01" [ INFO] [1636927786.401344832]: Setting odometry parameter "Vis/Iterations"="300" [ INFO] [1636927786.402417341]: Setting odometry parameter "Vis/MaxDepth"="0.7" [ INFO] [1636927786.403411056]: Setting odometry parameter "Vis/MaxFeatures"="1000" [ INFO] [1636927786.403796758]: Setting odometry parameter "Vis/MeanInliersDistance"="0.0" [ INFO] [1636927786.404760758]: Setting odometry parameter "Vis/MinDepth"="0.15" [ INFO] [1636927786.405704258]: Setting odometry parameter "Vis/MinInliers"="20" [ INFO] [1636927786.406075038]: Setting odometry parameter "Vis/MinInliersDistribution"="0.0" [ INFO] [1636927786.406438508]: Setting odometry parameter "Vis/PnPFlags"="0" [ INFO] [1636927786.406803966]: Setting odometry parameter "Vis/PnPRefineIterations"="0" [ INFO] [1636927786.407164637]: Setting odometry parameter "Vis/PnPReprojError"="2" [ INFO] [1636927786.408151072]: Setting odometry parameter "Vis/RefineIterations"="20" [ INFO] [1636927786.408560222]: Setting odometry parameter "Vis/RoiRatios"="0.0 0.0 0.0 0.0" [ INFO] [1636927786.408945205]: Setting odometry parameter "Vis/SubPixEps"="0.02" [ INFO] [1636927786.409338194]: Setting odometry parameter "Vis/SubPixIterations"="0" [ INFO] [1636927786.409685968]: Setting odometry parameter "Vis/SubPixWinSize"="3" [ INFO] [1636927786.410034186]: Setting odometry parameter "g2o/Baseline"="0.075" [ INFO] [1636927786.410372477]: Setting odometry parameter "g2o/Optimizer"="0" [ INFO] [1636927786.410711050]: Setting odometry parameter "g2o/PixelVariance"="1.0" [ INFO] [1636927786.411044714]: Setting odometry parameter "g2o/RobustKernelDelta"="8" [ INFO] [1636927786.411392006]: Setting odometry parameter "g2o/Solver"="0" [ INFO] [1636927786.475056353]: /rtabmap/rtabmap(maps): map_filter_radius = 0.000000 [ INFO] [1636927786.475105237]: /rtabmap/rtabmap(maps): map_filter_angle = 30.000000 [ INFO] [1636927786.475127017]: /rtabmap/rtabmap(maps): map_cleanup = true [ INFO] [1636927786.475145718]: /rtabmap/rtabmap(maps): map_always_update = false [ INFO] [1636927786.475162562]: /rtabmap/rtabmap(maps): map_empty_ray_tracing = true [ INFO] [1636927786.475179003]: /rtabmap/rtabmap(maps): cloud_output_voxelized = true [ INFO] [1636927786.475195579]: /rtabmap/rtabmap(maps): cloud_subtract_filtering = false [ INFO] [1636927786.475220359]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2 [ INFO] [1636927786.475755802]: /rtabmap/rtabmap(maps): octomap_tree_depth = 16 [ INFO] [1636927786.519567021]: rtabmap: frame_id = base_link [ INFO] [1636927786.519624869]: rtabmap: odom_frame_id = odom [ INFO] [1636927786.519658604]: rtabmap: map_frame_id = map [ INFO] [1636927786.519693568]: rtabmap: use_action_for_goal = false [ INFO] [1636927786.519735389]: rtabmap: tf_delay = 0.050000 [ INFO] [1636927786.519771376]: rtabmap: tf_tolerance = 0.100000 [ INFO] [1636927786.519837128]: rtabmap: odom_sensor_sync = false [ INFO] [1636927786.520553390]: rtabmap: gen_scan = false [ INFO] [1636927786.520610366]: rtabmap: gen_depth = false [ INFO] [1636927786.558138610]: Setting RTAB-Map parameter "BRIEF/Bytes"="32" [ INFO] [1636927786.558726299]: Setting RTAB-Map parameter "BRISK/Octaves"="3" [ INFO] [1636927786.559383957]: Setting RTAB-Map parameter "BRISK/PatternScale"="1" [ INFO] [1636927786.559895404]: Setting RTAB-Map parameter "BRISK/Thresh"="30" [ INFO] [1636927786.560550692]: Setting RTAB-Map parameter "Bayes/FullPredictionUpdate"="false" [ INFO] [1636927786.561058681]: Setting RTAB-Map parameter "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" [ INFO] [1636927786.561708251]: Setting RTAB-Map parameter "Bayes/VirtualPlacePriorThr"="0.9" [ INFO] [1636927786.562220094]: Setting RTAB-Map parameter "Db/TargetVersion"="" [ INFO] [1636927786.562919600]: Setting RTAB-Map parameter "DbSqlite3/CacheSize"="10000" [ INFO] [1636927786.563999504]: Setting RTAB-Map parameter "DbSqlite3/InMemory"="false" [ INFO] [1636927786.564702659]: Setting RTAB-Map parameter "DbSqlite3/JournalMode"="3" [ INFO] [1636927786.565476532]: Setting RTAB-Map parameter "DbSqlite3/Synchronous"="0" [ INFO] [1636927786.566113674]: Setting RTAB-Map parameter "DbSqlite3/TempStore"="2" [ INFO] [1636927786.566887085]: Setting RTAB-Map parameter "FAST/CV"="0" [ INFO] [1636927786.567484428]: Setting RTAB-Map parameter "FAST/Gpu"="false" [ INFO] [1636927786.568290395]: Setting RTAB-Map parameter "FAST/GpuKeypointsRatio"="0.05" [ INFO] [1636927786.568953948]: Setting RTAB-Map parameter "FAST/GridCols"="0" [ INFO] [1636927786.569666515]: Setting RTAB-Map parameter "FAST/GridRows"="0" [ INFO] [1636927786.570303172]: Setting RTAB-Map parameter "FAST/MaxThreshold"="200" [ INFO] [1636927786.571019204]: Setting RTAB-Map parameter "FAST/MinThreshold"="7" [ INFO] [1636927786.571694858]: Setting RTAB-Map parameter "FAST/NonmaxSuppression"="true" [ INFO] [1636927786.572462516]: Setting RTAB-Map parameter "FAST/Threshold"="20" [ INFO] [1636927786.573345435]: Setting RTAB-Map parameter "FREAK/NOctaves"="4" [ INFO] [1636927786.574001681]: Setting RTAB-Map parameter "FREAK/OrientationNormalized"="true" [ INFO] [1636927786.574660497]: Setting RTAB-Map parameter "FREAK/PatternScale"="22" [ INFO] [1636927786.575103465]: Setting RTAB-Map parameter "FREAK/ScaleNormalized"="true" [ INFO] [1636927786.575783740]: Setting RTAB-Map parameter "GFTT/BlockSize"="3" [ INFO] [1636927786.576233360]: Setting RTAB-Map parameter "GFTT/K"="0.04" [ INFO] [1636927786.576894945]: Setting RTAB-Map parameter "GFTT/MinDistance"="7" [ INFO] [1636927786.577401307]: Setting RTAB-Map parameter "GFTT/QualityLevel"="0.001" [ INFO] [1636927786.578127532]: Setting RTAB-Map parameter "GFTT/UseHarrisDetector"="false" [ INFO] [1636927786.578546335]: Setting RTAB-Map parameter "GMS/ThresholdFactor"="6.0" [ INFO] [1636927786.579211659]: Setting RTAB-Map parameter "GMS/WithRotation"="false" [ INFO] [1636927786.579855813]: Setting RTAB-Map parameter "GMS/WithScale"="false" [ INFO] [1636927786.580729982]: Setting RTAB-Map parameter "GTSAM/Optimizer"="1" [ INFO] [1636927786.581288094]: Setting RTAB-Map parameter "Grid/3D"="true" [ INFO] [1636927786.582131626]: Setting RTAB-Map parameter "Grid/CellSize"="0.05" [ INFO] [1636927786.582682224]: Setting RTAB-Map parameter "Grid/ClusterRadius"="0.1" [ INFO] [1636927786.583602926]: Setting RTAB-Map parameter "Grid/DepthDecimation"="4" [ INFO] [1636927786.584137944]: Setting RTAB-Map parameter "Grid/DepthRoiRatios"="0.0 0.0 0.0 0.0" [ INFO] [1636927786.585031478]: Setting RTAB-Map parameter "Grid/FlatObstacleDetected"="true" [ INFO] [1636927786.585525310]: Setting RTAB-Map parameter "Grid/FootprintHeight"="0.0" [ INFO] [1636927786.586435579]: Setting RTAB-Map parameter "Grid/FootprintLength"="0.0" [ INFO] [1636927786.586976247]: Setting RTAB-Map parameter "Grid/FootprintWidth"="0.0" [ INFO] [1636927786.587616245]: Setting RTAB-Map parameter "Grid/GroundIsObstacle"="false" [ INFO] [1636927786.588048429]: Setting RTAB-Map parameter "Grid/MapFrameProjection"="false" [ INFO] [1636927786.588717060]: Setting RTAB-Map parameter "Grid/MaxGroundAngle"="45" [ INFO] [1636927786.589131220]: Setting RTAB-Map parameter "Grid/MaxGroundHeight"="0.0" [ INFO] [1636927786.589768050]: Setting RTAB-Map parameter "Grid/MaxObstacleHeight"="0.0" [ INFO] [1636927786.590202143]: Setting RTAB-Map parameter "Grid/MinClusterSize"="10" [ INFO] [1636927786.590889806]: Setting RTAB-Map parameter "Grid/MinGroundHeight"="0.0" [ INFO] [1636927786.591355218]: Setting RTAB-Map parameter "Grid/NoiseFilteringMinNeighbors"="5" [ INFO] [1636927786.591999896]: Setting RTAB-Map parameter "Grid/NoiseFilteringRadius"="0.0" [ INFO] [1636927786.592667127]: Setting RTAB-Map parameter "Grid/NormalK"="20" [ INFO] [1636927786.593208149]: Setting RTAB-Map parameter "Grid/NormalsSegmentation"="true" [ INFO] [1636927786.593841847]: Setting RTAB-Map parameter "Grid/PreVoxelFiltering"="true" [ INFO] [1636927786.594286562]: Setting RTAB-Map parameter "Grid/RangeMax"="5.0" [ INFO] [1636927786.594929295]: Setting RTAB-Map parameter "Grid/RangeMin"="0.0" [ INFO] [1636927786.595420890]: Setting RTAB-Map parameter "Grid/RayTracing"="false" [ INFO] [1636927786.596532019]: Setting RTAB-Map parameter "Grid/Scan2dUnknownSpaceFilled"="false" [ INFO] [1636927786.597039450]: Setting RTAB-Map parameter "Grid/ScanDecimation"="1" [ INFO] [1636927786.598376945]: Setting RTAB-Map parameter "Grid/Sensor"="true" [ INFO] [1636927786.599118221]: Setting RTAB-Map parameter "GridGlobal/AltitudeDelta"="0" [ INFO] [1636927786.599641289]: Setting RTAB-Map parameter "GridGlobal/Eroded"="false" [ INFO] [1636927786.600455449]: Setting RTAB-Map parameter "GridGlobal/FloodFillDepth"="0" [ INFO] [1636927786.600950272]: Setting RTAB-Map parameter "GridGlobal/FootprintRadius"="0.0" [ INFO] [1636927786.601815806]: Setting RTAB-Map parameter "GridGlobal/FullUpdate"="true" [ INFO] [1636927786.602280824]: Setting RTAB-Map parameter "GridGlobal/MaxNodes"="0" [ INFO] [1636927786.603089661]: Setting RTAB-Map parameter "GridGlobal/MinSize"="0.0" [ INFO] [1636927786.603631040]: Setting RTAB-Map parameter "GridGlobal/OccupancyThr"="0.5" [ INFO] [1636927786.604437049]: Setting RTAB-Map parameter "GridGlobal/ProbClampingMax"="0.971" [ INFO] [1636927786.604915051]: Setting RTAB-Map parameter "GridGlobal/ProbClampingMin"="0.1192" [ INFO] [1636927786.605717871]: Setting RTAB-Map parameter "GridGlobal/ProbHit"="0.7" [ INFO] [1636927786.606090599]: Setting RTAB-Map parameter "GridGlobal/ProbMiss"="0.4" [ INFO] [1636927786.606685009]: Setting RTAB-Map parameter "GridGlobal/UpdateError"="0.01" [ INFO] [1636927786.607074006]: Setting RTAB-Map parameter "Icp/CCFilterOutFarthestPoints"="false" [ INFO] [1636927786.607735986]: Setting RTAB-Map parameter "Icp/CCMaxFinalRMS"="0.2" [ INFO] [1636927786.608182777]: Setting RTAB-Map parameter "Icp/CCSamplingLimit"="50000" [ INFO] [1636927786.608753384]: Setting RTAB-Map parameter "Icp/CorrespondenceRatio"="0.1" [ INFO] [1636927786.609196690]: Setting RTAB-Map parameter "Icp/DebugExportFormat"="" [ INFO] [1636927786.609774033]: Setting RTAB-Map parameter "Icp/DownsamplingStep"="1" [ INFO] [1636927786.610212584]: Setting RTAB-Map parameter "Icp/Epsilon"="0" [ INFO] [1636927786.610832165]: Setting RTAB-Map parameter "Icp/Force4DoF"="false" [ INFO] [1636927786.611283368]: Setting RTAB-Map parameter "Icp/Iterations"="30" [ INFO] [1636927786.611936537]: Setting RTAB-Map parameter "Icp/MaxCorrespondenceDistance"="0.1" [ INFO] [1636927786.612714094]: Setting RTAB-Map parameter "Icp/MaxRotation"="0.78" [ INFO] [1636927786.613358652]: Setting RTAB-Map parameter "Icp/MaxTranslation"="0.2" [ INFO] [1636927786.613878380]: Setting RTAB-Map parameter "Icp/OutlierRatio"="0.85" [ INFO] [1636927786.614451927]: Setting RTAB-Map parameter "Icp/PMConfig"="" [ INFO] [1636927786.614865782]: Setting RTAB-Map parameter "Icp/PMMatcherEpsilon"="0.0" [ INFO] [1636927786.615450656]: Setting RTAB-Map parameter "Icp/PMMatcherIntensity"="false" [ INFO] [1636927786.616209161]: Setting RTAB-Map parameter "Icp/PMMatcherKnn"="1" [ INFO] [1636927786.616708565]: Setting RTAB-Map parameter "Icp/PointToPlane"="true" [ INFO] [1636927786.617188664]: Setting RTAB-Map parameter "Icp/PointToPlaneGroundNormalsUp"="0.0" [ INFO] [1636927786.617755695]: Setting RTAB-Map parameter "Icp/PointToPlaneK"="5" [ INFO] [1636927786.618169309]: Setting RTAB-Map parameter "Icp/PointToPlaneLowComplexityStrategy"="1" [ INFO] [1636927786.618819578]: Setting RTAB-Map parameter "Icp/PointToPlaneMinComplexity"="0.02" [ INFO] [1636927786.619176855]: Setting RTAB-Map parameter "Icp/PointToPlaneRadius"="1.0" [ INFO] [1636927786.619804151]: Setting RTAB-Map parameter "Icp/RangeMax"="0" [ INFO] [1636927786.620145452]: Setting RTAB-Map parameter "Icp/RangeMin"="0" [ INFO] [1636927786.620818331]: Setting RTAB-Map parameter "Icp/Strategy"="1" [ INFO] [1636927786.621206818]: Setting RTAB-Map parameter "Icp/VoxelSize"="0.05" [ INFO] [1636927786.621830004]: Setting RTAB-Map parameter "ImuFilter/ComplementaryBiasAlpha"="0.01" [ INFO] [1636927786.622222931]: Setting RTAB-Map parameter "ImuFilter/ComplementaryDoAdpativeGain"="true" [ INFO] [1636927786.622818881]: Setting RTAB-Map parameter "ImuFilter/ComplementaryDoBiasEstimation"="true" [ INFO] [1636927786.623205962]: Setting RTAB-Map parameter "ImuFilter/ComplementaryGainAcc"="0.01" [ INFO] [1636927786.623829038]: Setting RTAB-Map parameter "ImuFilter/MadgwickGain"="0.1" [ INFO] [1636927786.624319606]: Setting RTAB-Map parameter "ImuFilter/MadgwickZeta"="0.0" [ INFO] [1636927786.624861002]: Setting RTAB-Map parameter "KAZE/Diffusivity"="1" [ INFO] [1636927786.625244700]: Setting RTAB-Map parameter "KAZE/Extended"="false" [ INFO] [1636927786.625867285]: Setting RTAB-Map parameter "KAZE/NOctaveLayers"="4" [ INFO] [1636927786.626507300]: Setting RTAB-Map parameter "KAZE/NOctaves"="4" [ INFO] [1636927786.626959926]: Setting RTAB-Map parameter "KAZE/Threshold"="0.001" [ INFO] [1636927786.627504161]: Setting RTAB-Map parameter "KAZE/Upright"="false" [ INFO] [1636927786.627906089]: Setting RTAB-Map parameter "Kp/BadSignRatio"="0.5" [ INFO] [1636927786.628494472]: Setting RTAB-Map parameter "Kp/ByteToFloat"="false" [ INFO] [1636927786.630230066]: Setting RTAB-Map parameter "Kp/DetectorStrategy"="11" [ INFO] [1636927786.630638394]: Setting RTAB-Map parameter "Kp/DictionaryPath"="" [ INFO] [1636927786.631195311]: Setting RTAB-Map parameter "Kp/FlannRebalancingFactor"="2.0" [ INFO] [1636927786.631587359]: Setting RTAB-Map parameter "Kp/GridCols"="1" [ INFO] [1636927786.632196993]: Setting RTAB-Map parameter "Kp/GridRows"="1" [ INFO] [1636927786.632630331]: Setting RTAB-Map parameter "Kp/IncrementalDictionary"="true" [ INFO] [1636927786.633548019]: Setting RTAB-Map parameter "Kp/IncrementalFlann"="true" [ INFO] [1636927786.634238278]: Setting RTAB-Map parameter "Kp/MaxDepth"="0.8" [ INFO] [1636927786.635031879]: Setting RTAB-Map parameter "Kp/MaxFeatures"="500" [ INFO] [1636927786.635738603]: Setting RTAB-Map parameter "Kp/MinDepth"="0" [ INFO] [1636927786.637968079]: Setting RTAB-Map parameter "Kp/NNStrategy"="1" [ INFO] [1636927786.638634457]: Setting RTAB-Map parameter "Kp/NewWordsComparedTogether"="true" [ INFO] [1636927786.639444693]: Setting RTAB-Map parameter "Kp/NndrRatio"="0.8" [ INFO] [1636927786.640123032]: Setting RTAB-Map parameter "Kp/Parallelized"="true" [ INFO] [1636927786.641000543]: Setting RTAB-Map parameter "Kp/RoiRatios"="0.0 0.0 0.0 0.0" [ INFO] [1636927786.641629374]: Setting RTAB-Map parameter "Kp/SubPixEps"="0.02" [ INFO] [1636927786.642592422]: Setting RTAB-Map parameter "Kp/SubPixIterations"="0" [ INFO] [1636927786.643516585]: Setting RTAB-Map parameter "Kp/SubPixWinSize"="3" [ INFO] [1636927786.644251361]: Setting RTAB-Map parameter "Kp/TfIdfLikelihoodUsed"="true" [ INFO] [1636927786.645176274]: Setting RTAB-Map parameter "Marker/CornerRefinementMethod"="0" [ INFO] [1636927786.647501038]: Setting RTAB-Map parameter "Marker/Dictionary"="16" [ INFO] [1636927786.648962904]: Setting RTAB-Map parameter "Marker/Length"="0.055" [ INFO] [1636927786.649357477]: Setting RTAB-Map parameter "Marker/MaxDepthError"="0.01" [ INFO] [1636927786.650440059]: Setting RTAB-Map parameter "Marker/MaxRange"="0.0" [ INFO] [1636927786.651185958]: Setting RTAB-Map parameter "Marker/MinRange"="0.0" [ INFO] [1636927786.652019429]: Setting RTAB-Map parameter "Marker/VarianceAngular"="0.01" [ INFO] [1636927786.652547221]: Setting RTAB-Map parameter "Marker/VarianceLinear"="0.001" [ INFO] [1636927786.653367562]: Setting RTAB-Map parameter "Mem/BadSignaturesIgnored"="false" [ INFO] [1636927786.653882550]: Setting RTAB-Map parameter "Mem/BinDataKept"="true" [ INFO] [1636927786.654694294]: Setting RTAB-Map parameter "Mem/CompressionParallelized"="true" [ INFO] [1636927786.655215250]: Setting RTAB-Map parameter "Mem/CovOffDiagIgnored"="true" [ INFO] [1636927786.656041302]: Setting RTAB-Map parameter "Mem/DepthAsMask"="true" [ INFO] [1636927786.656586685]: Setting RTAB-Map parameter "Mem/GenerateIds"="true" [ INFO] [1636927786.657473014]: Setting RTAB-Map parameter "Mem/ImageCompressionFormat"=".jpg" [ INFO] [1636927786.657911296]: Setting RTAB-Map parameter "Mem/ImageKept"="false" [ INFO] [1636927786.658791353]: Setting RTAB-Map parameter "Mem/ImagePostDecimation"="1" [ INFO] [1636927786.659199941]: Setting RTAB-Map parameter "Mem/ImagePreDecimation"="1" [ INFO] [1636927786.659642205]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="true" [ INFO] [1636927786.660191436]: Setting RTAB-Map parameter "Mem/InitWMWithAllNodes"="false" [ INFO] [1636927786.660613666]: Setting RTAB-Map parameter "Mem/IntermediateNodeDataKept"="false" [ INFO] [1636927786.661178386]: Setting RTAB-Map parameter "Mem/LaserScanDownsampleStepSize"="1" [ INFO] [1636927786.661610679]: Setting RTAB-Map parameter "Mem/LaserScanNormalK"="0" [ INFO] [1636927786.662177401]: Setting RTAB-Map parameter "Mem/LaserScanNormalRadius"="0.0" [ INFO] [1636927786.662845758]: Setting RTAB-Map parameter "Mem/LaserScanVoxelSize"="0.0" [ INFO] [1636927786.663312649]: Setting RTAB-Map parameter "Mem/LocalizationDataSaved"="false" [ INFO] [1636927786.663929824]: Setting RTAB-Map parameter "Mem/MapLabelsAdded"="true" [ INFO] [1636927786.664350988]: Setting RTAB-Map parameter "Mem/NotLinkedNodesKept"="true" [ INFO] [1636927786.664943009]: Setting RTAB-Map parameter "Mem/RawDescriptorsKept"="true" [ INFO] [1636927786.665660547]: Setting RTAB-Map parameter "Mem/RecentWmRatio"="0.2" [ INFO] [1636927786.666093284]: Setting RTAB-Map parameter "Mem/ReduceGraph"="false" [ INFO] [1636927786.666691406]: Setting RTAB-Map parameter "Mem/RehearsalIdUpdatedToNewOne"="false" [ INFO] [1636927786.667544408]: Setting RTAB-Map parameter "Mem/RehearsalSimilarity"="0.6" [ INFO] [1636927786.668026059]: Setting RTAB-Map parameter "Mem/RehearsalWeightIgnoredWhileMoving"="false" [ INFO] [1636927786.668612778]: Setting RTAB-Map parameter "Mem/STMSize"="10" [ INFO] [1636927786.669094382]: Setting RTAB-Map parameter "Mem/SaveDepth16Format"="false" [ INFO] [1636927786.669666413]: Setting RTAB-Map parameter "Mem/StereoFromMotion"="false" [ INFO] [1636927786.670084957]: Setting RTAB-Map parameter "Mem/TransferSortingByWeightId"="false" [ INFO] [1636927786.670664068]: Setting RTAB-Map parameter "Mem/UseOdomFeatures"="true" [ INFO] [1636927786.671099471]: Setting RTAB-Map parameter "Mem/UseOdomGravity"="false" [ INFO] [1636927786.671705865]: Setting RTAB-Map parameter "ORB/EdgeThreshold"="19" [ INFO] [1636927786.672186106]: Setting RTAB-Map parameter "ORB/FirstLevel"="0" [ INFO] [1636927786.672796553]: Setting RTAB-Map parameter "ORB/Gpu"="false" [ INFO] [1636927786.673282534]: Setting RTAB-Map parameter "ORB/NLevels"="3" [ INFO] [1636927786.673981092]: Setting RTAB-Map parameter "ORB/PatchSize"="31" [ INFO] [1636927786.674579998]: Setting RTAB-Map parameter "ORB/ScaleFactor"="2" [ INFO] [1636927786.675059718]: Setting RTAB-Map parameter "ORB/ScoreType"="0" [ INFO] [1636927786.675659553]: Setting RTAB-Map parameter "ORB/WTA_K"="2" [ INFO] [1636927786.676092555]: Setting RTAB-Map parameter "Optimizer/Epsilon"="0.00001" [ INFO] [1636927786.676734367]: Setting RTAB-Map parameter "Optimizer/GravitySigma"="0.3" [ INFO] [1636927786.677543214]: Setting RTAB-Map parameter "Optimizer/Iterations"="20" [ INFO] [1636927786.678025921]: Setting RTAB-Map parameter "Optimizer/LandmarksIgnored"="false" [ INFO] [1636927786.678633417]: Setting RTAB-Map parameter "Optimizer/PriorsIgnored"="true" [ INFO] [1636927786.679116979]: Setting RTAB-Map parameter "Optimizer/Robust"="false" [ INFO] [1636927786.680089142]: Setting RTAB-Map parameter "Optimizer/Strategy"="2" [ INFO] [1636927786.680624828]: Setting RTAB-Map parameter "Optimizer/VarianceIgnored"="false" [ INFO] [1636927786.681514306]: Setting RTAB-Map parameter "PyDetector/Cuda"="true" [ INFO] [1636927786.681987754]: Setting RTAB-Map parameter "PyDetector/Path"="" [ INFO] [1636927786.682870867]: Setting RTAB-Map parameter "PyMatcher/Cuda"="true" [ INFO] [1636927786.683364379]: Setting RTAB-Map parameter "PyMatcher/Iterations"="20" [ INFO] [1636927786.684234628]: Setting RTAB-Map parameter "PyMatcher/Model"="indoor" [ INFO] [1636927786.684709594]: Setting RTAB-Map parameter "PyMatcher/Path"="" [ INFO] [1636927786.685557819]: Setting RTAB-Map parameter "PyMatcher/Threshold"="0.2" [ INFO] [1636927786.686029731]: Setting RTAB-Map parameter "RGBD/AngularSpeedUpdate"="0.0" [ INFO] [1636927786.688251169]: Setting RTAB-Map parameter "RGBD/AngularUpdate"="0" [ INFO] [1636927786.688711517]: Setting RTAB-Map parameter "RGBD/CreateOccupancyGrid"="true" [ INFO] [1636927786.689569969]: Setting RTAB-Map parameter "RGBD/Enabled"="true" [ INFO] [1636927786.690035690]: Setting RTAB-Map parameter "RGBD/GoalReachedRadius"="0.5" [ INFO] [1636927786.690895802]: Setting RTAB-Map parameter "RGBD/GoalsSavedInUserData"="false" [ INFO] [1636927786.691402251]: Setting RTAB-Map parameter "RGBD/LinearSpeedUpdate"="0.0" [ INFO] [1636927786.693187160]: Setting RTAB-Map parameter "RGBD/LinearUpdate"="0.01" [ INFO] [1636927786.694055708]: Setting RTAB-Map parameter "RGBD/LocalBundleOnLoopClosure"="false" [ INFO] [1636927786.694554436]: Setting RTAB-Map parameter "RGBD/LocalImmunizationRatio"="0.25" [ INFO] [1636927786.696366023]: Setting RTAB-Map parameter "RGBD/LocalRadius"="5" [ INFO] [1636927786.697183694]: Setting RTAB-Map parameter "RGBD/LoopClosureIdentityGuess"="false" [ INFO] [1636927786.697755370]: Setting RTAB-Map parameter "RGBD/LoopClosureReextractFeatures"="true" [ INFO] [1636927786.698398720]: Setting RTAB-Map parameter "RGBD/LoopCovLimited"="false" [ INFO] [1636927786.699510737]: Setting RTAB-Map parameter "RGBD/MarkerDetection"="true" [ INFO] [1636927786.700004556]: Setting RTAB-Map parameter "RGBD/MaxLocalRetrieved"="2" [ INFO] [1636927786.700641778]: Setting RTAB-Map parameter "RGBD/MaxLoopClosureDistance"="0.0" [ INFO] [1636927786.701222706]: Setting RTAB-Map parameter "RGBD/MaxOdomCacheSize"="0" [ INFO] [1636927786.701944257]: Setting RTAB-Map parameter "RGBD/NeighborLinkRefining"="false" [ INFO] [1636927786.702743876]: Setting RTAB-Map parameter "RGBD/NewMapOdomChangeDistance"="0" [ INFO] [1636927786.703234147]: Setting RTAB-Map parameter "RGBD/OptimizeFromGraphEnd"="false" [ INFO] [1636927786.703847236]: Setting RTAB-Map parameter "RGBD/OptimizeMaxError"="3" [ INFO] [1636927786.704322536]: Setting RTAB-Map parameter "RGBD/PlanAngularVelocity"="0" [ INFO] [1636927786.704964855]: Setting RTAB-Map parameter "RGBD/PlanLinearVelocity"="0" [ INFO] [1636927786.705675390]: Setting RTAB-Map parameter "RGBD/PlanStuckIterations"="0" [ INFO] [1636927786.707086486]: Setting RTAB-Map parameter "RGBD/ProximityAngle"="25" [ INFO] [1636927786.707713816]: Setting RTAB-Map parameter "RGBD/ProximityBySpace"="true" [ INFO] [1636927786.708168889]: Setting RTAB-Map parameter "RGBD/ProximityByTime"="false" [ INFO] [1636927786.708779967]: Setting RTAB-Map parameter "RGBD/ProximityGlobalScanMap"="false" [ INFO] [1636927786.709231368]: Setting RTAB-Map parameter "RGBD/ProximityMaxGraphDepth"="50" [ INFO] [1636927786.709872612]: Setting RTAB-Map parameter "RGBD/ProximityMaxPaths"="3" [ INFO] [1636927786.710705459]: Setting RTAB-Map parameter "RGBD/ProximityOdomGuess"="false" [ INFO] [1636927786.711450485]: Setting RTAB-Map parameter "RGBD/ProximityPathFilteringRadius"="1" [ INFO] [1636927786.712414191]: Setting RTAB-Map parameter "RGBD/ProximityPathMaxNeighbors"="0" [ INFO] [1636927786.713277572]: Setting RTAB-Map parameter "RGBD/ProximityPathRawPosesUsed"="true" [ INFO] [1636927786.714288409]: Setting RTAB-Map parameter "RGBD/ScanMatchingIdsSavedInLinks"="true" [ INFO] [1636927786.714989405]: Setting RTAB-Map parameter "RGBD/StartAtOrigin"="false" [ INFO] [1636927786.716416601]: Setting RTAB-Map parameter "Reg/Force3DoF"="false" [ INFO] [1636927786.717239113]: Setting RTAB-Map parameter "Reg/RepeatOnce"="true" [ INFO] [1636927786.718254880]: Setting RTAB-Map parameter "Reg/Strategy"="0" [ INFO] [1636927786.718976026]: Setting RTAB-Map parameter "Rtabmap/ComputeRMSE"="true" [ INFO] [1636927786.719787640]: Setting RTAB-Map parameter "Rtabmap/CreateIntermediateNodes"="false" [ INFO] [1636927786.721635140]: Setting RTAB-Map parameter "Rtabmap/DetectionRate"="0" [ INFO] [1636927786.722104824]: Setting RTAB-Map parameter "Rtabmap/ImageBufferSize"="1" [ INFO] [1636927786.722695197]: Setting RTAB-Map parameter "Rtabmap/ImagesAlreadyRectified"="true" [ INFO] [1636927786.723137814]: Setting RTAB-Map parameter "Rtabmap/LoopGPS"="true" [ INFO] [1636927786.723767579]: Setting RTAB-Map parameter "Rtabmap/LoopRatio"="0" [ INFO] [1636927786.724213099]: Setting RTAB-Map parameter "Rtabmap/LoopThr"="0.11" [ INFO] [1636927786.724830712]: Setting RTAB-Map parameter "Rtabmap/MaxRetrieved"="2" [ INFO] [1636927786.725273634]: Setting RTAB-Map parameter "Rtabmap/MemoryThr"="0" [ INFO] [1636927786.725883084]: Setting RTAB-Map parameter "Rtabmap/PublishLastSignature"="true" [ INFO] [1636927786.726357733]: Setting RTAB-Map parameter "Rtabmap/PublishLikelihood"="true" [ INFO] [1636927786.726977771]: Setting RTAB-Map parameter "Rtabmap/PublishPdf"="true" [ INFO] [1636927786.727717605]: Setting RTAB-Map parameter "Rtabmap/PublishRAMUsage"="false" [ INFO] [1636927786.728162287]: Setting RTAB-Map parameter "Rtabmap/PublishStats"="true" [ INFO] [1636927786.728903169]: Setting RTAB-Map parameter "Rtabmap/RectifyOnlyFeatures"="false" [ INFO] [1636927786.729441809]: Setting RTAB-Map parameter "Rtabmap/SaveWMState"="false" [ INFO] [1636927786.730100922]: Setting RTAB-Map parameter "Rtabmap/StartNewMapOnGoodSignature"="false" [ INFO] [1636927786.730508120]: Setting RTAB-Map parameter "Rtabmap/StartNewMapOnLoopClosure"="false" [ INFO] [1636927786.730896445]: Setting RTAB-Map parameter "Rtabmap/StatisticLogged"="false" [ INFO] [1636927786.731314318]: Setting RTAB-Map parameter "Rtabmap/StatisticLoggedHeaders"="true" [ INFO] [1636927786.731746320]: Setting RTAB-Map parameter "Rtabmap/StatisticLogsBufferedInRAM"="true" [ INFO] [1636927786.732146188]: Setting RTAB-Map parameter "Rtabmap/TimeThr"="0" [ INFO] [1636927786.732547449]: Setting RTAB-Map parameter "Rtabmap/WorkingDirectory"="/home/szx/.ros" [ INFO] [1636927786.732943385]: Setting RTAB-Map parameter "SIFT/ContrastThreshold"="0.04" [ INFO] [1636927786.733333784]: Setting RTAB-Map parameter "SIFT/EdgeThreshold"="10" [ INFO] [1636927786.733721778]: Setting RTAB-Map parameter "SIFT/NFeatures"="0" [ INFO] [1636927786.734107837]: Setting RTAB-Map parameter "SIFT/NOctaveLayers"="3" [ INFO] [1636927786.734496352]: Setting RTAB-Map parameter "SIFT/RootSIFT"="false" [ INFO] [1636927786.734882628]: Setting RTAB-Map parameter "SIFT/Sigma"="1.6" [ INFO] [1636927786.735291230]: Setting RTAB-Map parameter "SURF/Extended"="false" [ INFO] [1636927786.735686146]: Setting RTAB-Map parameter "SURF/GpuKeypointsRatio"="0.01" [ INFO] [1636927786.736072579]: Setting RTAB-Map parameter "SURF/GpuVersion"="false" [ INFO] [1636927786.736461418]: Setting RTAB-Map parameter "SURF/HessianThreshold"="500" [ INFO] [1636927786.736847337]: Setting RTAB-Map parameter "SURF/OctaveLayers"="2" [ INFO] [1636927786.737235130]: Setting RTAB-Map parameter "SURF/Octaves"="4" [ INFO] [1636927786.737627878]: Setting RTAB-Map parameter "SURF/Upright"="false" [ INFO] [1636927786.738012524]: Setting RTAB-Map parameter "Stereo/DenseStrategy"="0" [ INFO] [1636927786.738396850]: Setting RTAB-Map parameter "Stereo/Eps"="0.01" [ INFO] [1636927786.738778351]: Setting RTAB-Map parameter "Stereo/Iterations"="30" [ INFO] [1636927786.739131277]: Setting RTAB-Map parameter "Stereo/MaxDisparity"="128.0" [ INFO] [1636927786.739465596]: Setting RTAB-Map parameter "Stereo/MaxLevel"="5" [ INFO] [1636927786.739793223]: Setting RTAB-Map parameter "Stereo/MinDisparity"="0.5" [ INFO] [1636927786.740118809]: Setting RTAB-Map parameter "Stereo/OpticalFlow"="true" [ INFO] [1636927786.740442951]: Setting RTAB-Map parameter "Stereo/SSD"="true" [ INFO] [1636927786.740761415]: Setting RTAB-Map parameter "Stereo/WinHeight"="3" [ INFO] [1636927786.741076955]: Setting RTAB-Map parameter "Stereo/WinWidth"="15" [ INFO] [1636927786.741390339]: Setting RTAB-Map parameter "StereoBM/BlockSize"="15" [ INFO] [1636927786.741705451]: Setting RTAB-Map parameter "StereoBM/Disp12MaxDiff"="-1" [ INFO] [1636927786.742018839]: Setting RTAB-Map parameter "StereoBM/MinDisparity"="0" [ INFO] [1636927786.742332472]: Setting RTAB-Map parameter "StereoBM/NumDisparities"="128" [ INFO] [1636927786.742644399]: Setting RTAB-Map parameter "StereoBM/PreFilterCap"="31" [ INFO] [1636927786.742960590]: Setting RTAB-Map parameter "StereoBM/PreFilterSize"="9" [ INFO] [1636927786.743288821]: Setting RTAB-Map parameter "StereoBM/SpeckleRange"="4" [ INFO] [1636927786.743610575]: Setting RTAB-Map parameter "StereoBM/SpeckleWindowSize"="100" [ INFO] [1636927786.743924811]: Setting RTAB-Map parameter "StereoBM/TextureThreshold"="10" [ INFO] [1636927786.744237854]: Setting RTAB-Map parameter "StereoBM/UniquenessRatio"="15" [ INFO] [1636927786.744548859]: Setting RTAB-Map parameter "StereoSGBM/BlockSize"="15" [ INFO] [1636927786.744861524]: Setting RTAB-Map parameter "StereoSGBM/Disp12MaxDiff"="1" [ INFO] [1636927786.745175334]: Setting RTAB-Map parameter "StereoSGBM/MinDisparity"="0" [ INFO] [1636927786.745486650]: Setting RTAB-Map parameter "StereoSGBM/Mode"="2" [ INFO] [1636927786.745800353]: Setting RTAB-Map parameter "StereoSGBM/NumDisparities"="128" [ INFO] [1636927786.746132958]: Setting RTAB-Map parameter "StereoSGBM/P1"="2" [ INFO] [1636927786.746447404]: Setting RTAB-Map parameter "StereoSGBM/P2"="5" [ INFO] [1636927786.746766792]: Setting RTAB-Map parameter "StereoSGBM/PreFilterCap"="31" [ INFO] [1636927786.747080333]: Setting RTAB-Map parameter "StereoSGBM/SpeckleRange"="4" [ INFO] [1636927786.747481649]: Setting RTAB-Map parameter "StereoSGBM/SpeckleWindowSize"="100" [ INFO] [1636927786.747802726]: Setting RTAB-Map parameter "StereoSGBM/UniquenessRatio"="20" [ INFO] [1636927786.748120383]: Setting RTAB-Map parameter "SuperPoint/Cuda"="true" [ INFO] [1636927786.748435189]: Setting RTAB-Map parameter "SuperPoint/ModelPath"="/home/szx/catkin_ws/src/rtabmap_ros/cfg/superpoint.pt" [ INFO] [1636927786.748748048]: Setting RTAB-Map parameter "SuperPoint/NMS"="true" [ INFO] [1636927786.749062847]: Setting RTAB-Map parameter "SuperPoint/NMSRadius"="4" [ INFO] [1636927786.749375020]: Setting RTAB-Map parameter "SuperPoint/Threshold"="0.010" [ INFO] [1636927786.749687618]: Setting RTAB-Map parameter "VhEp/Enabled"="false" [ INFO] [1636927786.749997347]: Setting RTAB-Map parameter "VhEp/MatchCountMin"="8" [ INFO] [1636927786.750310119]: Setting RTAB-Map parameter "VhEp/RansacParam1"="3" [ INFO] [1636927786.750627684]: Setting RTAB-Map parameter "VhEp/RansacParam2"="0.99" [ INFO] [1636927786.751695092]: Setting RTAB-Map parameter "Vis/BundleAdjustment"="1" [ INFO] [1636927786.752090462]: Setting RTAB-Map parameter "Vis/CorFlowEps"="0.01" [ INFO] [1636927786.752466355]: Setting RTAB-Map parameter "Vis/CorFlowIterations"="30" [ INFO] [1636927786.752822177]: Setting RTAB-Map parameter "Vis/CorFlowMaxLevel"="3" [ INFO] [1636927786.753179820]: Setting RTAB-Map parameter "Vis/CorFlowWinSize"="16" [ INFO] [1636927786.753535502]: Setting RTAB-Map parameter "Vis/CorGuessMatchToProjection"="false" [ INFO] [1636927786.754476095]: Setting RTAB-Map parameter "Vis/CorGuessWinSize"="20" [ INFO] [1636927786.754835272]: Setting RTAB-Map parameter "Vis/CorNNDR"="0.8" [ INFO] [1636927786.755792496]: Setting RTAB-Map parameter "Vis/CorNNType"="1" [ INFO] [1636927786.756741743]: Setting RTAB-Map parameter "Vis/CorType"="0" [ INFO] [1636927786.757143779]: Setting RTAB-Map parameter "Vis/DepthAsMask"="true" [ INFO] [1636927786.757511021]: Setting RTAB-Map parameter "Vis/EpipolarGeometryVar"="0.1" [ INFO] [1636927786.758475275]: Setting RTAB-Map parameter "Vis/EstimationType"="1" [ INFO] [1636927786.759437812]: Setting RTAB-Map parameter "Vis/FeatureType"="11" [ INFO] [1636927786.760795449]: Setting RTAB-Map parameter "Vis/ForwardEstOnly"="false" [ INFO] [1636927786.762552929]: Setting RTAB-Map parameter "Vis/GridCols"="20" [ INFO] [1636927786.764304682]: Setting RTAB-Map parameter "Vis/GridRows"="20" [ INFO] [1636927786.766077742]: Setting RTAB-Map parameter "Vis/InlierDistance"="0.01" [ INFO] [1636927786.766708843]: Setting RTAB-Map parameter "Vis/Iterations"="300" [ INFO] [1636927786.768100703]: Setting RTAB-Map parameter "Vis/MaxDepth"="0.8" [ INFO] [1636927786.769069422]: Setting RTAB-Map parameter "Vis/MaxFeatures"="1000" [ INFO] [1636927786.769435962]: Setting RTAB-Map parameter "Vis/MeanInliersDistance"="0.0" [ INFO] [1636927786.770361143]: Setting RTAB-Map parameter "Vis/MinDepth"="0.15" [ INFO] [1636927786.771319573]: Setting RTAB-Map parameter "Vis/MinInliers"="20" [ INFO] [1636927786.771688950]: Setting RTAB-Map parameter "Vis/MinInliersDistribution"="0.0" [ INFO] [1636927786.772038186]: Setting RTAB-Map parameter "Vis/PnPFlags"="0" [ INFO] [1636927786.772381631]: Setting RTAB-Map parameter "Vis/PnPRefineIterations"="0" [ INFO] [1636927786.772718342]: Setting RTAB-Map parameter "Vis/PnPReprojError"="2" [ INFO] [1636927786.773619324]: Setting RTAB-Map parameter "Vis/RefineIterations"="20" [ INFO] [1636927786.774070341]: Setting RTAB-Map parameter "Vis/RoiRatios"="0.0 0.0 0.0 0.0" [ INFO] [1636927786.774430595]: Setting RTAB-Map parameter "Vis/SubPixEps"="0.02" [ INFO] [1636927786.774776302]: Setting RTAB-Map parameter "Vis/SubPixIterations"="0" [ INFO] [1636927786.775112574]: Setting RTAB-Map parameter "Vis/SubPixWinSize"="3" [ INFO] [1636927786.775467930]: Setting RTAB-Map parameter "g2o/Baseline"="0.075" [ INFO] [1636927786.776455178]: Setting RTAB-Map parameter "g2o/Optimizer"="1" [ INFO] [1636927786.776883308]: Setting RTAB-Map parameter "g2o/PixelVariance"="1.0" [ INFO] [1636927786.777307019]: Setting RTAB-Map parameter "g2o/RobustKernelDelta"="8" [ INFO] [1636927786.777651127]: Setting RTAB-Map parameter "g2o/Solver"="0" [ INFO] [1636927786.961930460]: RTAB-Map detection rate = 0.000000 Hz [ INFO] [1636927786.963003217]: rtabmap: Deleted database "/home/szx/.ros/rtabmap.db" (--delete_db_on_start or -d are set). [ INFO] [1636927786.963100851]: rtabmap: Using database from "/home/szx/.ros/rtabmap.db" (0 MB). libpng warning: iCCP: known incorrect sRGB profile libpng warning: iCCP: known incorrect sRGB profile libpng warning: iCCP: known incorrect sRGB profile [ INFO] [1636927787.481988206]: rtabmapviz: Reading parameters from the ROS server... [ INFO] [1636927787.483014507]: rtabmapviz: Cannot get rtabmap's parameters, waiting max 5 seconds in case the node has just been launched. [ INFO] [1636927788.856846096]: RGBDOdometry: approx_sync = false [ INFO] [1636927788.856911205]: RGBDOdometry: queue_size = 10 [ INFO] [1636927788.856944951]: RGBDOdometry: subscribe_rgbd = true [ INFO] [1636927788.856966998]: RGBDOdometry: rgbd_cameras = 1 [ INFO] [1636927788.856988087]: RGBDOdometry: keep_color = false [ INFO] [1636927788.864507764]: /rtabmap/rgbd_odometry subscribed to: /rgbd_image [ INFO] [1636927791.895814629]: rtabmap: Database version = "0.20.15". [ INFO] [1636927791.895909214]: rtabmap: SLAM mode (Mem/IncrementalMemory=true) [ INFO] [1636927791.965518265]: /rtabmap/rtabmap: subscribe_depth = false [ INFO] [1636927791.965591417]: /rtabmap/rtabmap: subscribe_rgb = false [ INFO] [1636927791.965625190]: /rtabmap/rtabmap: subscribe_stereo = false [ INFO] [1636927791.965670703]: /rtabmap/rtabmap: subscribe_rgbd = true (rgbd_cameras=1) [ INFO] [1636927791.965708418]: /rtabmap/rtabmap: subscribe_odom_info = true [ INFO] [1636927791.965753024]: /rtabmap/rtabmap: subscribe_user_data = false [ INFO] [1636927791.965788364]: /rtabmap/rtabmap: subscribe_scan = false [ INFO] [1636927791.965823425]: /rtabmap/rtabmap: subscribe_scan_cloud = false [ INFO] [1636927791.965861824]: /rtabmap/rtabmap: subscribe_scan_descriptor = false [ INFO] [1636927791.965898043]: /rtabmap/rtabmap: queue_size = 10 [ INFO] [1636927791.965935317]: /rtabmap/rtabmap: approx_sync = false [ INFO] [1636927791.966033823]: Setup rgbd callback [ INFO] [1636927791.982692376]: /rtabmap/rtabmap subscribed to (exact sync): /rgbd_image \ /front_odom_info [ INFO] [1636927792.268626430]: rtabmapviz: rtabmap's parameters seem now there! continuing... [ INFO] [1636927792.403937232]: rtabmap 0.20.15 started... [ INFO] [1636927792.509324925]: rtabmapviz: Parameters read = 352 [ INFO] [1636927792.509375106]: rtabmapviz: Parameters successfully read. [ INFO] [1636927796.579595519]: /rtabmap/rtabmapviz: subscribe_depth = false [ INFO] [1636927796.579650911]: /rtabmap/rtabmapviz: subscribe_rgb = false [ INFO] [1636927796.579673724]: /rtabmap/rtabmapviz: subscribe_stereo = false [ INFO] [1636927796.579726165]: /rtabmap/rtabmapviz: subscribe_rgbd = true (rgbd_cameras=1) [ INFO] [1636927796.579749736]: /rtabmap/rtabmapviz: subscribe_odom_info = true [ INFO] [1636927796.579768008]: /rtabmap/rtabmapviz: subscribe_user_data = false [ INFO] [1636927796.579806435]: /rtabmap/rtabmapviz: subscribe_scan = false [ INFO] [1636927796.579832438]: /rtabmap/rtabmapviz: subscribe_scan_cloud = false [ INFO] [1636927796.579859839]: /rtabmap/rtabmapviz: subscribe_scan_descriptor = false [ INFO] [1636927796.579882742]: /rtabmap/rtabmapviz: queue_size = 10 [ INFO] [1636927796.579908342]: /rtabmap/rtabmapviz: approx_sync = true [ INFO] [1636927796.579974478]: Setup rgbd callback [ INFO] [1636927796.625146129]: /rtabmap/rtabmapviz subscribed to (approx sync): /front_odom \ /rgbd_image \ /front_odom_info [ INFO] [1636927796.625362521]: rtabmapviz started. [FATAL] (2021-11-14 16:15:03.416) Features2d.cpp:2092::generateKeypointsImpl() Condition (roi.x==0 && roi.y ==0) not met! [Not supporting ROI] terminate called after throwing an instance of 'UException' what(): [FATAL] (2021-11-14 16:15:03.416) Features2d.cpp:2092::generateKeypointsImpl() Condition (roi.x==0 && roi.y ==0) not met! [Not supporting ROI] [rtabmap/rgbd_odometry-2] process has died [pid 25811, exit code -6, cmd /home/szx/catkin_ws/devel/lib/rtabmap_ros/rgbd_odometry rgb/image:=/camera5/color/image_raw depth/image:=/camera5/aligned_depth_to_color/image_raw rgb/camera_info:=/camera5/color/camera_info rgbd_image:=/rgbd_image odom:=/front_odom odom_info:=/front_odom_info __name:=rgbd_odometry __log:=/home/szx/.ros/log/7368028a-4578-11ec-9aaa-989096b12255/rtabmap-rgbd_odometry-2.log]. log file: /home/szx/.ros/log/7368028a-4578-11ec-9aaa-989096b12255/rtabmap-rgbd_odometry-2*.log [ WARN] [1636928597.774591240, 1636862410.129271139]: /rtabmap/rtabmapviz: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10). /rtabmap/rtabmapviz subscribed to (approx sync): /front_odom \ /rgbd_image \ /front_odom_info [ WARN] [1636928597.774591300, 1636862410.129271139]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called. /rtabmap/rtabmap subscribed to (exact sync): /rgbd_image \ /front_odom_info ^C[rtabmap/rtabmapviz-4] killing on exit [rtabmap/rtabmap-3] killing on exit [rgbd_sync-1] killing on exit [ INFO] [1636928689.904613742, 1636862411.050575863]: rtabmapviz: ctrl-c catched! Exiting Qt app... rtabmap: Saving database/long-term memory... (located at /home/szx/.ros/rtabmap.db) rtabmap: Saving database/long-term memory...done! (located at /home/szx/.ros/rtabmap.db, 0 MB) shutting down processing monitor... ... shutting down processing monitor complete done `

matlabbe commented 2 years ago 
[ INFO] [1636927786.399028386]: Setting odometry parameter "Vis/GridCols"="20"
[ INFO] [1636927786.400009847]: Setting odometry parameter "Vis/GridRows"="20"

Set those parameters to 1. I will update the error message to be easier to detect this problem.

szx0112 commented 2 years ago

It works now. Thanks @matlabbe .

I see the Superpoint detected features are color coded as green/blue/red/yellow. Could you point out what are the meanings of these colors?

matlabbe commented 2 years ago

You can mouse over the label of the loop closure detection to see the color code: Screenshot from 2021-11-15 09-31-24

szx0112 commented 2 years ago

Got it. Thanks for the info.

szx0112 commented 2 years ago

@matlabbe This might also related to SuperPoint setup. I got my rtabmap node crashed at the middle of the mapping as below. I don't see this error when ORB or other feature is used for loop closure. I use GTFF for VO. Log is also attached .

[ INFO] [1637009483.962312404, 1636862612.929951076]: rtabmap (2230): Rate=0.00s, Limit=0.000s, Conversion=0.0046s, RTAB-Map=0.0607s, Maps update=0.0032s pub=0.0000s (local map=826, WM=826)
[ INFO] [1637009485.772973694, 1636862612.944458826]: Odom: quality=155, std dev=0.003095m|0.066028rad, update time=0.071611s
[ INFO] [1637009485.974480762, 1636862612.946072746]: rtabmap (2231): Rate=0.00s, Limit=0.000s, Conversion=0.0042s, RTAB-Map=0.1929s, Maps update=0.0027s pub=0.0000s (local map=827, WM=827)
[ INFO] [1637009488.690867839, 1636862612.967796757]: Odom: quality=146, std dev=0.003792m|0.074679rad, update time=0.070582s
**terminate called after throwing an instance of 'std::out_of_range'
  what():  map::at**
[rtabmap/rtabmap-3] process has died [pid 7411, exit code -6, cmd /home/szx/catkin_ws/devel/lib/rtabmap_ros/rtabmap --delete_db_on_start rgbd_image:=/rgbd_image odom_info:=/front_odom_info odom:=/front_odom __name:=rtabmap __log:=/home/szx/.ros/log/7368028a-4578-11ec-9aaa-989096b12255/rtabmap-rtabmap-3.log].
log file: /home/szx/.ros/log/7368028a-4578-11ec-9aaa-989096b12255/rtabmap-rtabmap-3*.log
[ INFO] [1637009493.025963220, 1636862613.002436800]: Odom: quality=142, std dev=0.002640m|0.071352rad, update time=0.067335s
[ INFO] [1637009493.099727246, 1636862613.003084130]: Odom: quality=129, std dev=0.002886m|0.073978rad, update time=0.072117s
[ INFO] [1637009494.247543852, 1636862613.012218656]: Odom: quality=166, std dev=0.002820m|0.070952rad, update time=0.061395s
[ INFO] [1637009497.522379295, 1636862613.038414389]: Odom: quality=231, std dev=0.001967m|0.062797rad, update time=0.060832s

roslaunch-szx19-7369.log

matlabbe commented 2 years ago

can you call rosservice call /rtabmap/rtabmap/log_debug to show more debug log?

szx0112 commented 2 years ago

Please see a snapshot of the command line returns in debug mode (start from the last Odom message before rtabmap node crash). 

[ INFO] [1637022259.598089279, 1636862607.811334906]: Odom: quality=112, std dev=0.004583m|0.082900rad, update time=0.075661s
[DEBUG] (2021-11-15 18:24:19.606) Features2d.cpp:751::generateKeypoints() Keypoints extraction time = 0.016018 s, keypoints extracted = 1000 (grid=1x1, mask empty=0)
[DEBUG] (2021-11-15 18:24:19.606) RegistrationVis.cpp:599::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:19.608) util2d.cpp:1162::computeRoi() roi ratios = 0.000000, 0.000000, 0.000000, 0.000000
[DEBUG] (2021-11-15 18:24:19.608) util2d.cpp:1163::computeRoi() roi = 0, 0, 640, 480
[DEBUG] (2021-11-15 18:24:19.608) util2d.cpp:1190::computeRoi() roi = 0, 0, 640, 480
[DEBUG] (2021-11-15 18:24:19.618) Features2d.cpp:751::generateKeypoints() Keypoints extraction time = 0.009736 s, keypoints extracted = 1000 (grid=1x1, mask empty=0)
[DEBUG] (2021-11-15 18:24:19.618) RegistrationVis.cpp:649::computeTransformationImpl() kptsFrom=1000 kptsFromSource=0
[DEBUG] (2021-11-15 18:24:19.618) RegistrationVis.cpp:650::computeTransformationImpl() kptsTo=1000 kptsToSource=0
[DEBUG] (2021-11-15 18:24:19.618) RegistrationVis.cpp:672::computeTransformationImpl() cleared orignalWordsFromIds
[DEBUG] (2021-11-15 18:24:19.622) Features2d.cpp:783::generateDescriptors() Descriptors extracted = 846, remaining kpts=846
[DEBUG] (2021-11-15 18:24:19.625) Features2d.cpp:783::generateDescriptors() Descriptors extracted = 797, remaining kpts=797
[DEBUG] (2021-11-15 18:24:19.625) RegistrationVis.cpp:733::computeTransformationImpl() generated kptsFrom3D=846
[DEBUG] (2021-11-15 18:24:19.625) Features2d.cpp:164::filterKeypointsByDepth() 
[DEBUG] (2021-11-15 18:24:19.625) Features2d.cpp:193::filterKeypointsByDepth() Removed 0 invalid 3D points
[DEBUG] (2021-11-15 18:24:19.626) Features2d.cpp:164::filterKeypointsByDepth() 
[DEBUG] (2021-11-15 18:24:19.626) Features2d.cpp:193::filterKeypointsByDepth() Removed 0 invalid 3D points
[DEBUG] (2021-11-15 18:24:19.626) RegistrationVis.cpp:783::computeTransformationImpl() descriptorsFrom=846
[DEBUG] (2021-11-15 18:24:19.626) RegistrationVis.cpp:784::computeTransformationImpl() descriptorsTo=797
[DEBUG] (2021-11-15 18:24:19.626) RegistrationVis.cpp:785::computeTransformationImpl() orignalWordsFromIds=0
[DEBUG] (2021-11-15 18:24:19.626) RegistrationVis.cpp:1165::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:19.626) RegistrationVis.cpp:1265::computeTransformationImpl() VWDictionary knn matching
[DEBUG] (2021-11-15 18:24:19.626) VWDictionary.cpp:470::update() incremental=1
[DEBUG] (2021-11-15 18:24:19.626) FlannIndex.cpp:52::release() 
[DEBUG] (2021-11-15 18:24:19.626) FlannIndex.cpp:80::release() 
[DEBUG] (2021-11-15 18:24:19.626) VWDictionary.cpp:681::update() Dictionary updated! (size=0 added=0 removed=0)
[DEBUG] (2021-11-15 18:24:19.626) VWDictionary.cpp:689::update() 
[DEBUG] (2021-11-15 18:24:19.626) VWDictionary.cpp:763::addNewWords() id=1 descriptors=846
[DEBUG] (2021-11-15 18:24:19.627) VWDictionary.cpp:1059::addNewWords() naive search and add ref/words time = 0.001109 s
[DEBUG] (2021-11-15 18:24:19.627) VWDictionary.cpp:1061::addNewWords() 846 new words added...
[DEBUG] (2021-11-15 18:24:19.627) VWDictionary.cpp:1063::addNewWords() 0 duplicated words added (from current image = 0)...
[DEBUG] (2021-11-15 18:24:19.627) VWDictionary.cpp:1064::addNewWords() total time 0.001288s
[DEBUG] (2021-11-15 18:24:19.627) VWDictionary.cpp:470::update() incremental=1
[DEBUG] (2021-11-15 18:24:19.627) VWDictionary.cpp:491::update() Incremental FLANN: Removing 0 words...
[DEBUG] (2021-11-15 18:24:19.627) VWDictionary.cpp:500::update() Incremental FLANN: Removing 0 words... done!
[DEBUG] (2021-11-15 18:24:19.627) VWDictionary.cpp:504::update() Incremental FLANN: Inserting 846 words...
[DEBUG] (2021-11-15 18:24:19.627) VWDictionary.cpp:531::update() Building FLANN index...
[DEBUG] (2021-11-15 18:24:19.627) FlannIndex.cpp:208::buildKDTreeIndex() 
[DEBUG] (2021-11-15 18:24:19.627) FlannIndex.cpp:52::release() 
[DEBUG] (2021-11-15 18:24:19.627) FlannIndex.cpp:80::release() 
[DEBUG] (2021-11-15 18:24:19.627) FlannIndex.cpp:260::buildKDTreeIndex() 
[DEBUG] (2021-11-15 18:24:19.627) VWDictionary.cpp:549::update() Building FLANN index... done!
[DEBUG] (2021-11-15 18:24:19.627) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 1 -> 3
[DEBUG] (2021-11-15 18:24:19.627) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 3 -> 7
[DEBUG] (2021-11-15 18:24:19.628) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 7 -> 15
[DEBUG] (2021-11-15 18:24:19.628) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 15 -> 31
[DEBUG] (2021-11-15 18:24:19.628) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 31 -> 63
[DEBUG] (2021-11-15 18:24:19.629) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 63 -> 127
[DEBUG] (2021-11-15 18:24:19.630) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 127 -> 255
[DEBUG] (2021-11-15 18:24:19.633) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 255 -> 511
[DEBUG] (2021-11-15 18:24:19.639) VWDictionary.cpp:564::update() Incremental FLANN: Inserting 846 words... done!
[DEBUG] (2021-11-15 18:24:19.639) VWDictionary.cpp:681::update() Dictionary updated! (size=0 added=846 removed=0)
[DEBUG] (2021-11-15 18:24:19.639) VWDictionary.cpp:689::update() 
[DEBUG] (2021-11-15 18:24:19.639) VWDictionary.cpp:763::addNewWords() id=2 descriptors=797
[DEBUG] (2021-11-15 18:24:19.639) VWDictionary.cpp:857::addNewWords() newPts.total()=797 
[DEBUG] (2021-11-15 18:24:19.654) VWDictionary.cpp:924::addNewWords() Time to find nn = 0.014495 s
[DEBUG] (2021-11-15 18:24:19.655) VWDictionary.cpp:1059::addNewWords() naive search and add ref/words time = 0.001186 s
[DEBUG] (2021-11-15 18:24:19.655) VWDictionary.cpp:1061::addNewWords() 631 new words added...
[DEBUG] (2021-11-15 18:24:19.655) VWDictionary.cpp:1063::addNewWords() 166 duplicated words added (from current image = 0)...
[DEBUG] (2021-11-15 18:24:19.655) VWDictionary.cpp:1064::addNewWords() total time 0.015883s
[DEBUG] (2021-11-15 18:24:19.655) VWDictionary.cpp:694::clear() 
[DEBUG] (2021-11-15 18:24:19.655) FlannIndex.cpp:52::release() 
[DEBUG] (2021-11-15 18:24:19.656) FlannIndex.cpp:80::release() 
[DEBUG] (2021-11-15 18:24:19.656) VWDictionary.cpp:694::clear() 
[DEBUG] (2021-11-15 18:24:19.656) FlannIndex.cpp:52::release() 
[DEBUG] (2021-11-15 18:24:19.656) FlannIndex.cpp:80::release() 
[DEBUG] (2021-11-15 18:24:19.656) FlannIndex.cpp:52::release() 
[DEBUG] (2021-11-15 18:24:19.656) FlannIndex.cpp:80::release() 
[DEBUG] (2021-11-15 18:24:19.657) RegistrationVis.cpp:1481::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:19.657) RegistrationVis.cpp:1494::computeTransformationImpl() words from3D=846 to2D=739
[DEBUG] (2021-11-15 18:24:19.658) util3d_motion_estimation.cpp:99::estimateMotion3DTo2D() words3A=846 words2B=739 matches=108 words3B=739 guess=xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000 reprojError=2.000000 iterations=300
[DEBUG] (2021-11-15 18:24:19.684) RegistrationVis.cpp:1537::computeTransformationImpl() inliers: 18/108
[DEBUG] (2021-11-15 18:24:19.684) RegistrationVis.cpp:1481::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:19.684) RegistrationVis.cpp:1494::computeTransformationImpl() words from3D=739 to2D=846
[DEBUG] (2021-11-15 18:24:19.685) util3d_motion_estimation.cpp:99::estimateMotion3DTo2D() words3A=739 words2B=846 matches=108 words3B=846 guess=xyz=-0.002839,0.013064,-0.110083 rpy=0.045194,-0.001568,0.017060 reprojError=2.000000 iterations=300
[DEBUG] (2021-11-15 18:24:19.710) RegistrationVis.cpp:1537::computeTransformationImpl() inliers: 24/108
[DEBUG] (2021-11-15 18:24:19.710) RegistrationVis.cpp:1614::computeTransformationImpl() from->to=xyz=0.002788,-0.008124,0.110558 rpy=-0.045215,0.000796,-0.017113
[DEBUG] (2021-11-15 18:24:19.710) RegistrationVis.cpp:1615::computeTransformationImpl() to->from=xyz=-0.001407,0.009523,-0.110012 rpy=0.039935,-0.004219,0.015938
[DEBUG] (2021-11-15 18:24:19.710) RegistrationVis.cpp:1658::computeTransformationImpl() Refine with bundle adjustment
[DEBUG] (2021-11-15 18:24:19.710) OptimizerG2O.cpp:1263::optimizeBA() Optimizing graph...
[DEBUG] (2021-11-15 18:24:19.710) OptimizerG2O.cpp:1343::optimizeBA() fill poses to g2o...
[DEBUG] (2021-11-15 18:24:19.710) OptimizerG2O.cpp:1396::optimizeBA() fill edges to g2o...
[DEBUG] (2021-11-15 18:24:19.710) OptimizerG2O.cpp:1490::optimizeBA() fill 3D points to g2o...
[DEBUG] (2021-11-15 18:24:19.710) OptimizerG2O.cpp:1497::optimizeBA() stepVertexId=3, negVertexOffset=785
[DEBUG] (2021-11-15 18:24:19.710) OptimizerG2O.cpp:1623::optimizeBA() Initial optimization...
[DEBUG] (2021-11-15 18:24:19.710) OptimizerG2O.cpp:1628::optimizeBA() g2o optimizing begin (max iterations=20, robustKernel=8.000000)
[DEBUG] (2021-11-15 18:24:19.711) OptimizerG2O.cpp:1648::optimizeBA() iteration 0: 26 nodes, 50 edges, chi2: 159.786739
[DEBUG] (2021-11-15 18:24:19.711) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (5<->1) d=89.238251 var=1.000000 kernel=8.000000 chi2=10.731449
[DEBUG] (2021-11-15 18:24:19.711) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (304<->1) d=72.425232 var=1.000000 kernel=8.000000 chi2=9.812050
[DEBUG] (2021-11-15 18:24:19.711) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (414<->1) d=126.040649 var=1.000000 kernel=8.000000 chi2=11.160821
[DEBUG] (2021-11-15 18:24:19.711) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (424<->1) d=85.801208 var=1.000000 kernel=8.000000 chi2=12.567121
[DEBUG] (2021-11-15 18:24:19.711) OptimizerG2O.cpp:1703::optimizeBA() outliers=4 outliersCountFar=0
[DEBUG] (2021-11-15 18:24:19.711) OptimizerG2O.cpp:1648::optimizeBA() iteration 1: 26 nodes, 50 edges, chi2: 80.919276
[DEBUG] (2021-11-15 18:24:19.711) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (782<->1) d=72.882019 var=1.000000 kernel=8.000000 chi2=9.091216
[DEBUG] (2021-11-15 18:24:19.711) OptimizerG2O.cpp:1703::optimizeBA() outliers=5 outliersCountFar=0
[DEBUG] (2021-11-15 18:24:19.711) OptimizerG2O.cpp:1706::optimizeBA() g2o optimizing end (25 iterations done, error=80.919276, outliers=5/48 (delta=8.000000) time = 0.001202 s)
[DEBUG] (2021-11-15 18:24:19.711) OptimizerG2O.cpp:1801::optimizeBA() Optimizing graph...end!
[DEBUG] (2021-11-15 18:24:19.711) RegistrationVis.cpp:1800::computeTransformationImpl() Pose optimization: xyz=0.002788,-0.008124,0.110558 rpy=-0.045215,0.000796,-0.017113 -> xyz=0.005320,-0.001608,0.114051 rpy=-0.032902,-0.000643,-0.005948
[DEBUG] (2021-11-15 18:24:19.711) RegistrationVis.cpp:1814::computeTransformationImpl() BA outliers ratio 0.208333
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:1981::computeTransformationImpl() inliers=19/108
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:1982::computeTransformationImpl() transform=xyz=0.005320,-0.001608,0.114051 rpy=-0.032902,-0.000643,-0.005948
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:282::computeTransformationImpl() Vis/MinInliers=10
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:283::computeTransformationImpl() Vis/InlierDistance=0.010000
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:284::computeTransformationImpl() Vis/Iterations=300
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:285::computeTransformationImpl() Vis/EstimationType=1
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:286::computeTransformationImpl() Vis/ForwardEstOnly=0
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:287::computeTransformationImpl() Vis/EpipolarGeometryVar=0.100000
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:288::computeTransformationImpl() Vis/PnPReprojError=2.000000
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:289::computeTransformationImpl() Vis/PnPFlags=0
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:290::computeTransformationImpl() Vis/CorType=0
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:291::computeTransformationImpl() Vis/CorFlowWinSize=16
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:292::computeTransformationImpl() Vis/CorFlowIterations=30
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:293::computeTransformationImpl() Vis/CorFlowEps=0.010000
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:294::computeTransformationImpl() Vis/CorFlowMaxLevel=3
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:295::computeTransformationImpl() Vis/CorNNDR=0.800000
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:296::computeTransformationImpl() Vis/CorNNType=1
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:297::computeTransformationImpl() Vis/CorGuessWinSize=20
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:298::computeTransformationImpl() Vis/CorGuessMatchToProjection=0
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:299::computeTransformationImpl() Feature Detector = 6
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:300::computeTransformationImpl() guess=xyz=0.005320,-0.001608,0.114051 rpy=-0.032902,-0.000643,-0.005948
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:313::computeTransformationImpl() Input(1923): from=846 words, 846 3D words, 846 words descriptors,  846 kpts, 846 kpts3D, 846 descriptors, image=640x480 models=1 stereo=0
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:326::computeTransformationImpl() Input(1934): to=739 words, 739 3D words, 739 words descriptors, 797 kpts, 797 kpts3D, 797 descriptors, image=640x480 models=1 stereo=0
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:346::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:599::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:649::computeTransformationImpl() kptsFrom=846 kptsFromSource=2
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:650::computeTransformationImpl() kptsTo=739 kptsToSource=2
[DEBUG] (2021-11-15 18:24:19.712) Features2d.cpp:164::filterKeypointsByDepth() 
[DEBUG] (2021-11-15 18:24:19.712) Features2d.cpp:193::filterKeypointsByDepth() Removed 0 invalid 3D points
[DEBUG] (2021-11-15 18:24:19.712) Features2d.cpp:164::filterKeypointsByDepth() 
[DEBUG] (2021-11-15 18:24:19.712) Features2d.cpp:193::filterKeypointsByDepth() Removed 0 invalid 3D points
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:783::computeTransformationImpl() descriptorsFrom=846
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:784::computeTransformationImpl() descriptorsTo=739
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:785::computeTransformationImpl() orignalWordsFromIds=846
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:806::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:828::computeTransformationImpl() Projected points=846
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:846::computeTransformationImpl() corners in frame=477
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:851::computeTransformationImpl() guessMatchToProjection=0, cornersProjected=477
[DEBUG] (2021-11-15 18:24:19.712) RegistrationVis.cpp:998::computeTransformationImpl() match projected to frame
[DEBUG] (2021-11-15 18:24:19.717) RegistrationVis.cpp:1016::computeTransformationImpl() radius search done for guess
[DEBUG] (2021-11-15 18:24:19.719) RegistrationVis.cpp:1108::computeTransformationImpl() bruteForceDescCopy=0.000486s, bruteForceTotalTime=0.000830s
[DEBUG] (2021-11-15 18:24:19.719) RegistrationVis.cpp:1144::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:19.719) RegistrationVis.cpp:1481::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:19.719) RegistrationVis.cpp:1494::computeTransformationImpl() words from3D=846 to2D=739
[DEBUG] (2021-11-15 18:24:19.720) util3d_motion_estimation.cpp:99::estimateMotion3DTo2D() words3A=846 words2B=739 matches=184 words3B=739 guess=xyz=0.005320,-0.001608,0.114051 rpy=-0.032902,-0.000643,-0.005948 reprojError=2.000000 iterations=300
[DEBUG] (2021-11-15 18:24:19.747) RegistrationVis.cpp:1537::computeTransformationImpl() inliers: 49/184
[DEBUG] (2021-11-15 18:24:19.747) RegistrationVis.cpp:1481::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:19.747) RegistrationVis.cpp:1494::computeTransformationImpl() words from3D=739 to2D=846
[DEBUG] (2021-11-15 18:24:19.748) util3d_motion_estimation.cpp:99::estimateMotion3DTo2D() words3A=739 words2B=846 matches=184 words3B=846 guess=xyz=-0.004716,-0.007780,-0.119401 rpy=0.010773,-0.002035,-0.002527 reprojError=2.000000 iterations=300
[DEBUG] (2021-11-15 18:24:19.774) RegistrationVis.cpp:1537::computeTransformationImpl() inliers: 65/184
[DEBUG] (2021-11-15 18:24:19.774) RegistrationVis.cpp:1614::computeTransformationImpl() from->to=xyz=0.004940,0.009077,0.119300 rpy=-0.010768,0.002063,0.002505
[DEBUG] (2021-11-15 18:24:19.774) RegistrationVis.cpp:1615::computeTransformationImpl() to->from=xyz=-0.005683,0.004350,-0.116260 rpy=0.033011,0.003343,0.034747
[DEBUG] (2021-11-15 18:24:19.775) RegistrationVis.cpp:1658::computeTransformationImpl() Refine with bundle adjustment
[DEBUG] (2021-11-15 18:24:19.775) OptimizerG2O.cpp:1263::optimizeBA() Optimizing graph...
[DEBUG] (2021-11-15 18:24:19.775) OptimizerG2O.cpp:1343::optimizeBA() fill poses to g2o...
[DEBUG] (2021-11-15 18:24:19.775) OptimizerG2O.cpp:1396::optimizeBA() fill edges to g2o...
[DEBUG] (2021-11-15 18:24:19.775) OptimizerG2O.cpp:1490::optimizeBA() fill 3D points to g2o...
[DEBUG] (2021-11-15 18:24:19.775) OptimizerG2O.cpp:1497::optimizeBA() stepVertexId=3, negVertexOffset=837
[DEBUG] (2021-11-15 18:24:19.775) OptimizerG2O.cpp:1623::optimizeBA() Initial optimization...
[DEBUG] (2021-11-15 18:24:19.775) OptimizerG2O.cpp:1628::optimizeBA() g2o optimizing begin (max iterations=20, robustKernel=8.000000)
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1648::optimizeBA() iteration 0: 76 nodes, 150 edges, chi2: 750.915528
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (3<->1) d=87.197205 var=1.000000 kernel=8.000000 chi2=13.030531
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (87<->1) d=114.863586 var=1.000000 kernel=8.000000 chi2=12.900938
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (168<->1) d=121.466019 var=1.000000 kernel=8.000000 chi2=9.409182
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (193<->1) d=88.300720 var=1.000000 kernel=8.000000 chi2=17.126374
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (193<->2) d=118.735992 var=1.000000 kernel=8.000000 chi2=10.084274
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (195<->1) d=66.751495 var=1.000000 kernel=8.000000 chi2=15.176878
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (195<->2) d=78.681549 var=1.000000 kernel=8.000000 chi2=12.648429
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (215<->1) d=101.496704 var=1.000000 kernel=8.000000 chi2=10.773355
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (292<->1) d=145.994186 var=1.000000 kernel=8.000000 chi2=12.433928
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (304<->1) d=72.425232 var=1.000000 kernel=8.000000 chi2=11.328111
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (308<->1) d=86.798096 var=1.000000 kernel=8.000000 chi2=16.120985
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (308<->2) d=110.142456 var=1.000000 kernel=8.000000 chi2=10.527219
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (342<->1) d=126.862198 var=1.000000 kernel=8.000000 chi2=22.359784
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (367<->1) d=156.663086 var=1.000000 kernel=8.000000 chi2=35.946088
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (367<->2) d=262.644379 var=1.000000 kernel=8.000000 chi2=21.476754
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (424<->1) d=85.801208 var=1.000000 kernel=8.000000 chi2=8.744878
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (464<->1) d=100.326447 var=1.000000 kernel=8.000000 chi2=22.340913
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (464<->2) d=136.132263 var=1.000000 kernel=8.000000 chi2=12.717869
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (483<->1) d=90.416473 var=1.000000 kernel=8.000000 chi2=27.064067
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (483<->2) d=114.048828 var=1.000000 kernel=8.000000 chi2=16.135449
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (495<->1) d=140.636353 var=1.000000 kernel=8.000000 chi2=31.753046
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (495<->2) d=230.896179 var=1.000000 kernel=8.000000 chi2=9.581667
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (563<->1) d=95.779144 var=1.000000 kernel=8.000000 chi2=8.963075
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (610<->1) d=97.470551 var=1.000000 kernel=8.000000 chi2=9.426810
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (752<->1) d=87.766296 var=1.000000 kernel=8.000000 chi2=28.509669
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (752<->2) d=111.252502 var=1.000000 kernel=8.000000 chi2=17.864639
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (782<->1) d=72.882019 var=1.000000 kernel=8.000000 chi2=8.378200
[DEBUG] (2021-11-15 18:24:19.776) OptimizerG2O.cpp:1703::optimizeBA() outliers=27 outliersCountFar=0
[DEBUG] (2021-11-15 18:24:19.778) OptimizerG2O.cpp:1648::optimizeBA() iteration 1: 76 nodes, 150 edges, chi2: 254.550537
[DEBUG] (2021-11-15 18:24:19.778) OptimizerG2O.cpp:1703::optimizeBA() outliers=27 outliersCountFar=0
[DEBUG] (2021-11-15 18:24:19.778) OptimizerG2O.cpp:1706::optimizeBA() g2o optimizing end (25 iterations done, error=254.550537, outliers=27/148 (delta=8.000000) time = 0.002468 s)
[DEBUG] (2021-11-15 18:24:19.778) OptimizerG2O.cpp:1801::optimizeBA() Optimizing graph...end!
[DEBUG] (2021-11-15 18:24:19.778) RegistrationVis.cpp:1800::computeTransformationImpl() Pose optimization: xyz=0.004940,0.009077,0.119300 rpy=-0.010768,0.002063,0.002505 -> xyz=0.004599,0.004180,0.120791 rpy=-0.022891,0.001114,-0.016257
[DEBUG] (2021-11-15 18:24:19.778) RegistrationVis.cpp:1814::computeTransformationImpl() BA outliers ratio 0.256757
[DEBUG] (2021-11-15 18:24:19.778) RegistrationVis.cpp:1981::computeTransformationImpl() inliers=55/184
[DEBUG] (2021-11-15 18:24:19.778) RegistrationVis.cpp:1982::computeTransformationImpl() transform=xyz=0.004599,0.004180,0.120791 rpy=-0.022891,0.001114,-0.016257
[ INFO] (2021-11-15 18:24:19.778) Rtabmap.cpp:2505::process() [Visual] Add local loop closure in SPACE (1934->1923) xyz=-0.004396,-0.001488,-0.120862 rpy=0.022870,-0.001486,0.016227
[ INFO] (2021-11-15 18:24:19.778) Memory.cpp:3240::addLink() to=1923, from=1934 transform: xyz=-0.004396,-0.001488,-0.120862 rpy=0.022870,-0.001486,0.016227 var=0.000028
[DEBUG] (2021-11-15 18:24:19.778) Memory.cpp:3252::addLink() Add link between 1923 and 1934
[DEBUG] (2021-11-15 18:24:19.778) Signature.cpp:121::addLink() Add link 1934 to 1923 (type=2 var=0.000028,0.005525)
[DEBUG] (2021-11-15 18:24:19.778) Signature.cpp:121::addLink() Add link 1923 to 1934 (type=2 var=0.000028,0.005525)
[DEBUG] (2021-11-15 18:24:19.778) Graph.cpp:2265::getPosesInRadius() found nodes=1
[DEBUG] (2021-11-15 18:24:19.778) Memory.cpp:2730::computeTransform() 
[DEBUG] (2021-11-15 18:24:19.778) SensorData.cpp:506::uncompressData() 1837 data(1,1,0,0,0,0,0)
[DEBUG] (2021-11-15 18:24:19.778) SensorData.cpp:506::uncompressData() 1934 data(1,1,0,0,0,0,0)
[DEBUG] (2021-11-15 18:24:19.778) Memory.cpp:2773::computeTransform() 
[DEBUG] (2021-11-15 18:24:19.778) RegistrationVis.cpp:282::computeTransformationImpl() Vis/MinInliers=10
[DEBUG] (2021-11-15 18:24:19.778) RegistrationVis.cpp:283::computeTransformationImpl() Vis/InlierDistance=0.010000
[DEBUG] (2021-11-15 18:24:19.778) RegistrationVis.cpp:284::computeTransformationImpl() Vis/Iterations=300
[DEBUG] (2021-11-15 18:24:19.778) RegistrationVis.cpp:285::computeTransformationImpl() Vis/EstimationType=1
[DEBUG] (2021-11-15 18:24:19.778) RegistrationVis.cpp:286::computeTransformationImpl() Vis/ForwardEstOnly=0
[DEBUG] (2021-11-15 18:24:19.778) RegistrationVis.cpp:287::computeTransformationImpl() Vis/EpipolarGeometryVar=0.100000
[DEBUG] (2021-11-15 18:24:19.778) RegistrationVis.cpp:288::computeTransformationImpl() Vis/PnPReprojError=2.000000
[DEBUG] (2021-11-15 18:24:19.778) RegistrationVis.cpp:289::computeTransformationImpl() Vis/PnPFlags=0
[DEBUG] (2021-11-15 18:24:19.778) RegistrationVis.cpp:290::computeTransformationImpl() Vis/CorType=0
[DEBUG] (2021-11-15 18:24:19.778) RegistrationVis.cpp:291::computeTransformationImpl() Vis/CorFlowWinSize=16
[DEBUG] (2021-11-15 18:24:19.779) RegistrationVis.cpp:292::computeTransformationImpl() Vis/CorFlowIterations=30
[DEBUG] (2021-11-15 18:24:19.779) RegistrationVis.cpp:293::computeTransformationImpl() Vis/CorFlowEps=0.010000
[DEBUG] (2021-11-15 18:24:19.779) RegistrationVis.cpp:294::computeTransformationImpl() Vis/CorFlowMaxLevel=3
[DEBUG] (2021-11-15 18:24:19.779) RegistrationVis.cpp:295::computeTransformationImpl() Vis/CorNNDR=0.800000
[DEBUG] (2021-11-15 18:24:19.779) RegistrationVis.cpp:296::computeTransformationImpl() Vis/CorNNType=1
[DEBUG] (2021-11-15 18:24:19.779) RegistrationVis.cpp:297::computeTransformationImpl() Vis/CorGuessWinSize=20
[DEBUG] (2021-11-15 18:24:19.779) RegistrationVis.cpp:298::computeTransformationImpl() Vis/CorGuessMatchToProjection=0
[DEBUG] (2021-11-15 18:24:19.779) RegistrationVis.cpp:299::computeTransformationImpl() Feature Detector = 6
[DEBUG] (2021-11-15 18:24:19.779) RegistrationVis.cpp:300::computeTransformationImpl() guess=xyz=[null] rpy=[null]
[DEBUG] (2021-11-15 18:24:19.779) RegistrationVis.cpp:313::computeTransformationImpl() Input(1837): from=0 words, 0 3D words, 0 words descriptors,  0 kpts, 0 kpts3D, 0 descriptors, image=640x480 models=1 stereo=0
[DEBUG] (2021-11-15 18:24:19.779) RegistrationVis.cpp:326::computeTransformationImpl() Input(1934): to=0 words, 0 3D words, 0 words descriptors, 0 kpts, 0 kpts3D, 0 descriptors, image=640x480 models=1 stereo=0
[DEBUG] (2021-11-15 18:24:19.779) RegistrationVis.cpp:346::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:19.781) util2d.cpp:1162::computeRoi() roi ratios = 0.000000, 0.000000, 0.000000, 0.000000
[DEBUG] (2021-11-15 18:24:19.781) util2d.cpp:1163::computeRoi() roi = 0, 0, 640, 480
[DEBUG] (2021-11-15 18:24:19.781) util2d.cpp:1190::computeRoi() roi = 0, 0, 640, 480
[DEBUG] (2021-11-15 18:24:19.790) Features2d.cpp:751::generateKeypoints() Keypoints extraction time = 0.009325 s, keypoints extracted = 1000 (grid=1x1, mask empty=0)
[DEBUG] (2021-11-15 18:24:19.790) RegistrationVis.cpp:599::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:19.792) util2d.cpp:1162::computeRoi() roi ratios = 0.000000, 0.000000, 0.000000, 0.000000
[DEBUG] (2021-11-15 18:24:19.792) util2d.cpp:1163::computeRoi() roi = 0, 0, 640, 480
[DEBUG] (2021-11-15 18:24:19.792) util2d.cpp:1190::computeRoi() roi = 0, 0, 640, 480
[DEBUG] (2021-11-15 18:24:19.802) Features2d.cpp:751::generateKeypoints() Keypoints extraction time = 0.009662 s, keypoints extracted = 1000 (grid=1x1, mask empty=0)
[DEBUG] (2021-11-15 18:24:19.802) RegistrationVis.cpp:649::computeTransformationImpl() kptsFrom=1000 kptsFromSource=0
[DEBUG] (2021-11-15 18:24:19.802) RegistrationVis.cpp:650::computeTransformationImpl() kptsTo=1000 kptsToSource=0
[DEBUG] (2021-11-15 18:24:19.802) RegistrationVis.cpp:672::computeTransformationImpl() cleared orignalWordsFromIds
[DEBUG] (2021-11-15 18:24:19.805) Features2d.cpp:783::generateDescriptors() Descriptors extracted = 688, remaining kpts=688
[DEBUG] (2021-11-15 18:24:19.808) Features2d.cpp:783::generateDescriptors() Descriptors extracted = 797, remaining kpts=797
[DEBUG] (2021-11-15 18:24:19.808) RegistrationVis.cpp:733::computeTransformationImpl() generated kptsFrom3D=688
[DEBUG] (2021-11-15 18:24:19.808) Features2d.cpp:164::filterKeypointsByDepth() 
[DEBUG] (2021-11-15 18:24:19.809) Features2d.cpp:193::filterKeypointsByDepth() Removed 0 invalid 3D points
[DEBUG] (2021-11-15 18:24:19.809) Features2d.cpp:164::filterKeypointsByDepth() 
[DEBUG] (2021-11-15 18:24:19.809) Features2d.cpp:193::filterKeypointsByDepth() Removed 0 invalid 3D points
[DEBUG] (2021-11-15 18:24:19.809) RegistrationVis.cpp:783::computeTransformationImpl() descriptorsFrom=688
[DEBUG] (2021-11-15 18:24:19.809) RegistrationVis.cpp:784::computeTransformationImpl() descriptorsTo=797
[DEBUG] (2021-11-15 18:24:19.809) RegistrationVis.cpp:785::computeTransformationImpl() orignalWordsFromIds=0
[DEBUG] (2021-11-15 18:24:19.809) RegistrationVis.cpp:1165::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:19.809) RegistrationVis.cpp:1265::computeTransformationImpl() VWDictionary knn matching
[DEBUG] (2021-11-15 18:24:19.809) VWDictionary.cpp:470::update() incremental=1
[DEBUG] (2021-11-15 18:24:19.809) FlannIndex.cpp:52::release() 
[DEBUG] (2021-11-15 18:24:19.809) FlannIndex.cpp:80::release() 
[DEBUG] (2021-11-15 18:24:19.809) VWDictionary.cpp:681::update() Dictionary updated! (size=0 added=0 removed=0)
[DEBUG] (2021-11-15 18:24:19.809) VWDictionary.cpp:689::update() 
[DEBUG] (2021-11-15 18:24:19.809) VWDictionary.cpp:763::addNewWords() id=1 descriptors=688
[DEBUG] (2021-11-15 18:24:19.810) VWDictionary.cpp:1059::addNewWords() naive search and add ref/words time = 0.000855 s
[DEBUG] (2021-11-15 18:24:19.810) VWDictionary.cpp:1061::addNewWords() 688 new words added...
[DEBUG] (2021-11-15 18:24:19.810) VWDictionary.cpp:1063::addNewWords() 0 duplicated words added (from current image = 0)...
[DEBUG] (2021-11-15 18:24:19.810) VWDictionary.cpp:1064::addNewWords() total time 0.001020s
[DEBUG] (2021-11-15 18:24:19.810) VWDictionary.cpp:470::update() incremental=1
[DEBUG] (2021-11-15 18:24:19.810) VWDictionary.cpp:491::update() Incremental FLANN: Removing 0 words...
[DEBUG] (2021-11-15 18:24:19.810) VWDictionary.cpp:500::update() Incremental FLANN: Removing 0 words... done!
[DEBUG] (2021-11-15 18:24:19.810) VWDictionary.cpp:504::update() Incremental FLANN: Inserting 688 words...
[DEBUG] (2021-11-15 18:24:19.810) VWDictionary.cpp:531::update() Building FLANN index...
[DEBUG] (2021-11-15 18:24:19.810) FlannIndex.cpp:208::buildKDTreeIndex() 
[DEBUG] (2021-11-15 18:24:19.810) FlannIndex.cpp:52::release() 
[DEBUG] (2021-11-15 18:24:19.810) FlannIndex.cpp:80::release() 
[DEBUG] (2021-11-15 18:24:19.810) FlannIndex.cpp:260::buildKDTreeIndex() 
[DEBUG] (2021-11-15 18:24:19.810) VWDictionary.cpp:549::update() Building FLANN index... done!
[DEBUG] (2021-11-15 18:24:19.810) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 1 -> 3
[DEBUG] (2021-11-15 18:24:19.810) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 3 -> 7
[DEBUG] (2021-11-15 18:24:19.810) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 7 -> 15
[DEBUG] (2021-11-15 18:24:19.811) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 15 -> 31
[DEBUG] (2021-11-15 18:24:19.811) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 31 -> 63
[DEBUG] (2021-11-15 18:24:19.812) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 63 -> 127
[DEBUG] (2021-11-15 18:24:19.813) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 127 -> 255
[DEBUG] (2021-11-15 18:24:19.816) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 255 -> 511
[DEBUG] (2021-11-15 18:24:19.821) VWDictionary.cpp:564::update() Incremental FLANN: Inserting 688 words... done!
[DEBUG] (2021-11-15 18:24:19.821) VWDictionary.cpp:681::update() Dictionary updated! (size=0 added=688 removed=0)
[DEBUG] (2021-11-15 18:24:19.821) VWDictionary.cpp:689::update() 
[DEBUG] (2021-11-15 18:24:19.821) VWDictionary.cpp:763::addNewWords() id=2 descriptors=797
[DEBUG] (2021-11-15 18:24:19.821) VWDictionary.cpp:857::addNewWords() newPts.total()=797 
[DEBUG] (2021-11-15 18:24:19.835) VWDictionary.cpp:924::addNewWords() Time to find nn = 0.014227 s
[DEBUG] (2021-11-15 18:24:19.837) VWDictionary.cpp:1059::addNewWords() naive search and add ref/words time = 0.001179 s
[DEBUG] (2021-11-15 18:24:19.837) VWDictionary.cpp:1061::addNewWords() 658 new words added...
[DEBUG] (2021-11-15 18:24:19.837) VWDictionary.cpp:1063::addNewWords() 139 duplicated words added (from current image = 0)...
[DEBUG] (2021-11-15 18:24:19.837) VWDictionary.cpp:1064::addNewWords() total time 0.015613s
[DEBUG] (2021-11-15 18:24:19.837) VWDictionary.cpp:694::clear() 
[DEBUG] (2021-11-15 18:24:19.837) FlannIndex.cpp:52::release() 
[DEBUG] (2021-11-15 18:24:19.837) FlannIndex.cpp:80::release() 
[DEBUG] (2021-11-15 18:24:19.837) VWDictionary.cpp:694::clear() 
[DEBUG] (2021-11-15 18:24:19.837) FlannIndex.cpp:52::release() 
[DEBUG] (2021-11-15 18:24:19.837) FlannIndex.cpp:80::release() 
[DEBUG] (2021-11-15 18:24:19.837) FlannIndex.cpp:52::release() 
[DEBUG] (2021-11-15 18:24:19.837) FlannIndex.cpp:80::release() 
[DEBUG] (2021-11-15 18:24:19.839) RegistrationVis.cpp:1481::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:19.839) RegistrationVis.cpp:1494::computeTransformationImpl() words from3D=688 to2D=730
[DEBUG] (2021-11-15 18:24:19.840) util3d_motion_estimation.cpp:99::estimateMotion3DTo2D() words3A=688 words2B=730 matches=72 words3B=730 guess=xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000 reprojError=2.000000 iterations=300
[DEBUG] (2021-11-15 18:24:19.864) RegistrationVis.cpp:1537::computeTransformationImpl() inliers: 0/72
[ INFO] (2021-11-15 18:24:19.864) RegistrationVis.cpp:1542::computeTransformationImpl() Not enough inliers 0/10 (matches=72) between 1837 and 1934
[DEBUG] (2021-11-15 18:24:19.864) RegistrationVis.cpp:1481::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:19.864) RegistrationVis.cpp:1494::computeTransformationImpl() words from3D=730 to2D=688
[DEBUG] (2021-11-15 18:24:19.865) util3d_motion_estimation.cpp:99::estimateMotion3DTo2D() words3A=730 words2B=688 matches=72 words3B=688 guess=xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000 reprojError=2.000000 iterations=300
[DEBUG] (2021-11-15 18:24:19.889) RegistrationVis.cpp:1537::computeTransformationImpl() inliers: 0/72
[ INFO] (2021-11-15 18:24:19.889) RegistrationVis.cpp:1542::computeTransformationImpl() Not enough inliers 0/10 (matches=72) between 1934 and 1837
[DEBUG] (2021-11-15 18:24:19.889) RegistrationVis.cpp:1614::computeTransformationImpl() from->to=xyz=[null] rpy=[null]
[DEBUG] (2021-11-15 18:24:19.889) RegistrationVis.cpp:1615::computeTransformationImpl() to->from=xyz=[null] rpy=[null]
[DEBUG] (2021-11-15 18:24:19.889) RegistrationVis.cpp:1981::computeTransformationImpl() inliers=0/72
[DEBUG] (2021-11-15 18:24:19.889) RegistrationVis.cpp:1982::computeTransformationImpl() transform=xyz=[null] rpy=[null]
[ INFO] (2021-11-15 18:24:19.889) Rtabmap.cpp:2549::process() timeProximityBySpaceVisualDetection=0.309244s
[DEBUG] (2021-11-15 18:24:19.889) Rtabmap.cpp:2554::process() Proximity detection (local loop closure in SPACE with scan matching)
[DEBUG] (2021-11-15 18:24:19.889) Rtabmap.cpp:2557::process() Proximity by scan matching is disabled (RGBD/ProximityPathMaxNeighbors=0).
[ INFO] (2021-11-15 18:24:19.889) Rtabmap.cpp:2740::process() timeProximityBySpaceDetection=0.000095s
[ INFO] (2021-11-15 18:24:19.889) Rtabmap.cpp:2804::process() timeAddLoopClosureLink=0.000013s
[ INFO] (2021-11-15 18:24:19.890) Rtabmap.cpp:3103::process() Update map correction
[ INFO] (2021-11-15 18:24:19.890) Rtabmap.cpp:4363::optimizeCurrentMap() Optimize map: around location 1934 (lookInDatabase=false)
[ INFO] (2021-11-15 18:24:19.890) Rtabmap.cpp:4372::optimizeCurrentMap() get 666 ids time 0.000759 s
[DEBUG] (2021-11-15 18:24:19.891) Rtabmap.cpp:4413::optimizeGraph() ids=666
[DEBUG] (2021-11-15 18:24:19.891) Memory.cpp:5921::getMetricConstraints() 
[ INFO] (2021-11-15 18:24:19.894) Rtabmap.cpp:4415::optimizeGraph() get constraints (ids=666, 666 poses, 951 edges) time 0.003412 s
[DEBUG] (2021-11-15 18:24:19.894) Rtabmap.cpp:4458::optimizeGraph() use input guess poses
[DEBUG] (2021-11-15 18:24:19.894) OptimizerGTSAM.cpp:103::optimize() Optimizing graph...
[DEBUG] (2021-11-15 18:24:19.894) OptimizerGTSAM.cpp:141::optimize() hasPriorPoses=false, gpsPriorOnly=false
[DEBUG] (2021-11-15 18:24:19.894) OptimizerGTSAM.cpp:159::optimize() fill poses to gtsam... rootId=1 (priorsIgnored=1 gpsPriorOnly=0 landmarksIgnored=0)
[DEBUG] (2021-11-15 18:24:19.894) OptimizerGTSAM.cpp:218::optimize() fill edges to gtsam...
[DEBUG] (2021-11-15 18:24:19.896) OptimizerGTSAM.cpp:474::optimize() create optimizer
[DEBUG] (2021-11-15 18:24:19.897) OptimizerGTSAM.cpp:499::optimize() GTSAM optimizing begin (max iterations=20, robust=0)
[DEBUG] (2021-11-15 18:24:19.908) OptimizerGTSAM.cpp:579::optimize() iteration 1 error =13.572603
[DEBUG] (2021-11-15 18:24:19.917) OptimizerGTSAM.cpp:579::optimize() iteration 2 error =309465214733745921641668712960291418554613816179502349632969286512135975755973393914491682532569273695269907928810519864292103990811520980539610577086278637107618447794139104290143356126315618156917672533023358590066801363443411129710086691757773447440365582775117867384832.000000
[DEBUG] (2021-11-15 18:24:19.917) OptimizerGTSAM.cpp:585::optimize() Negative improvement?! Ignore and continue optimizing... (-309465214733745921641668712960291418554613816179502349632969286512135975755973393914491682532569273695269907928810519864292103990811520980539610577086278637107618447794139104290143356126315618156917672533023358590066801363443411129710086691757773447440365582775117867384832.000000 < 0.000010)
[DEBUG] (2021-11-15 18:24:19.926) OptimizerGTSAM.cpp:579::optimize() iteration 3 error =467880917962098550118877521193338604353973373981833322051620068764959067488153115818368238633949641114218334146570480323685443962103092128070172665041343088105124406440158943896435169573124872346411187916687631902165599539264331659027609404078523848907219285907079168.000000
[DEBUG] (2021-11-15 18:24:19.935) OptimizerGTSAM.cpp:579::optimize() iteration 4 error =19899847231808175072428900078048826752955207512658355240989304249590100992692419706174813586158249505075737637480234368402100406482325704854554530182215600640951125212157956844314806580241009762481833603733117471243925887116936600073831418641223277211223932931145728.000000
[DEBUG] (2021-11-15 18:24:19.943) OptimizerGTSAM.cpp:579::optimize() iteration 5 error =673783818938113400396869841533645736735233093957475139153777353176645737015265912937178102495943248683977023814945715369300332598551169900355633921620607735986036248827249059687149230952006244748370848692064422649384109832862760276025619815799186151833715762790400.000000
[DEBUG] (2021-11-15 18:24:19.952) OptimizerGTSAM.cpp:579::optimize() iteration 6 error =486044415790970100162764879859805383411300109414480275687463523354715255960064957310658428424133429810892912059984069389506916956938431383007754075326042212157403913053156383824204973414281528907753740677812857076541187684856561231273155168663377656692410211631104.000000
[DEBUG] (2021-11-15 18:24:19.960) OptimizerGTSAM.cpp:579::optimize() iteration 7 error =634487396418861719567269363853006010657212122582232591013734617722231521929253114109864132706580570989063634499635179603732513114395088294374915902157627525210934558416145941970387268831057484857742816268890661971081276701821640714830779751968943671697940479475712.000000
[DEBUG] (2021-11-15 18:24:19.960) OptimizerGTSAM.cpp:585::optimize() Negative improvement?! Ignore and continue optimizing... (-148442980627891619404504483993200627245912013167752315326271094367516265969188156799205704282447141178170722439651110214225596157456656911367161826831585313053530645362989558146182295416775955949989075591077804894540089016965079483557624583305566015005530267844608.000000 < 0.000010)
[DEBUG] (2021-11-15 18:24:19.968) OptimizerGTSAM.cpp:579::optimize() iteration 8 error =33883408428152563710100715188769201059162257572977187336564855799559820600184216723814686417715221116153838937350122973286616409572112852646695892073506110906258457798070157970139582764733319742494414189307144444097282258992531156139292467454991849113471607111680.000000
[DEBUG] (2021-11-15 18:24:19.976) OptimizerGTSAM.cpp:579::optimize() iteration 9 error =23764139942723898614588995919959491160453551792133502547709796547775113772294121867282946095867486006492696638276375856046787502320020064913444155450032541105523449705605178237977174801223183515400459724322804273524512655227417705223931165371599652921103389992943616.000000
[DEBUG] (2021-11-15 18:24:19.976) OptimizerGTSAM.cpp:585::optimize() Negative improvement?! Ignore and continue optimizing... (-23730256534295745876080897655485070076955522751877184962901679127457411071802276869851953642403236609685110512811752933822039037649951613716090057476212351031940030349585796919650136366046266954283445891926839559225434853975839435078957927550521714200903571300417536.000000 < 0.000010)
[DEBUG] (2021-11-15 18:24:19.985) OptimizerGTSAM.cpp:579::optimize() iteration 10 error =20276432875118766123428005229942163055209598182034115765741648420610093108115057272023758547414470478727172904002556810427895868193625888068474755035130816973933137597369658909096381801649081285504953268424245618093032318263747695826224411060448654419707908885315584.000000
[DEBUG] (2021-11-15 18:24:19.993) OptimizerGTSAM.cpp:579::optimize() iteration 11 error =21123381903179090142854067761145462283047925287151764434074670505841644392492152161871555828151038389153606548055940197093979788069158941224382163031253223067159829619471209428707902245198716058110463172604060614902521168278306237800377677119779486219231916195840.000000
[DEBUG] (2021-11-15 18:24:20.002) OptimizerGTSAM.cpp:579::optimize() iteration 12 error =6054326821298284307843270356930573733455500000447441269666675658429338159814198534352261934478322678717043671319702729121206842142890806828382120301939226350130342248129445478889641480200900695913162571133169019566884117297885757120126045409407298061447460177313792.000000
[DEBUG] (2021-11-15 18:24:20.002) OptimizerGTSAM.cpp:585::optimize() Negative improvement?! Ignore and continue optimizing... (-6033203439395104955503419965240950447514151901135278909025272141146282195584215211129623728080370376790741634980737590046920089964077140370096635016287947183047441071178007528925585299337427117793272980650578604169510817640728842399074749701853098268598707632996352.000000 < 0.000010)
[DEBUG] (2021-11-15 18:24:20.009) OptimizerGTSAM.cpp:579::optimize() iteration 13 error =14355916544128327017291706457550369582370175086173781434697233520655359556341510476964642079151312596320557133154398494166132769466314364416247912880073027496033556813643199830543244134803719026568596387820470353764545365357716831409554168114492340572327118502887424.000000
[DEBUG] (2021-11-15 18:24:20.009) OptimizerGTSAM.cpp:585::optimize() Negative improvement?! Ignore and continue optimizing... (-8301589722830043604414183552962333487001673013065043000084906992558912941572615139833130311951244897143646768854332497212410590417164809912767691236676823034810278364406867492680924778953196526492973237905388091855161505301870057912257922915634530490841755402895360.000000 < 0.000010)
[DEBUG] (2021-11-15 18:24:20.020) OptimizerGTSAM.cpp:579::optimize() iteration 14 error =24890474839273769147517018274133022945023277637975355930275125016450197189673420773160403405151680863691295620469661283820347652650616118123634726616029938838898719719005178934264300084335046715473998769677888802258833551175621614677441461933883804952471410659295232.000000
[DEBUG] (2021-11-15 18:24:20.020) OptimizerGTSAM.cpp:585::optimize() Negative improvement?! Ignore and continue optimizing... (-10534558295145442130225311816582653362653102551801574495577891495794837633331910296195761326000368267370738487315262789654214883184301753707386813735956911342865162905361979103721055949531327688905402381857418448494288185817904783267887293819391464380144292156407808.000000 < 0.000010)
[DEBUG] (2021-11-15 18:24:20.028) OptimizerGTSAM.cpp:579::optimize() iteration 15 error =6261944730640638373193560787346256221126025528089210994411685716989157046833640538982005638145707654922321749058026361209163584569746211744921572875971552596631221633188287848138673920983087518743129937393483496752376365714316295143855320910429695534656737968652288.000000
[DEBUG] (2021-11-15 18:24:20.036) OptimizerGTSAM.cpp:579::optimize() iteration 16 error =9172392025064712954300066186755232216912493173933786443351927451200518227763682973806853927793419982710718476680859940531439897003910616251168702116373133961846224613130751423002061985255910176778068095982562645378078700617460963842184340235616356465595425549713408.000000
[DEBUG] (2021-11-15 18:24:20.036) OptimizerGTSAM.cpp:585::optimize() Negative improvement?! Ignore and continue optimizing... (-2910447294424074581106505399408975995786467645844575448940241734211361180930042434824848289647712327788396727622833579322276312434164404506247129240401581365215002979942463574863388064272822658034938158589079148625702334903144668698329019325186660930938687581061120.000000 < 0.000010)
[DEBUG] (2021-11-15 18:24:20.044) OptimizerGTSAM.cpp:579::optimize() iteration 17 error =4309761245874011431494225804966109857571883756204623467937486932587889960698961646692616147207325690318577961285801171495961531348172437229115240118006019974493499908516468655635541622301662146026493837509286274001896924149653820181785282704577680395180001164525568.000000
[DEBUG] (2021-11-15 18:24:20.052) OptimizerGTSAM.cpp:579::optimize() iteration 18 error =1218537939669785615548430651870111385404654327502618029604933355313260525854287650036997323189088679646226513206411021405679774521950617469120289127971839495968532375294897672540443256594211358092076997498072970839852524880556035032093266980691110406458900920401920.000000
[DEBUG] (2021-11-15 18:24:20.061) OptimizerGTSAM.cpp:579::optimize() iteration 19 error =34815022148238730580922763556378828201334296160668787050793902686597662791263049192868051941645086929051666305768645920423830327263330667858971895899517764912910447014804114830802869580287576987891740105142543205457964097522250711689470833920117943777785964134400.000000
[DEBUG] (2021-11-15 18:24:20.070) OptimizerGTSAM.cpp:579::optimize() iteration 20 error =634994148215532948981485495808996561581486428794648718684581220628615348486891485790302006511108332285856180227093273328509323495286906877535238886694291170524412385611529920380861173915834429470417062067353311171496726338998339166362523687061503794770781134651392.000000
[DEBUG] (2021-11-15 18:24:20.070) OptimizerGTSAM.cpp:585::optimize() Negative improvement?! Ignore and continue optimizing... (-600179126067294225392482634224043808677706803941313547532649420522743400891294867825721065251324612384461805382415539378143967098443429743464563074064640764118588374525578251964334258432034182137506098690477070760237983001179518014226410667286303725836449053933568.000000 < 0.000010)
[DEBUG] (2021-11-15 18:24:20.071) OptimizerGTSAM.cpp:609::optimize() GTSAM optimizing end (20 iterations done, error=634994148215532948981485495808996561581486428794648718684581220628615348486891485790302006511108332285856180227093273328509323495286906877535238886694291170524412385611529920380861173915834429470417062067353311171496726338998339166362523687061503794770781134651392.000000 (initial=14.435729 final=634994148215532948981485495808996561581486428794648718684581220628615348486891485790302006511108332285856180227093273328509323495286906877535238886694291170524412385611529920380861173915834429470417062067353311171496726338998339166362523687061503794770781134651392.000000), time=0.172709 s)
[DEBUG] (2021-11-15 18:24:20.072) OptimizerGTSAM.cpp:667::optimize() Computing marginals...
[DEBUG] (2021-11-15 18:24:20.088) OptimizerGTSAM.cpp:671::optimize() Computed marginals = 0.015944s (key=1934)
[DEBUG] (2021-11-15 18:24:20.089) OptimizerGTSAM.cpp:717::optimize() Optimizing graph...end!
[ INFO] (2021-11-15 18:24:20.089) Rtabmap.cpp:4473::optimizeGraph() Optimization time 0.194887 s
[ INFO] (2021-11-15 18:24:20.089) Rtabmap.cpp:4375::optimizeCurrentMap() optimize time 0.198666 s
[ INFO] (2021-11-15 18:24:20.089) Rtabmap.cpp:4384::optimizeCurrentMap() Correction (from node 1) xyz=0.000000,-0.000000,0.000000 rpy=0.000001,0.000000,0.000000
[ INFO] (2021-11-15 18:24:20.089) Rtabmap.cpp:3142::process() Compute max graph errors...
[DEBUG] (2021-11-15 18:24:20.089) Graph.cpp:908::computeMaxGraphErrors() poses=666 links=951
[ INFO] (2021-11-15 18:24:20.092) Rtabmap.cpp:3162::process() Max optimization linear error = 0.012380 m (link 472->485, var=0.000155, ratio error/std=0.993851)
[ INFO] (2021-11-15 18:24:20.092) Rtabmap.cpp:3186::process() Max optimization angular error = 6.126644 deg (link 472->485, var=0.009449, ratio error/std=1.100058)
[ INFO] (2021-11-15 18:24:20.092) Rtabmap.cpp:3226::process() Updated local map (old size=666, new size=666)
[ INFO] (2021-11-15 18:24:20.092) Rtabmap.cpp:3276::process() timeMapOptimization=0.202738s
[ INFO] (2021-11-15 18:24:20.092) Rtabmap.cpp:3303::process() sending stats...
[ INFO] (2021-11-15 18:24:20.092) Rtabmap.cpp:3315::process() send all stats...
[ INFO] (2021-11-15 18:24:20.092) Rtabmap.cpp:3365::process() Set loop closure transform = xyz=0.004599,0.004180,0.120791 rpy=-0.022891,0.001114,-0.016257
[ INFO] (2021-11-15 18:24:20.092) Rtabmap.cpp:3439::process() Localization pose = xyz=6.518633,-1.986725,1.027154 rpy=-3.027722,0.692033,2.873248
[ INFO] (2021-11-15 18:24:20.092) Rtabmap.cpp:3443::process() Set map correction = xyz=0.094648,0.242915,-0.057500 rpy=0.017194,-0.011380,-0.025043
[ INFO] (2021-11-15 18:24:20.092) Rtabmap.cpp:3516::process() Time creating stats = 0.000144...
[DEBUG] (2021-11-15 18:24:20.092) Memory.cpp:2682::removeRawData() id=1934 image=1 scan=0 userData=1
[DEBUG] (2021-11-15 18:24:20.092) Memory.cpp:2013::cleanup() 
[DEBUG] (2021-11-15 18:24:20.092) Memory.cpp:2600::saveLocationData() Saving location data 1934
[DEBUG] (2021-11-15 18:24:20.093) DBDriver.cpp:386::asyncSave() s=1934
[ INFO] (2021-11-15 18:24:20.093) Rtabmap.cpp:3596::process() timeMemoryCleanup = 0.000695s... 0 signatures removed
[ INFO] (2021-11-15 18:24:20.093) Rtabmap.cpp:3609::process() Total time processing = 0.559583s...
[ INFO] (2021-11-15 18:24:20.093) Rtabmap.cpp:3748::process() Time limit reached processing = 0.000020...
[DEBUG] (2021-11-15 18:24:20.093) Rtabmap.cpp:3789::process() 
[ INFO] (2021-11-15 18:24:20.093) Rtabmap.cpp:3792::process() Adding data 1934 [0] (rgb/left=1 depth/right=1)
[DEBUG] (2021-11-15 18:24:20.093) Rtabmap.cpp:3815::process() 
[DEBUG] (2021-11-15 18:24:20.094) Rtabmap.cpp:3884::process() wmState=666
[DEBUG] (2021-11-15 18:24:20.094) DBDriverSqlite3.cpp:4657::addStatisticsQuery() Ref ID = 1934
[DEBUG] (2021-11-15 18:24:20.095) DBDriverSqlite3.cpp:4730::addStatisticsQuery() Time=0.000635s
[DEBUG] (2021-11-15 18:24:20.095) Rtabmap.cpp:3894::process() Empty trash...
[DEBUG] (2021-11-15 18:24:20.095) DBDriver.cpp:315::emptyTrashes() Async emptying, start the trash thread
[DEBUG] (2021-11-15 18:24:20.095) DBDriver.cpp:327::emptyTrashes() signatures=1, visualWords=0
[DEBUG] (2021-11-15 18:24:20.095) DBDriver.cpp:293::beginTransaction() 
[DEBUG] (2021-11-15 18:24:20.095) Rtabmap.cpp:3971::process() End process, timeFinalizingStatistics=0.001550s
[DEBUG] (2021-11-15 18:24:20.095) DBDriverSqlite3.cpp:521::executeNoResultQuery() Time=0.000009s
[DEBUG] (2021-11-15 18:24:20.095) DBDriver.cpp:409::saveOrUpdate() 
[DEBUG] (2021-11-15 18:24:20.095) DBDriverSqlite3.cpp:4258::saveQuery() 
[DEBUG] (2021-11-15 18:24:20.095) CoreWrapper.cpp:4045::publishStats() Publishing stats...
[DEBUG] (2021-11-15 18:24:20.095) DBDriverSqlite3.cpp:5470::stepNode() Save node 1934
[DEBUG] (2021-11-15 18:24:20.095) DBDriverSqlite3.cpp:4286::saveQuery() Time=0.000145s
[DEBUG] (2021-11-15 18:24:20.095) DBDriverSqlite3.cpp:6290::stepLink() Save link from 1934 to 1923, type=2
[DEBUG] (2021-11-15 18:24:20.095) DBDriverSqlite3.cpp:6290::stepLink() Save link from 1934 to 1932, type=0
[DEBUG] (2021-11-15 18:24:20.095) DBDriverSqlite3.cpp:4314::saveQuery() Time=0.000079s
[DEBUG] (2021-11-15 18:24:20.095) DBDriverSqlite3.cpp:4348::saveQuery() Time=0.000060s
[DEBUG] (2021-11-15 18:24:20.095) DBDriverSqlite3.cpp:4368::saveQuery() Time=0.000028s
[DEBUG] (2021-11-15 18:24:20.095) DBDriverSqlite3.cpp:4377::saveQuery() Saving 1 images
[DEBUG] (2021-11-15 18:24:20.095) DBDriverSqlite3.cpp:5958::stepSensorData() Save sensor data 1934 (image=58360 depth=101234) depth2d = 0
[DEBUG] (2021-11-15 18:24:20.095) CameraModel.cpp:471::serialize() Header: 0 20 15 0 640 480 9 5 9 12 12
[DEBUG] (2021-11-15 18:24:20.095) DBDriverSqlite3.cpp:4396::saveQuery() Time=0.000243s
[DEBUG] (2021-11-15 18:24:20.095) DBDriverSqlite3.cpp:4437::saveQuery() Time=0.000015s
[DEBUG] (2021-11-15 18:24:20.095) DBDriver.cpp:355::emptyTrashes() Time emptying memory signatures trash = 0.000618...
[DEBUG] (2021-11-15 18:24:20.095) DBDriver.cpp:299::commit() 
[DEBUG] (2021-11-15 18:24:20.096) DBDriverSqlite3.cpp:521::executeNoResultQuery() Time=0.000203s
[DEBUG] (2021-11-15 18:24:20.096) DBDriver.cpp:377::emptyTrashes() Total time emptying trashes = 0.000941s...
[DEBUG] (2021-11-15 18:24:20.096) Memory.cpp:2474::getLastWorkingSignature() 
[DEBUG] (2021-11-15 18:24:20.096) Memory.cpp:2474::getLastWorkingSignature() 
[DEBUG] (2021-11-15 18:24:20.096) MapsManager.cpp:482::updateMapCaches() Updating map caches...
[DEBUG] (2021-11-15 18:24:20.097) FlannIndex.cpp:52::release() 
[DEBUG] (2021-11-15 18:24:20.097) FlannIndex.cpp:80::release() 
[DEBUG] (2021-11-15 18:24:20.097) FlannIndex.cpp:52::release() 
[DEBUG] (2021-11-15 18:24:20.097) FlannIndex.cpp:80::release() 
[ INFO] [1637022260.097134659, 1636862607.816274012]: rtabmap (1934): Rate=0.00s, Limit=0.000s, Conversion=0.0105s, RTAB-Map=0.5623s, Maps update=0.0018s pub=0.0001s (local map=666, WM=666)
[DEBUG] (2021-11-15 18:24:20.100) Rtabmap.cpp:1070::process() 
[ INFO] (2021-11-15 18:24:20.101) Rtabmap.cpp:1130::process() getting data...
[DEBUG] (2021-11-15 18:24:20.101) Rtabmap.cpp:1188::process() incremental=1 odomPose=xyz=6.494133,-2.053111,1.035221 rpy=-3.027038,0.674914,2.912659 optimizedPoses=666 mapCorrection=xyz=0.094648,0.242915,-0.057500 rpy=0.017194,-0.011380,-0.025043 lastLocalizationPose=xyz=6.518633,-1.986725,1.027154 rpy=-3.027722,0.692033,2.873248 lastLocalizationNodeId=1923
[DEBUG] (2021-11-15 18:24:20.101) Memory.cpp:2474::getLastWorkingSignature() 
[DEBUG] (2021-11-15 18:24:20.101) Memory.cpp:2474::getLastWorkingSignature() 
[ INFO] (2021-11-15 18:24:20.101) Rtabmap.cpp:1306::process() Updating memory...
[DEBUG] (2021-11-15 18:24:20.101) Memory.cpp:777::update() 
[DEBUG] (2021-11-15 18:24:20.101) Memory.cpp:786::update() pre-updating...
[DEBUG] (2021-11-15 18:24:20.101) Memory.cpp:5701::cleanUnusedWords() Removing 0 words (dictionary size=18336)...
[DEBUG] (2021-11-15 18:24:20.101) Memory.cpp:790::update() time preUpdate=0.025034 ms
[DEBUG] (2021-11-15 18:24:20.101) Memory.cpp:4336::createSignature() 
[DEBUG] (2021-11-15 18:24:20.101) Memory.cpp:4503::createSignature() Start dictionary update thread
[DEBUG] (2021-11-15 18:24:20.101) VWDictionary.cpp:470::update() incremental=1
[DEBUG] (2021-11-15 18:24:20.101) VWDictionary.cpp:685::update() Dictionary has not changed, so no need to update it! (size=0)
[DEBUG] (2021-11-15 18:24:20.101) VWDictionary.cpp:689::update() 
[DEBUG] (2021-11-15 18:24:20.101) Memory.cpp:4511::createSignature() Received kpts=0 kpts3D=0, descriptors=0 _useOdometryFeatures=false
[ INFO] (2021-11-15 18:24:20.101) Memory.cpp:4567::createSignature() Extract features
[DEBUG] (2021-11-15 18:24:20.101) Memory.cpp:4631::createSignature() rawDescriptorsKept=1, pose=1, maxFeatures=500, visMaxFeatures=1000
[DEBUG] (2021-11-15 18:24:20.101) Memory.cpp:4636::createSignature() Changing temporary max features from 500 to 1000
[DEBUG] (2021-11-15 18:24:20.103) util2d.cpp:1162::computeRoi() roi ratios = 0.000000, 0.000000, 0.000000, 0.000000
[DEBUG] (2021-11-15 18:24:20.103) util2d.cpp:1163::computeRoi() roi = 0, 0, 640, 480
[DEBUG] (2021-11-15 18:24:20.103) util2d.cpp:1190::computeRoi() roi = 0, 0, 640, 480
[DEBUG] (2021-11-15 18:24:20.124) Features2d.cpp:751::generateKeypoints() Keypoints extraction time = 0.020733 s, keypoints extracted = 56 (grid=1x1, mask empty=0)
[DEBUG] (2021-11-15 18:24:20.124) Memory.cpp:4652::createSignature() time keypoints (56) = 0.023335s
[DEBUG] (2021-11-15 18:24:20.125) Features2d.cpp:783::generateDescriptors() Descriptors extracted = 56, remaining kpts=56
[DEBUG] (2021-11-15 18:24:20.125) Memory.cpp:4658::createSignature() time descriptors (56) = 0.000542s
[DEBUG] (2021-11-15 18:24:20.125) Memory.cpp:4660::createSignature() ratio=0.500000, meanWordsPerLocation=104
[DEBUG] (2021-11-15 18:24:20.125) Memory.cpp:4806::createSignature() time keypoints 3D (56) = 0.000052s
[DEBUG] (2021-11-15 18:24:20.125) Memory.cpp:4889::createSignature() Joining dictionary update thread...
[DEBUG] (2021-11-15 18:24:20.125) Memory.cpp:4891::createSignature() Joining dictionary update thread... thread finished!
[DEBUG] (2021-11-15 18:24:20.125) Memory.cpp:4898::createSignature() time descriptor and memory update (56 of size=256) = 0.000025s
[DEBUG] (2021-11-15 18:24:20.125) VWDictionary.cpp:763::addNewWords() id=1935 descriptors=56
[DEBUG] (2021-11-15 18:24:20.125) VWDictionary.cpp:857::addNewWords() newPts.total()=56 
[DEBUG] (2021-11-15 18:24:20.126) VWDictionary.cpp:924::addNewWords() Time to find nn = 0.001693 s
[DEBUG] (2021-11-15 18:24:20.127) VWDictionary.cpp:1059::addNewWords() naive search and add ref/words time = 0.000447 s
[DEBUG] (2021-11-15 18:24:20.127) VWDictionary.cpp:1061::addNewWords() 32 new words added...
[DEBUG] (2021-11-15 18:24:20.127) VWDictionary.cpp:1063::addNewWords() 24 duplicated words added (from current image = 5)...
[DEBUG] (2021-11-15 18:24:20.127) VWDictionary.cpp:1064::addNewWords() total time 0.002176s
[DEBUG] (2021-11-15 18:24:20.127) Memory.cpp:4987::createSignature() time addNewWords 0.002204s indexed=18336 not=32
[DEBUG] (2021-11-15 18:24:20.127) Memory.cpp:5038::createSignature() Detecting markers...
[DEBUG] (2021-11-15 18:24:20.131) MarkerDetector.cpp:153::detect() Markers detected=0 rejected=0
[DEBUG] (2021-11-15 18:24:20.131) Memory.cpp:5074::createSignature() Markers detected = 0
[DEBUG] (2021-11-15 18:24:20.131) Memory.cpp:5082::createSignature() time markers detection = 0.004113s
[DEBUG] (2021-11-15 18:24:20.131) Memory.cpp:5313::createSignature() Bin data kept: rgb=1, depth=1, scan=0, userData=0
[DEBUG] (2021-11-15 18:24:20.139) Memory.cpp:5520::createSignature() data.groundTruth()      =xyz=[null] rpy=[null]
[DEBUG] (2021-11-15 18:24:20.139) Memory.cpp:5521::createSignature() data.gps()              =false
[DEBUG] (2021-11-15 18:24:20.139) Memory.cpp:5522::createSignature() data.envSensors()       =0
[DEBUG] (2021-11-15 18:24:20.139) Memory.cpp:5523::createSignature() data.globalDescriptors()=0
[DEBUG] (2021-11-15 18:24:20.139) Memory.cpp:5531::createSignature() time compressing data (id=1935) 0.007999s
[DEBUG] (2021-11-15 18:24:20.139) OccupancyGrid.cpp:290::createLocalMap() scan format=NA, occupancySensor_=1 normalsSegmentation_=1 grid3D_=1
[DEBUG] (2021-11-15 18:24:20.139) OccupancyGrid.cpp:388::createLocalMap() Depth image : decimation=4 max=5.000000 min=0.000000
[DEBUG] (2021-11-15 18:24:20.139) util3d.cpp:1051::cloudRGBFromSensorData() 
[DEBUG] (2021-11-15 18:24:20.139) util3d.cpp:443::cloudFromDepthRGB() 
[DEBUG] (2021-11-15 18:24:20.139) util3d.cpp:554::cloudFromDepthRGB() rgb=640x480 depth=640x480 fx=616.338928 fy=616.474487 cx=319.256104 cy=239.730392 (depth factors=1.000000 1.000000) decimation=4
[DEBUG] (2021-11-15 18:24:20.140) util3d.cpp:607::cloudFromDepthRGB() 
[DEBUG] (2021-11-15 18:24:20.141) OccupancyGrid.hpp:88::segmentCloud() node.getPose()=xyz=6.494133,-2.053111,1.035221 rpy=-3.027038,0.674914,2.912659 projMapFrame_=0
[DEBUG] (2021-11-15 18:24:20.141) OccupancyGrid.hpp:124::segmentCloud() normalKSearch=20
[DEBUG] (2021-11-15 18:24:20.141) OccupancyGrid.hpp:125::segmentCloud() maxGroundAngle=0.785398
[DEBUG] (2021-11-15 18:24:20.141) OccupancyGrid.hpp:126::segmentCloud() Cluster radius=0.100000
[DEBUG] (2021-11-15 18:24:20.141) OccupancyGrid.hpp:127::segmentCloud() flatObstaclesDetected=1
[DEBUG] (2021-11-15 18:24:20.141) OccupancyGrid.hpp:128::segmentCloud() maxGroundHeight=0.000000
[DEBUG] (2021-11-15 18:24:20.142) OccupancyGrid.hpp:142::segmentCloud() viewPoint=-0.039954,0.004362,-0.027062
[DEBUG] (2021-11-15 18:24:20.142) OccupancyGrid.hpp:164::segmentCloud() groundIndices=0 obstaclesIndices=161
[DEBUG] (2021-11-15 18:24:20.142) OccupancyGrid.cpp:523::createLocalMap() groundIndices=0, obstaclesIndices=161
[DEBUG] (2021-11-15 18:24:20.143) OccupancyGrid.cpp:567::createLocalMap() ground=0 obstacles=161
[DEBUG] (2021-11-15 18:24:20.143) OccupancyGrid.cpp:654::createLocalMap() ground=0 obstacles=161 empty=0, channels=4
[DEBUG] (2021-11-15 18:24:20.143) SensorData.cpp:407::setOccupancyGrid() ground=0 obstacles=161 empty=0
[DEBUG] (2021-11-15 18:24:20.143) Memory.cpp:5552::createSignature() time grid map = 0.003881s
[DEBUG] (2021-11-15 18:24:20.143) Memory.cpp:808::update() time creating signature=42.227028 ms
[DEBUG] (2021-11-15 18:24:20.143) Memory.cpp:893::addSignatureToStm() adding 1935 (pose=xyz=6.494133,-2.053111,1.035221 rpy=-3.027038,0.674914,2.912659)
[DEBUG] (2021-11-15 18:24:20.143) Signature.cpp:121::addLink() Add link 1935 to 1934 (type=0 var=0.001000,0.001000)
[DEBUG] (2021-11-15 18:24:20.143) Signature.cpp:121::addLink() Add link 1934 to 1935 (type=0 var=0.001000,0.001000)
[DEBUG] (2021-11-15 18:24:20.143) Memory.cpp:974::addSignatureToStm() Min STM id = 1924
[DEBUG] (2021-11-15 18:24:20.143) Memory.cpp:1016::addSignatureToStm() 56 words ref for the signature 1935 (weight=0)
[DEBUG] (2021-11-15 18:24:20.143) Memory.cpp:1024::addSignatureToStm() time = 0.000085s
[DEBUG] (2021-11-15 18:24:20.143) Memory.cpp:3608::rehearsal() Comparing with signature (1934)...
[DEBUG] (2021-11-15 18:24:20.143) Memory.cpp:3631::rehearsal() merged=0, sim=0.000000 t=0.000013s
[DEBUG] (2021-11-15 18:24:20.143) Memory.cpp:829::update() time rehearsal=0.142813 ms
[DEBUG] (2021-11-15 18:24:20.143) Memory.cpp:1047::moveSignatureToWMFromSTM() Inserting node 1924 from STM in WM...
[DEBUG] (2021-11-15 18:24:20.143) Memory.cpp:882::update() totalTimer = 0.042424s
[DEBUG] (2021-11-15 18:24:20.143) Memory.cpp:2474::getLastWorkingSignature() 
[ INFO] (2021-11-15 18:24:20.143) Rtabmap.cpp:1329::process() Processing signature 1935 w=0 map=0
[ INFO] (2021-11-15 18:24:20.143) Rtabmap.cpp:1331::process() timeMemoryUpdate=0.042547s
[DEBUG] (2021-11-15 18:24:20.143) Rtabmap.cpp:1507::process() Added pose xyz=6.523103,-1.989360,1.016107 rpy=-3.051934,0.681946,2.872089 (odom=xyz=6.494133,-2.053111,1.035221 rpy=-3.027038,0.674914,2.912659)
[ INFO] (2021-11-15 18:24:20.143) Rtabmap.cpp:1696::process() timeProximityByTimeDetection=0.000046s
[ INFO] (2021-11-15 18:24:20.143) Rtabmap.cpp:1715::process() computing likelihood...
[DEBUG] (2021-11-15 18:24:20.144) Memory.cpp:1874::computeLikelihood() processing... 
[DEBUG] (2021-11-15 18:24:20.144) Memory.cpp:1911::computeLikelihood() compute likelihood (tf-idf) 0.000188 s
[DEBUG] (2021-11-15 18:24:20.144) Rtabmap.cpp:4480::adjustLikelihood() likelihood.size()=658
[DEBUG] (2021-11-15 18:24:20.144) Rtabmap.cpp:4499::adjustLikelihood() values.size=31
[DEBUG] (2021-11-15 18:24:20.144) Rtabmap.cpp:4546::adjustLikelihood() mean=0.029455, stdDev=0.023387, max=0.126155, maxId=1825, time=0.000035s
[ INFO] (2021-11-15 18:24:20.144) Rtabmap.cpp:1833::process() timeLikelihoodCalculation=0.000676s
[ INFO] (2021-11-15 18:24:20.144) Rtabmap.cpp:1839::process() getting posterior...
[DEBUG] (2021-11-15 18:24:20.144) BayesFilter.cpp:147::computePosterior() 
[DEBUG] (2021-11-15 18:24:20.144) BayesFilter.cpp:507::updatePrediction() 
[DEBUG] (2021-11-15 18:24:20.144) BayesFilter.cpp:516::updatePrediction() time creating prediction = 0.000151s
[DEBUG] (2021-11-15 18:24:20.144) BayesFilter.cpp:524::updatePrediction() time creating old ids set = 0.000119s
[DEBUG] (2021-11-15 18:24:20.144) BayesFilter.cpp:540::updatePrediction() time creating id-index vector (size=657 oldIds.back()=1922 newIds.back()=1924) = 0.000117s
[DEBUG] (2021-11-15 18:24:20.144) BayesFilter.cpp:553::updatePrediction() time getting removed ids = 0.000033s
[DEBUG] (2021-11-15 18:24:20.145) BayesFilter.cpp:562::updatePrediction() Copied all old prediction: = 0.000262s
[DEBUG] (2021-11-15 18:24:20.145) BayesFilter.cpp:621::updatePrediction() From added id 1923, 16 neighbors to update.
[DEBUG] (2021-11-15 18:24:20.145) BayesFilter.cpp:621::updatePrediction() From added id 1924, 15 neighbors to update.
[DEBUG] (2021-11-15 18:24:20.145) BayesFilter.cpp:624::updatePrediction() time getting 16 ids to update = 0.000161s
[DEBUG] (2021-11-15 18:24:20.145) BayesFilter.cpp:652::updatePrediction() time updating modified/added 16 ids = 0.000096s (e0=0.000001 e1=0.000004 e2=0.000000 e3=0.000045 e4=0.000031)
[DEBUG] (2021-11-15 18:24:20.145) BayesFilter.cpp:706::updatePrediction() time updating virtual place = 0.000022s
[DEBUG] (2021-11-15 18:24:20.145) BayesFilter.cpp:708::updatePrediction() Modified=16, Added=2, Copied=0
[DEBUG] (2021-11-15 18:24:20.145) BayesFilter.cpp:179::computePosterior() STEP1-generate prior=0.001161s, rows=658, cols=658
[DEBUG] (2021-11-15 18:24:20.145) BayesFilter.cpp:714::updatePosterior() 
[DEBUG] (2021-11-15 18:24:20.145) BayesFilter.cpp:191::computePosterior() STEP1-update posterior=0.000000s, posterior=658, _posterior size=658
[DEBUG] (2021-11-15 18:24:20.146) BayesFilter.cpp:197::computePosterior() STEP1-matrix mult time=0.000832s
[DEBUG] (2021-11-15 18:24:20.146) BayesFilter.cpp:200::computePosterior() STEP1-matrix mult time=0.000016s
[DEBUG] (2021-11-15 18:24:20.146) BayesFilter.cpp:219::computePosterior() STEP2-likelihood time=0.000109s
[DEBUG] (2021-11-15 18:24:20.146) BayesFilter.cpp:223::computePosterior() sum=2.011281
[DEBUG] (2021-11-15 18:24:20.146) BayesFilter.cpp:231::computePosterior() normalize time=0.000038s
[ INFO] (2021-11-15 18:24:20.146) Rtabmap.cpp:1844::process() timePosteriorCalculation=0.002268s
[ INFO] (2021-11-15 18:24:20.146) Rtabmap.cpp:1855::process() creating hypotheses...
[ INFO] (2021-11-15 18:24:20.146) Rtabmap.cpp:1869::process() Highest hypothesis=1825, value=0.098844, timeHypothesesCreation=0.000233s
[DEBUG] (2021-11-15 18:24:20.146) Memory.cpp:2155::joinTrashThread() 
[DEBUG] (2021-11-15 18:24:20.146) Memory.cpp:2157::joinTrashThread() 
[ INFO] (2021-11-15 18:24:20.146) Rtabmap.cpp:1944::process() Time emptying memory trash = 0.000941s,  joining (actual overhead) = 0.000066s
[ INFO] (2021-11-15 18:24:20.146) Rtabmap.cpp:2329::process() timeReactivations=0.000011s
[DEBUG] (2021-11-15 18:24:20.146) Rtabmap.cpp:2408::process() Proximity detection (local loop closure in SPACE using matching images, local radius=5.000000m)
[DEBUG] (2021-11-15 18:24:20.147) Rtabmap.cpp:2418::process() nearestIds=79/667
[DEBUG] (2021-11-15 18:24:20.147) Rtabmap.cpp:2427::process() nearestPoses=69
[DEBUG] (2021-11-15 18:24:20.147) Rtabmap.cpp:4348::getPaths() e0=0.000000s e1=0.000109s e2=0.000081s e3=0.000069s e4=0.000009s
[DEBUG] (2021-11-15 18:24:20.147) Rtabmap.cpp:2431::process() got 2 paths
[DEBUG] (2021-11-15 18:24:20.147) Rtabmap.cpp:2450::process() nearestPaths=2 proximityMaxPaths=3
[DEBUG] (2021-11-15 18:24:20.147) Graph.cpp:2265::getPosesInRadius() found nodes=0
[DEBUG] (2021-11-15 18:24:20.147) Graph.cpp:2265::getPosesInRadius() found nodes=45
[DEBUG] (2021-11-15 18:24:20.147) Memory.cpp:2730::computeTransform() 
[DEBUG] (2021-11-15 18:24:20.147) DBDriverSqlite3.cpp:1297::loadNodeDataQuery() load data for 1 signatures images=1 scan=1 userData=1, grid=1
[DEBUG] (2021-11-15 18:24:20.147) DBDriverSqlite3.cpp:1437::loadNodeDataQuery() Loading data for 1924...
[DEBUG] (2021-11-15 18:24:20.147) CameraModel.cpp:528::deserialize() Header: 0 20 15 0 640 480 9 5 9 12 12
[DEBUG] (2021-11-15 18:24:20.147) SensorData.cpp:407::setOccupancyGrid() ground=0 obstacles=2533 empty=0
[DEBUG] (2021-11-15 18:24:20.147) DBDriverSqlite3.cpp:1846::loadNodeDataQuery() Time=0.000284s
[DEBUG] (2021-11-15 18:24:20.148) SensorData.cpp:506::uncompressData() 1924 data(1,1,0,0,0,0,0)
[DEBUG] (2021-11-15 18:24:20.158) SensorData.cpp:506::uncompressData() 1935 data(1,1,0,0,0,0,0)
[DEBUG] (2021-11-15 18:24:20.158) Memory.cpp:2773::computeTransform() 
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:282::computeTransformationImpl() Vis/MinInliers=10
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:283::computeTransformationImpl() Vis/InlierDistance=0.010000
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:284::computeTransformationImpl() Vis/Iterations=300
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:285::computeTransformationImpl() Vis/EstimationType=1
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:286::computeTransformationImpl() Vis/ForwardEstOnly=0
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:287::computeTransformationImpl() Vis/EpipolarGeometryVar=0.100000
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:288::computeTransformationImpl() Vis/PnPReprojError=2.000000
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:289::computeTransformationImpl() Vis/PnPFlags=0
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:290::computeTransformationImpl() Vis/CorType=0
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:291::computeTransformationImpl() Vis/CorFlowWinSize=16
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:292::computeTransformationImpl() Vis/CorFlowIterations=30
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:293::computeTransformationImpl() Vis/CorFlowEps=0.010000
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:294::computeTransformationImpl() Vis/CorFlowMaxLevel=3
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:295::computeTransformationImpl() Vis/CorNNDR=0.800000
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:296::computeTransformationImpl() Vis/CorNNType=1
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:297::computeTransformationImpl() Vis/CorGuessWinSize=20
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:298::computeTransformationImpl() Vis/CorGuessMatchToProjection=0
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:299::computeTransformationImpl() Feature Detector = 6
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:300::computeTransformationImpl() guess=xyz=[null] rpy=[null]
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:313::computeTransformationImpl() Input(1924): from=0 words, 0 3D words, 0 words descriptors,  0 kpts, 0 kpts3D, 0 descriptors, image=640x480 models=1 stereo=0
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:326::computeTransformationImpl() Input(1935): to=0 words, 0 3D words, 0 words descriptors, 0 kpts, 0 kpts3D, 0 descriptors, image=640x480 models=1 stereo=0
[DEBUG] (2021-11-15 18:24:20.158) RegistrationVis.cpp:346::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:20.161) util2d.cpp:1162::computeRoi() roi ratios = 0.000000, 0.000000, 0.000000, 0.000000
[DEBUG] (2021-11-15 18:24:20.161) util2d.cpp:1163::computeRoi() roi = 0, 0, 640, 480
[DEBUG] (2021-11-15 18:24:20.161) util2d.cpp:1190::computeRoi() roi = 0, 0, 640, 480
[DEBUG] (2021-11-15 18:24:20.176) Features2d.cpp:751::generateKeypoints() Keypoints extraction time = 0.015175 s, keypoints extracted = 1000 (grid=1x1, mask empty=0)
[DEBUG] (2021-11-15 18:24:20.176) RegistrationVis.cpp:599::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:20.179) util2d.cpp:1162::computeRoi() roi ratios = 0.000000, 0.000000, 0.000000, 0.000000
[DEBUG] (2021-11-15 18:24:20.179) util2d.cpp:1163::computeRoi() roi = 0, 0, 640, 480
[DEBUG] (2021-11-15 18:24:20.179) util2d.cpp:1190::computeRoi() roi = 0, 0, 640, 480
[DEBUG] (2021-11-15 18:24:20.188) Features2d.cpp:751::generateKeypoints() Keypoints extraction time = 0.008955 s, keypoints extracted = 1000 (grid=1x1, mask empty=0)
[DEBUG] (2021-11-15 18:24:20.188) RegistrationVis.cpp:649::computeTransformationImpl() kptsFrom=1000 kptsFromSource=0
[DEBUG] (2021-11-15 18:24:20.188) RegistrationVis.cpp:650::computeTransformationImpl() kptsTo=1000 kptsToSource=0
[DEBUG] (2021-11-15 18:24:20.188) RegistrationVis.cpp:672::computeTransformationImpl() cleared orignalWordsFromIds
[DEBUG] (2021-11-15 18:24:20.192) Features2d.cpp:783::generateDescriptors() Descriptors extracted = 824, remaining kpts=824
[DEBUG] (2021-11-15 18:24:20.195) Features2d.cpp:783::generateDescriptors() Descriptors extracted = 803, remaining kpts=803
[DEBUG] (2021-11-15 18:24:20.195) RegistrationVis.cpp:733::computeTransformationImpl() generated kptsFrom3D=824
[DEBUG] (2021-11-15 18:24:20.195) Features2d.cpp:164::filterKeypointsByDepth() 
[DEBUG] (2021-11-15 18:24:20.196) Features2d.cpp:193::filterKeypointsByDepth() Removed 0 invalid 3D points
[DEBUG] (2021-11-15 18:24:20.196) Features2d.cpp:164::filterKeypointsByDepth() 
[DEBUG] (2021-11-15 18:24:20.196) Features2d.cpp:193::filterKeypointsByDepth() Removed 0 invalid 3D points
[DEBUG] (2021-11-15 18:24:20.196) RegistrationVis.cpp:783::computeTransformationImpl() descriptorsFrom=824
[DEBUG] (2021-11-15 18:24:20.196) RegistrationVis.cpp:784::computeTransformationImpl() descriptorsTo=803
[DEBUG] (2021-11-15 18:24:20.196) RegistrationVis.cpp:785::computeTransformationImpl() orignalWordsFromIds=0
[DEBUG] (2021-11-15 18:24:20.196) RegistrationVis.cpp:1165::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:20.196) RegistrationVis.cpp:1265::computeTransformationImpl() VWDictionary knn matching
[DEBUG] (2021-11-15 18:24:20.196) VWDictionary.cpp:470::update() incremental=1
[DEBUG] (2021-11-15 18:24:20.196) FlannIndex.cpp:52::release() 
[DEBUG] (2021-11-15 18:24:20.196) FlannIndex.cpp:80::release() 
[DEBUG] (2021-11-15 18:24:20.196) VWDictionary.cpp:681::update() Dictionary updated! (size=0 added=0 removed=0)
[DEBUG] (2021-11-15 18:24:20.196) VWDictionary.cpp:689::update() 
[DEBUG] (2021-11-15 18:24:20.196) VWDictionary.cpp:763::addNewWords() id=1 descriptors=824
[DEBUG] (2021-11-15 18:24:20.197) VWDictionary.cpp:1059::addNewWords() naive search and add ref/words time = 0.000969 s
[DEBUG] (2021-11-15 18:24:20.197) VWDictionary.cpp:1061::addNewWords() 824 new words added...
[DEBUG] (2021-11-15 18:24:20.197) VWDictionary.cpp:1063::addNewWords() 0 duplicated words added (from current image = 0)...
[DEBUG] (2021-11-15 18:24:20.197) VWDictionary.cpp:1064::addNewWords() total time 0.001150s
[DEBUG] (2021-11-15 18:24:20.197) VWDictionary.cpp:470::update() incremental=1
[DEBUG] (2021-11-15 18:24:20.197) VWDictionary.cpp:491::update() Incremental FLANN: Removing 0 words...
[DEBUG] (2021-11-15 18:24:20.197) VWDictionary.cpp:500::update() Incremental FLANN: Removing 0 words... done!
[DEBUG] (2021-11-15 18:24:20.197) VWDictionary.cpp:504::update() Incremental FLANN: Inserting 824 words...
[DEBUG] (2021-11-15 18:24:20.197) VWDictionary.cpp:531::update() Building FLANN index...
[DEBUG] (2021-11-15 18:24:20.197) FlannIndex.cpp:208::buildKDTreeIndex() 
[DEBUG] (2021-11-15 18:24:20.197) FlannIndex.cpp:52::release() 
[DEBUG] (2021-11-15 18:24:20.197) FlannIndex.cpp:80::release() 
[DEBUG] (2021-11-15 18:24:20.197) FlannIndex.cpp:260::buildKDTreeIndex() 
[DEBUG] (2021-11-15 18:24:20.197) VWDictionary.cpp:549::update() Building FLANN index... done!
[DEBUG] (2021-11-15 18:24:20.197) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 1 -> 3
[DEBUG] (2021-11-15 18:24:20.197) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 3 -> 7
[DEBUG] (2021-11-15 18:24:20.197) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 7 -> 15
[DEBUG] (2021-11-15 18:24:20.198) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 15 -> 31
[DEBUG] (2021-11-15 18:24:20.198) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 31 -> 63
[DEBUG] (2021-11-15 18:24:20.199) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 63 -> 127
[DEBUG] (2021-11-15 18:24:20.200) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 127 -> 255
[DEBUG] (2021-11-15 18:24:20.203) FlannIndex.cpp:405::addPoints() Rebuilding FLANN index: 255 -> 511
[DEBUG] (2021-11-15 18:24:20.208) VWDictionary.cpp:564::update() Incremental FLANN: Inserting 824 words... done!
[DEBUG] (2021-11-15 18:24:20.208) VWDictionary.cpp:681::update() Dictionary updated! (size=0 added=824 removed=0)
[DEBUG] (2021-11-15 18:24:20.208) VWDictionary.cpp:689::update() 
[DEBUG] (2021-11-15 18:24:20.208) VWDictionary.cpp:763::addNewWords() id=2 descriptors=803
[DEBUG] (2021-11-15 18:24:20.208) VWDictionary.cpp:857::addNewWords() newPts.total()=803 
[DEBUG] (2021-11-15 18:24:20.223) VWDictionary.cpp:924::addNewWords() Time to find nn = 0.014305 s
[DEBUG] (2021-11-15 18:24:20.224) VWDictionary.cpp:1059::addNewWords() naive search and add ref/words time = 0.001090 s
[DEBUG] (2021-11-15 18:24:20.224) VWDictionary.cpp:1061::addNewWords() 596 new words added...
[DEBUG] (2021-11-15 18:24:20.224) VWDictionary.cpp:1063::addNewWords() 207 duplicated words added (from current image = 0)...
[DEBUG] (2021-11-15 18:24:20.224) VWDictionary.cpp:1064::addNewWords() total time 0.015590s
[DEBUG] (2021-11-15 18:24:20.224) VWDictionary.cpp:694::clear() 
[DEBUG] (2021-11-15 18:24:20.224) FlannIndex.cpp:52::release() 
[DEBUG] (2021-11-15 18:24:20.224) FlannIndex.cpp:80::release() 
[DEBUG] (2021-11-15 18:24:20.224) VWDictionary.cpp:694::clear() 
[DEBUG] (2021-11-15 18:24:20.224) FlannIndex.cpp:52::release() 
[DEBUG] (2021-11-15 18:24:20.224) FlannIndex.cpp:80::release() 
[DEBUG] (2021-11-15 18:24:20.224) FlannIndex.cpp:52::release() 
[DEBUG] (2021-11-15 18:24:20.224) FlannIndex.cpp:80::release() 
[DEBUG] (2021-11-15 18:24:20.226) RegistrationVis.cpp:1481::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:20.226) RegistrationVis.cpp:1494::computeTransformationImpl() words from3D=824 to2D=720
[DEBUG] (2021-11-15 18:24:20.227) util3d_motion_estimation.cpp:99::estimateMotion3DTo2D() words3A=824 words2B=720 matches=124 words3B=720 guess=xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000 reprojError=2.000000 iterations=300
[DEBUG] (2021-11-15 18:24:20.252) RegistrationVis.cpp:1537::computeTransformationImpl() inliers: 18/124
[DEBUG] (2021-11-15 18:24:20.252) RegistrationVis.cpp:1481::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:20.252) RegistrationVis.cpp:1494::computeTransformationImpl() words from3D=720 to2D=824
[DEBUG] (2021-11-15 18:24:20.253) util3d_motion_estimation.cpp:99::estimateMotion3DTo2D() words3A=720 words2B=824 matches=124 words3B=824 guess=xyz=-0.001031,-0.000948,-0.091701 rpy=0.034841,-0.007732,0.010345 reprojError=2.000000 iterations=300
[DEBUG] (2021-11-15 18:24:20.278) RegistrationVis.cpp:1537::computeTransformationImpl() inliers: 25/124
[DEBUG] (2021-11-15 18:24:20.279) RegistrationVis.cpp:1614::computeTransformationImpl() from->to=xyz=0.001750,0.004131,0.091602 rpy=-0.034921,0.007366,-0.010609
[DEBUG] (2021-11-15 18:24:20.279) RegistrationVis.cpp:1615::computeTransformationImpl() to->from=xyz=-0.012280,-0.020108,-0.105167 rpy=-0.002371,0.002132,-0.030400
[DEBUG] (2021-11-15 18:24:20.279) RegistrationVis.cpp:1658::computeTransformationImpl() Refine with bundle adjustment
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1263::optimizeBA() Optimizing graph...
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1343::optimizeBA() fill poses to g2o...
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1396::optimizeBA() fill edges to g2o...
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1490::optimizeBA() fill 3D points to g2o...
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1497::optimizeBA() stepVertexId=3, negVertexOffset=759
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1623::optimizeBA() Initial optimization...
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1628::optimizeBA() g2o optimizing begin (max iterations=20, robustKernel=8.000000)
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1648::optimizeBA() iteration 0: 37 nodes, 72 edges, chi2: 411.785279
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (99<->1) d=84.710419 var=1.000000 kernel=8.000000 chi2=16.364072
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (99<->2) d=108.446716 var=1.000000 kernel=8.000000 chi2=10.361190
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (172<->1) d=98.221329 var=1.000000 kernel=8.000000 chi2=11.334711
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (209<->1) d=108.050659 var=1.000000 kernel=8.000000 chi2=11.396681
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (233<->1) d=81.923645 var=1.000000 kernel=8.000000 chi2=22.477567
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (233<->2) d=102.808807 var=1.000000 kernel=8.000000 chi2=14.846019
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (340<->1) d=70.762207 var=1.000000 kernel=8.000000 chi2=10.296170
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (390<->1) d=67.267548 var=1.000000 kernel=8.000000 chi2=14.968169
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (390<->2) d=79.940186 var=1.000000 kernel=8.000000 chi2=10.278909
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (527<->1) d=93.704132 var=1.000000 kernel=8.000000 chi2=19.059515
[DEBUG] (2021-11-15 18:24:20.279) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (527<->2) d=124.136719 var=1.000000 kernel=8.000000 chi2=12.268537
[DEBUG] (2021-11-15 18:24:20.280) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (544<->1) d=73.534180 var=1.000000 kernel=8.000000 chi2=8.910424
[DEBUG] (2021-11-15 18:24:20.280) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (544<->2) d=85.602600 var=1.000000 kernel=8.000000 chi2=8.806442
[DEBUG] (2021-11-15 18:24:20.280) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (656<->1) d=67.580994 var=1.000000 kernel=8.000000 chi2=16.582933
[DEBUG] (2021-11-15 18:24:20.280) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (656<->2) d=84.700714 var=1.000000 kernel=8.000000 chi2=12.391432
[DEBUG] (2021-11-15 18:24:20.280) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (738<->1) d=100.435455 var=1.000000 kernel=8.000000 chi2=10.745253
[DEBUG] (2021-11-15 18:24:20.280) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (756<->1) d=66.471527 var=1.000000 kernel=8.000000 chi2=17.163415
[DEBUG] (2021-11-15 18:24:20.280) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (756<->2) d=80.148102 var=1.000000 kernel=8.000000 chi2=11.819285
[DEBUG] (2021-11-15 18:24:20.280) OptimizerG2O.cpp:1703::optimizeBA() outliers=18 outliersCountFar=0
[DEBUG] (2021-11-15 18:24:20.280) OptimizerG2O.cpp:1648::optimizeBA() iteration 1: 37 nodes, 72 edges, chi2: 143.402025
[DEBUG] (2021-11-15 18:24:20.280) OptimizerG2O.cpp:1703::optimizeBA() outliers=18 outliersCountFar=0
[DEBUG] (2021-11-15 18:24:20.280) OptimizerG2O.cpp:1706::optimizeBA() g2o optimizing end (25 iterations done, error=143.402025, outliers=18/70 (delta=8.000000) time = 0.001415 s)
[DEBUG] (2021-11-15 18:24:20.280) OptimizerG2O.cpp:1801::optimizeBA() Optimizing graph...end!
[DEBUG] (2021-11-15 18:24:20.280) RegistrationVis.cpp:1800::computeTransformationImpl() Pose optimization: xyz=0.001750,0.004131,0.091602 rpy=-0.034921,0.007366,-0.010609 -> xyz=0.008410,0.013187,0.118327 rpy=-0.021135,0.009946,0.022947
[DEBUG] (2021-11-15 18:24:20.280) RegistrationVis.cpp:1814::computeTransformationImpl() BA outliers ratio 0.314286
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:1981::computeTransformationImpl() inliers=24/124
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:1982::computeTransformationImpl() transform=xyz=0.008410,0.013187,0.118327 rpy=-0.021135,0.009946,0.022947
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:282::computeTransformationImpl() Vis/MinInliers=10
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:283::computeTransformationImpl() Vis/InlierDistance=0.010000
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:284::computeTransformationImpl() Vis/Iterations=300
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:285::computeTransformationImpl() Vis/EstimationType=1
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:286::computeTransformationImpl() Vis/ForwardEstOnly=0
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:287::computeTransformationImpl() Vis/EpipolarGeometryVar=0.100000
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:288::computeTransformationImpl() Vis/PnPReprojError=2.000000
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:289::computeTransformationImpl() Vis/PnPFlags=0
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:290::computeTransformationImpl() Vis/CorType=0
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:291::computeTransformationImpl() Vis/CorFlowWinSize=16
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:292::computeTransformationImpl() Vis/CorFlowIterations=30
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:293::computeTransformationImpl() Vis/CorFlowEps=0.010000
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:294::computeTransformationImpl() Vis/CorFlowMaxLevel=3
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:295::computeTransformationImpl() Vis/CorNNDR=0.800000
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:296::computeTransformationImpl() Vis/CorNNType=1
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:297::computeTransformationImpl() Vis/CorGuessWinSize=20
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:298::computeTransformationImpl() Vis/CorGuessMatchToProjection=0
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:299::computeTransformationImpl() Feature Detector = 6
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:300::computeTransformationImpl() guess=xyz=0.008410,0.013187,0.118327 rpy=-0.021135,0.009946,0.022947
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:313::computeTransformationImpl() Input(1924): from=824 words, 824 3D words, 824 words descriptors,  824 kpts, 824 kpts3D, 824 descriptors, image=640x480 models=1 stereo=0
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:326::computeTransformationImpl() Input(1935): to=720 words, 720 3D words, 720 words descriptors, 803 kpts, 803 kpts3D, 803 descriptors, image=640x480 models=1 stereo=0
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:346::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:599::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:649::computeTransformationImpl() kptsFrom=824 kptsFromSource=2
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:650::computeTransformationImpl() kptsTo=720 kptsToSource=2
[DEBUG] (2021-11-15 18:24:20.281) Features2d.cpp:164::filterKeypointsByDepth() 
[DEBUG] (2021-11-15 18:24:20.281) Features2d.cpp:193::filterKeypointsByDepth() Removed 0 invalid 3D points
[DEBUG] (2021-11-15 18:24:20.281) Features2d.cpp:164::filterKeypointsByDepth() 
[DEBUG] (2021-11-15 18:24:20.281) Features2d.cpp:193::filterKeypointsByDepth() Removed 0 invalid 3D points
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:783::computeTransformationImpl() descriptorsFrom=824
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:784::computeTransformationImpl() descriptorsTo=720
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:785::computeTransformationImpl() orignalWordsFromIds=824
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:806::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:828::computeTransformationImpl() Projected points=824
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:846::computeTransformationImpl() corners in frame=476
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:851::computeTransformationImpl() guessMatchToProjection=0, cornersProjected=476
[DEBUG] (2021-11-15 18:24:20.281) RegistrationVis.cpp:998::computeTransformationImpl() match projected to frame
[DEBUG] (2021-11-15 18:24:20.286) RegistrationVis.cpp:1016::computeTransformationImpl() radius search done for guess
[DEBUG] (2021-11-15 18:24:20.287) RegistrationVis.cpp:1108::computeTransformationImpl() bruteForceDescCopy=0.000458s, bruteForceTotalTime=0.000881s
[DEBUG] (2021-11-15 18:24:20.288) RegistrationVis.cpp:1144::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:20.288) RegistrationVis.cpp:1481::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:20.288) RegistrationVis.cpp:1494::computeTransformationImpl() words from3D=824 to2D=720
[DEBUG] (2021-11-15 18:24:20.289) util3d_motion_estimation.cpp:99::estimateMotion3DTo2D() words3A=824 words2B=720 matches=200 words3B=720 guess=xyz=0.008410,0.013187,0.118327 rpy=-0.021135,0.009946,0.022947 reprojError=2.000000 iterations=300
[DEBUG] (2021-11-15 18:24:20.315) RegistrationVis.cpp:1537::computeTransformationImpl() inliers: 47/200
[DEBUG] (2021-11-15 18:24:20.315) RegistrationVis.cpp:1481::computeTransformationImpl() 
[DEBUG] (2021-11-15 18:24:20.315) RegistrationVis.cpp:1494::computeTransformationImpl() words from3D=720 to2D=824
[DEBUG] (2021-11-15 18:24:20.316) util3d_motion_estimation.cpp:99::estimateMotion3DTo2D() words3A=720 words2B=824 matches=200 words3B=824 guess=xyz=-0.015153,-0.006271,-0.111544 rpy=0.028403,0.005857,-0.019829 reprojError=2.000000 iterations=300
[DEBUG] (2021-11-15 18:24:20.342) RegistrationVis.cpp:1537::computeTransformationImpl() inliers: 81/200
[DEBUG] (2021-11-15 18:24:20.342) RegistrationVis.cpp:1614::computeTransformationImpl() from->to=xyz=0.014372,0.009738,0.111399 rpy=-0.028514,-0.005291,0.019988
[DEBUG] (2021-11-15 18:24:20.342) RegistrationVis.cpp:1615::computeTransformationImpl() to->from=xyz=-0.011632,-0.004726,-0.113634 rpy=0.030822,0.000769,-0.005709
[DEBUG] (2021-11-15 18:24:20.342) RegistrationVis.cpp:1658::computeTransformationImpl() Refine with bundle adjustment
[DEBUG] (2021-11-15 18:24:20.343) OptimizerG2O.cpp:1263::optimizeBA() Optimizing graph...
[DEBUG] (2021-11-15 18:24:20.343) OptimizerG2O.cpp:1343::optimizeBA() fill poses to g2o...
[DEBUG] (2021-11-15 18:24:20.343) OptimizerG2O.cpp:1396::optimizeBA() fill edges to g2o...
[DEBUG] (2021-11-15 18:24:20.343) OptimizerG2O.cpp:1490::optimizeBA() fill 3D points to g2o...
[DEBUG] (2021-11-15 18:24:20.343) OptimizerG2O.cpp:1497::optimizeBA() stepVertexId=3, negVertexOffset=780
[DEBUG] (2021-11-15 18:24:20.343) OptimizerG2O.cpp:1623::optimizeBA() Initial optimization...
[DEBUG] (2021-11-15 18:24:20.343) OptimizerG2O.cpp:1628::optimizeBA() g2o optimizing begin (max iterations=20, robustKernel=8.000000)
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1648::optimizeBA() iteration 0: 88 nodes, 174 edges, chi2: 982.083843
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (2<->1) d=95.470062 var=1.000000 kernel=8.000000 chi2=9.039529
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (5<->1) d=66.556061 var=1.000000 kernel=8.000000 chi2=8.813086
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (12<->1) d=122.188446 var=1.000000 kernel=8.000000 chi2=31.186177
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (12<->2) d=185.830811 var=1.000000 kernel=8.000000 chi2=10.329916
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (24<->1) d=103.066711 var=1.000000 kernel=8.000000 chi2=10.218969
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (31<->1) d=117.248993 var=1.000000 kernel=8.000000 chi2=8.810709
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (115<->1) d=115.060150 var=1.000000 kernel=8.000000 chi2=12.861548
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (150<->1) d=75.531738 var=1.000000 kernel=8.000000 chi2=15.744107
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (150<->2) d=88.142822 var=1.000000 kernel=8.000000 chi2=14.066523
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (152<->1) d=132.356232 var=1.000000 kernel=8.000000 chi2=15.517253
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (152<->2) d=199.247498 var=1.000000 kernel=8.000000 chi2=9.630820
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (158<->1) d=115.060150 var=1.000000 kernel=8.000000 chi2=11.935184
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (163<->1) d=86.808289 var=1.000000 kernel=8.000000 chi2=9.045093
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (169<->1) d=114.507919 var=1.000000 kernel=8.000000 chi2=10.128916
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (169<->2) d=168.859970 var=1.000000 kernel=8.000000 chi2=10.745732
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (310<->1) d=82.269897 var=1.000000 kernel=8.000000 chi2=9.766492
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (323<->1) d=95.236481 var=1.000000 kernel=8.000000 chi2=15.723708
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (335<->1) d=78.248688 var=1.000000 kernel=8.000000 chi2=14.181134
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (335<->2) d=92.220276 var=1.000000 kernel=8.000000 chi2=9.477392
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (340<->1) d=70.762207 var=1.000000 kernel=8.000000 chi2=8.099583
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (442<->1) d=139.206955 var=1.000000 kernel=8.000000 chi2=25.592555
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (454<->1) d=98.811829 var=1.000000 kernel=8.000000 chi2=11.466654
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (454<->2) d=136.635239 var=1.000000 kernel=8.000000 chi2=9.363992
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (465<->1) d=117.248993 var=1.000000 kernel=8.000000 chi2=11.017878
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (465<->2) d=168.590515 var=1.000000 kernel=8.000000 chi2=8.454087
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (514<->1) d=126.644981 var=1.000000 kernel=8.000000 chi2=15.241190
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (544<->1) d=73.534180 var=1.000000 kernel=8.000000 chi2=13.696602
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (544<->2) d=85.602600 var=1.000000 kernel=8.000000 chi2=11.794023
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (553<->1) d=120.555267 var=1.000000 kernel=8.000000 chi2=12.242606
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (553<->2) d=180.744537 var=1.000000 kernel=8.000000 chi2=9.168586
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (588<->1) d=153.700439 var=1.000000 kernel=8.000000 chi2=10.111808
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (588<->2) d=265.091980 var=1.000000 kernel=8.000000 chi2=11.060484
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (597<->1) d=91.967987 var=1.000000 kernel=8.000000 chi2=13.390480
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (597<->2) d=120.791550 var=1.000000 kernel=8.000000 chi2=8.340263
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (600<->1) d=70.620300 var=1.000000 kernel=8.000000 chi2=10.480352
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (622<->1) d=93.763519 var=1.000000 kernel=8.000000 chi2=13.972785
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (622<->2) d=123.970276 var=1.000000 kernel=8.000000 chi2=8.138576
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (628<->1) d=81.311188 var=1.000000 kernel=8.000000 chi2=10.336904
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (751<->1) d=76.818298 var=1.000000 kernel=8.000000 chi2=14.480115
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (751<->2) d=101.039154 var=1.000000 kernel=8.000000 chi2=8.666147
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (755<->1) d=115.203522 var=1.000000 kernel=8.000000 chi2=9.485810
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (768<->1) d=104.316879 var=1.000000 kernel=8.000000 chi2=14.731648
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (768<->2) d=146.572876 var=1.000000 kernel=8.000000 chi2=12.546334
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (777<->1) d=101.427139 var=1.000000 kernel=8.000000 chi2=16.527153
[DEBUG] (2021-11-15 18:24:20.344) OptimizerG2O.cpp:1703::optimizeBA() outliers=44 outliersCountFar=0
[DEBUG] (2021-11-15 18:24:20.346) OptimizerG2O.cpp:1648::optimizeBA() iteration 1: 88 nodes, 174 edges, chi2: 361.846792
[DEBUG] (2021-11-15 18:24:20.346) OptimizerG2O.cpp:1676::optimizeBA() Ignoring edge (236<->1) d=109.215393 var=1.000000 kernel=8.000000 chi2=8.107185
[DEBUG] (2021-11-15 18:24:20.346) OptimizerG2O.cpp:1703::optimizeBA() outliers=45 outliersCountFar=0
[DEBUG] (2021-11-15 18:24:20.346) OptimizerG2O.cpp:1706::optimizeBA() g2o optimizing end (25 iterations done, error=361.846792, outliers=45/172 (delta=8.000000) time = 0.002655 s)
[DEBUG] (2021-11-15 18:24:20.346) OptimizerG2O.cpp:1801::optimizeBA() Optimizing graph...end!
[DEBUG] (2021-11-15 18:24:20.346) RegistrationVis.cpp:1800::computeTransformationImpl() Pose optimization: xyz=0.014372,0.009738,0.111399 rpy=-0.028514,-0.005291,0.019988 -> xyz=0.011677,0.010111,0.119854 rpy=-0.031207,0.001781,0.010022
[DEBUG] (2021-11-15 18:24:20.346) RegistrationVis.cpp:1814::computeTransformationImpl() BA outliers ratio 0.360465
[DEBUG] (2021-11-15 18:24:20.346) RegistrationVis.cpp:1981::computeTransformationImpl() inliers=55/200
[DEBUG] (2021-11-15 18:24:20.346) RegistrationVis.cpp:1982::computeTransformationImpl() transform=xyz=0.011677,0.010111,0.119854 rpy=-0.031207,0.001781,0.010022
[ INFO] (2021-11-15 18:24:20.346) Rtabmap.cpp:2505::process() [Visual] Add local loop closure in SPACE (1935->1924) xyz=-0.011564,-0.006248,-0.120128 rpy=0.031224,-0.001468,-0.010073
[ INFO] (2021-11-15 18:24:20.346) Memory.cpp:3240::addLink() to=1924, from=1935 transform: xyz=-0.011564,-0.006248,-0.120128 rpy=0.031224,-0.001468,-0.010073 var=0.000141
[DEBUG] (2021-11-15 18:24:20.346) Memory.cpp:3252::addLink() Add link between 1924 and 1935
[DEBUG] (2021-11-15 18:24:20.346) Signature.cpp:121::addLink() Add link 1935 to 1924 (type=2 var=0.000141,0.006395)
[DEBUG] (2021-11-15 18:24:20.346) Signature.cpp:121::addLink() Add link 1924 to 1935 (type=2 var=0.000141,0.006395)
[ INFO] (2021-11-15 18:24:20.346) Rtabmap.cpp:2549::process() timeProximityBySpaceVisualDetection=0.199715s
[DEBUG] (2021-11-15 18:24:20.346) Rtabmap.cpp:2554::process() Proximity detection (local loop closure in SPACE with scan matching)
[DEBUG] (2021-11-15 18:24:20.346) Rtabmap.cpp:2557::process() Proximity by scan matching is disabled (RGBD/ProximityPathMaxNeighbors=0).
[ INFO] (2021-11-15 18:24:20.346) Rtabmap.cpp:2740::process() timeProximityBySpaceDetection=0.000054s
[ INFO] (2021-11-15 18:24:20.346) Rtabmap.cpp:2804::process() timeAddLoopClosureLink=0.000010s
[ INFO] (2021-11-15 18:24:20.346) Rtabmap.cpp:3103::process() Update map correction
[ INFO] (2021-11-15 18:24:20.346) Rtabmap.cpp:4363::optimizeCurrentMap() Optimize map: around location 1935 (lookInDatabase=false)
[ INFO] (2021-11-15 18:24:20.347) Rtabmap.cpp:4372::optimizeCurrentMap() get 667 ids time 0.000696 s
[DEBUG] (2021-11-15 18:24:20.347) Rtabmap.cpp:4413::optimizeGraph() ids=667
[DEBUG] (2021-11-15 18:24:20.347) Memory.cpp:5921::getMetricConstraints() 
[ INFO] (2021-11-15 18:24:20.350) Rtabmap.cpp:4415::optimizeGraph() get constraints (ids=667, 667 poses, 953 edges) time 0.003230 s
[DEBUG] (2021-11-15 18:24:20.351) Rtabmap.cpp:4458::optimizeGraph() use input guess poses
[DEBUG] (2021-11-15 18:24:20.351) OptimizerGTSAM.cpp:103::optimize() Optimizing graph...
[DEBUG] (2021-11-15 18:24:20.351) OptimizerGTSAM.cpp:141::optimize() hasPriorPoses=false, gpsPriorOnly=false
[DEBUG] (2021-11-15 18:24:20.351) OptimizerGTSAM.cpp:159::optimize() fill poses to gtsam... rootId=1 (priorsIgnored=1 gpsPriorOnly=0 landmarksIgnored=0)
[DEBUG] (2021-11-15 18:24:20.351) OptimizerGTSAM.cpp:218::optimize() fill edges to gtsam...
[DEBUG] (2021-11-15 18:24:20.352) OptimizerGTSAM.cpp:474::optimize() create optimizer
[DEBUG] (2021-11-15 18:24:20.354) OptimizerGTSAM.cpp:499::optimize() GTSAM optimizing begin (max iterations=20, robust=0)
[DEBUG] (2021-11-15 18:24:20.365) OptimizerGTSAM.cpp:579::optimize() iteration 1 error =13.602309
[DEBUG] (2021-11-15 18:24:20.373) OptimizerGTSAM.cpp:579::optimize() iteration 2 error =29729937928646867850515028909076928774870563432782369298282651904636528786484602906413102539228243440889631274592671803654749305470318897778696648074600588212475001279471885634691541709009435908108424969260874483395609016237476103583293716554943551806043360564296967782400.000000
[DEBUG] (2021-11-15 18:24:20.373) OptimizerGTSAM.cpp:585::optimize() Negative improvement?! Ignore and continue optimizing... (-29729937928646867850515028909076928774870563432782369298282651904636528786484602906413102539228243440889631274592671803654749305470318897778696648074600588212475001279471885634691541709009435908108424969260874483395609016237476103583293716554943551806043360564296967782400.000000 < 0.000010)
[DEBUG] (2021-11-15 18:24:20.382) OptimizerGTSAM.cpp:579::optimize() iteration 3 error =4973834168400063867606448499559477630609082221442316152344710280244568862528085076763347302027921918739378025748397651343747233554716637361966074603215145687572218894399430767978040908057965714398046131313820125993097588507097227304287227397291112620367277049401407700992.000000
[DEBUG] (2021-11-15 18:24:20.390) OptimizerGTSAM.cpp:579::optimize() iteration 4 error =29292516997576251899234450287886660679405402361518844237838170918646607558837265133786730719811528355412366884031222878508281981302194926406867548065296170789112045212702096617033188532368470076820130889508456142017986815389757593482193717131259058406428900159651840.000000
[DEBUG] (2021-11-15 18:24:20.399) OptimizerGTSAM.cpp:579::optimize() iteration 5 error =2981293703954029802238207798407435809193440088620904378634602935593679745099427668077179675541075898912490137872530450475686930276500046001535060558165408576813282735856584003105463832905261193010525551782227472255739872777986945173145187445615906078089279422070784.000000
[DEBUG] (2021-11-15 18:24:20.407) OptimizerGTSAM.cpp:579::optimize() iteration 6 error =16422692547402511514791620203137452120129341062268498499211020321746337239991579897155592146304235668110595188552351003949675006669128405591323145571647115828549201628757814206473768204552711660943218738200709058240284277338940795202832747439580250850748385095319552.000000
[DEBUG] (2021-11-15 18:24:20.407) OptimizerGTSAM.cpp:585::optimize() Negative improvement?! Ignore and continue optimizing... (-13441398843448481712553412404730016310935900973647594120576417386152657494892152229078412470763159769198105050679820553473988076392628359589788085013481707251735918892901230203368304371647450467932693186418481585984544404560953850029687559993964344772659105673248768.000000 < 0.000010)
[DEBUG] (2021-11-15 18:24:20.416) OptimizerGTSAM.cpp:579::optimize() iteration 7 error =1777745516972351931948998223345629417760872240776900846338033382051947121866427541377029306885494216828088066854097953508378573714467459917440763875917236582408186694074920165915494379995310147325318220461479566467295304894230688512781658302172583180563340228296704.000000
[DEBUG] (2021-11-15 18:24:20.424) OptimizerGTSAM.cpp:579::optimize() iteration 8 error =1220480493409369595353655332536259768149365593227795463959441460965760180276312887161218498189795734313537479099219503786437243800158631383111554604025458245373503951924942772035122547601310900597360488880441949390882431177270448093819737853185009426199857386749952.000000
[DEBUG] (2021-11-15 18:24:20.433) OptimizerGTSAM.cpp:579::optimize() iteration 9 error =488224574903646827565224829057091961234933300353652671979834062329973553111661695605325600473381544592329188126000325325743491314980099441525232987283431360546665195219770525658200450999869329469523412623469759947362980956046820081301038934116213935629427227492352.000000
[DEBUG] (2021-11-15 18:24:20.441) OptimizerGTSAM.cpp:579::optimize() iteration 10 error =3057605065752706539246554323174703301984144043256346818745341302120969890328675297796569640865436078878987643423049365215287449809409391861788873691624632641128029014624131888714152039754726732473555509485611538525323277055962779557530151703665972270128002412576768.000000
[DEBUG] (2021-11-15 18:24:20.441) OptimizerGTSAM.cpp:585::optimize() Negative improvement?! Ignore and continue optimizing... (-2569380490849059487939892631031976931227461261068018438001919957207773450955687802886056498572490789401625128542139856847672792720993979339038166039705445808354597869681083077799121057667262854044647241557620089163585231789406213570521662716912386339508050915753984.000000 < 0.000010)
[DEBUG] (2021-11-15 18:24:20.453) OptimizerGTSAM.cpp:579::optimize() iteration 11 error =473889367698261683050570101543722902298202805365099741117956164311943682014609960525292988979571954886025728612531662733958635443053312434341286709433253003445143855764237353998800532409721857770842441509651228352101740385242382072463582778351011534282382846722048.000000
[DEBUG] (2021-11-15 18:24:20.461) OptimizerGTSAM.cpp:579::optimize() iteration 12 error =573598191095632875040526376387536049863102764426735828453231947080138698721168111676753455364536454804650435069785153334784643444464245255578724339569723030845840651808072358869483643508898106099680061134231615737261516663930593394671695901172380551201941037776896.000000
[DEBUG] (2021-11-15 18:24:20.461) OptimizerGTSAM.cpp:585::optimize() Negative improvement?! Ignore and continue optimizing... (-99708823397371191989956274843813147564899959061636087335275782768195016706558151151460466384964499918624706457253490600826008001410932821237437630136470027400696796043835004870683111099176248328837619624580387385159776278688211322208113122821369016919558191054848.000000 < 0.000010)
terminate called after throwing an instance of 'std::out_of_range'
  what():  map::at
[ INFO] [1637022260.770067483, 1636862607.823032064]: Odom: quality=93, std dev=0.009391m|0.079685rad, update time=0.074080s
**[rtabmap/rtabmap-3] process has died [pid 22481, exit code -6, cmd /home/szx/catkin_ws/devel/lib/rtabmap_ros/rtabmap --delete_db_on_start rgbd_image:=/rgbd_image odom_info:=/front_odom_info odom:=/front_odom __name:=rtabmap __log:=/home/szx/.ros/log/7368028a-4578-11ec-9aaa-989096b12255/rtabmap-rtabmap-3.log].
log file: /home/szx/.ros/log/7368028a-4578-11ec-9aaa-989096b12255/rtabmap-rtabmap-3*.log**
[ INFO] [1637022264.461852541, 1636862607.859918577]: Odom: quality=93, std dev=0.005822m|0.076025rad, update time=0.078461s
[ INFO] [1637022264.606549196, 1636862607.861431739]: Odom: quality=104, std dev=0.006840m|0.077613rad, update time=0.076144s
[ INFO] [1637022265.840372121, 1636862607.873764117]: Odom: quality=124, std dev=0.004562m|0.072132rad, update time=0.078361s
[ INFO] [1637022268.575516049, 1636862607.901054276]: Odom: quality=129, std dev=0.003533m|0.073256rad, update time=0.075268s
[ INFO] [1637022268.941270478, 1636862607.904792966]: Odom: quality=134, std dev=0.003664m|0.075361rad, update time=0.078438s
[ INFO] [1637022273.258745624, 1636862607.947954083]: Odom: quality=146, std dev=0.002415m|0.072512rad, update time=0.081833s
matlabbe commented 2 years ago

You could try with Optimizer/Strategy=1 (g2o). It seems crashing inside GTSAM for some reasons (after 12 iterations). Which GTSAM version are you using? To debug more, you would have to launch the node inside gdb and show the debug trace (see Section A of this issue), we may see where inside GTSAM the map is called with out of range.

szx0112 commented 2 years ago

Thanks for the suggestion. After swith to g2o, every works fine now. For the gtsam, I installed it through PPA. The version is 4.1 and my OS is Ubuntu 18.04.

Below is the screen shot of the gdb return when gtsam is selected as the optimizer.

Screenshot from 2021-11-16 12-15-45

matlabbe commented 2 years ago

Thank you, but I would also take the backtrace of gdb: type bt after the crash right you were in the screenshot.

szx0112 commented 2 years ago

No problem. Attached is the backtrace return. Please let me know if you need other info.

gbt.txt

matlabbe commented 2 years ago

I am using GTSAM 4.0.3-1ubuntu1:

sudo add-apt-repository ppa:borglab/gtsam-release-4.0

maybe this is an issue with GTSAM 4.1. Are you using the nightly build from https://launchpad.net/~borglab, which seems to be 4.1?

szx0112 commented 2 years ago

Yes, it seems the problem is related to the gtsam 4.1. I see no more errors after revert back to 4.0.

However, I found the gtsam shows much worse result than g2o. Please see the results of mapping a loop using gtsam (top) and g2o below (bottom). gtsam

g2o

Both use superpoint for loop closure. It seems gtsam detect wrong loops then mess the entire model. I have tried to tuning some paramters such as increase RGBD/OptimizeMaxError to 10, but it does not help much.

I am ok to stick with g2o because it works as expected. Just curious why this happen. Here are the a copy of returns printed on the command line when wrong loops are detected/rejected by gtsam:

`[ INFO] [1637611558.816105256, 1636862644.811008352]: rtabmap (3815): Rate=0.00s, Limit=0.000s, Conversion=0.0058s, RTAB-Map=1.3650s, Maps update=0.0085s pub=0.0000s (local map=2365, WM=2365) [ WARN] (2021-11-22 14:06:00.382) Rtabmap.cpp:3180::process() Rejecting all added loop closures (2, first is 3816 <-> 3804) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 16.195126 (edge 3815->3816, type=0, abs error=0.512135 m, stddev=0.031623). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation. [ WARN] (2021-11-22 14:06:00.382) Rtabmap.cpp:3214::process() Loop closure 3816->3804 rejected! [ WARN] (2021-11-22 14:06:00.382) Rtabmap.cpp:3214::process() Loop closure 3816->3798 rejected!

[ INFO] [1637611560.923651001, 1636862644.832097016]: Odom: quality=390, std dev=0.001342m|0.047716rad, update time=0.052916s [ WARN] (2021-11-22 14:06:02.385) Rtabmap.cpp:3180::process() Rejecting all added loop closures (2, first is 3817 <-> 3805) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 9.486082 (edge 3798->3800, type=0, abs error=0.299976 m, stddev=0.031623). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation. [ WARN] (2021-11-22 14:06:02.385) Rtabmap.cpp:3214::process() Loop closure 3817->3805 rejected! [ WARN] (2021-11-22 14:06:02.385) Rtabmap.cpp:3214::process() Loop closure 3817->3798 rejected!

[ INFO] [1637611566.996049509, 1636862644.892793945]: Odom: quality=374, std dev=0.001327m|0.047716rad, update time=0.067247s `

matlabbe commented 2 years ago

If you can share the database, I could take a look why there is so much difference between both.

szx0112 commented 2 years ago

@matlabbe Please try this

You might need to play it at the reduced speed (-r 0.01) or the VO can easily get lost.

matlabbe commented 2 years ago

The link expired, sorry I could not download it earlier

szx0112 commented 2 years ago

No problem, use this link

szx0112 commented 2 years ago

Hi @matlabbe , did you have a chance to look at the bag? I just noticed I adjusted some parameters in VO and RTABMAP nodes, not quite sure if it can be reproduced with the default settings. Pls let me know, I can send you my launch files if needed.

matlabbe commented 2 years ago

Hi, I tried the bag with this:

$ roslaunch rtabmap_ros rtabmap.launch \
   args:="-d \
      --Rtabmap/DetectionRate 2 \
      --GFTT/QualityLevel 0.0001 \
      --GFTT/MinDistance 7" \
   rgb_topic:=/camera5/color/image_raw  \
   camera_info_topic:=/camera5/color/camera_info \
   depth_topic:=/camera5/aligned_depth_to_color/image_raw \
   use_sim_time:=true \
   approx_sync:=false \
   frame_id:=camera5_link

$ rosbag play --clock --pause -r 0.2 loop2_camera5.bag

Indeed, we have to reduce speed of the bag for the fast motions in textureless parts of the pipe. There are some drifts, mostly along Z axis: Top and side views of odometry only: Screenshot from 2021-12-06 18-50-54 Screenshot from 2021-12-06 18-50-32

With loop closure: Screenshot from 2021-12-06 18-51-32 Screenshot from 2021-12-06 18-51-56

I couldn't reproduce the GTSAM problem though. I tried with gtsam4.0 and gtsam develop versions from PPA and the optimization is the same: Screenshot from 2021-12-06 19-49-57

I see yellow links on your graph, did you enable RGBD/ProximityByTime? If you can share the database directly, it would be easier to reproduce exactly your problem.

szx0112 commented 2 years ago

@matlabbe Thanks for testing the bag. I will try to rebuild trabmap and see if the problem happens again. I don't have RGBD/ProximityByTime setup, but indeed have RGBD/ProximityBySpace set to true. Maybe it relates to other parameters selected in my launch file.

My results are obtain from the exact same bag as I shared. The overall database is just multiple cameras mounted on a rig. I splited the database per camera, tried to register them one by one using same ideal as multi-session map, I start with registering two cameras like this (https://github.com/introlab/rtabmap_ros/issues/645#issue-1001375272), but find it is very difficult to register them together due to the sparse features and insufficient overlapping between the maps.

matlabbe commented 2 years ago

The launch file link is not working.

szx0112 commented 2 years ago

@matlabbe sorry I paste the wrong link, please try this

matlabbe commented 2 years ago

At first view I don't see why there are as may yellow links. If you can share your database directly, it will be easier to look at the problem.

szx0112 commented 2 years ago

@matlabbe please see the dataset. It contain multiple cams so it is quite large.

matlabbe commented 2 years ago

It looks like the link is not public. However, I would rather have the database you are generating than the raw bags, because I may ended up like my previous post without reproducing the wrong links you have...

szx0112 commented 2 years ago

That is weird, the link should work. I haven't got a chance to reinstall rtabmap on my desktop yet. Will test the results later.

Here are the separate bags cam4, cam1, cam2

Could you give a try for the multi-session mapping when you are available? I had difficulty to fully register them together (in sequence: 5->4->1->2). Thanks.

fbliman commented 1 year ago

@szx0112 could you please share your launch files for this project, thanks!!