Apriltags_ros with RTABMAP Integration

Theory: There could be definitely noise in apriltag(which is slight) but Rtabmap needs to destroy that tf after seeing it once and not continue to optimize it when in front of the robot or use the decision margin to garbage the landmark out instead of deformation in 2d mode but I don't understand still why Z-axis gets affected even though 3Dof is active, it should be x or y ?

Below is my Apriltag config

Attached is Log, https://easyupload.io/9oib83 https://easyupload.io/po3a7c (quick video of robot in motion vs stationary in front of tag)

I don't know why Marker/Prior in rqt(dynamic reconfigure) shows "2 and not whole string.

Please let me know your thoughts on below for decision margin, https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_slam/src/CoreWrapper.cpp#L2434 (> 75 and < 90)

        if(camToTag.isNull())
        {
            RCLCPP_WARN(get_logger(), "Could not get TF between %s and %s frames for tag detection %d.",
                frameId_.c_str(),
                tagFrameId.c_str(),
                tagDetections->detections[i].id);
                continue;
        }
        if (tagDetections->detections[i].decision_margin > 75 && tagDetections->detections[i].decision_margin < 95)
        {
            geometry_msgs::msg::PoseWithCovarianceStamped p;
            rtabmap_conversions::transformToPoseMsg(camToTag, p.pose.pose);
            p.header = tagDetections->header;

            uInsert(landmarks_,
                    std::make_pair(tagDetections->detections[i].id,
                            std::make_pair(p, 0.0f)));
        }
    }

The z/roll/ptich values of the localized pose may be set if the input odometry poses are not 3DoF. There is a check here when landmark localization is done:

if(_graphOptimizer->isSlam2d() && signature->getPose().is3DoF())
{
    // in case of 3d landmarks, transform constraint to 2D
    newPose = newPose.to3DoF();
    UDEBUG("newPose 2D=%s", newPose.prettyPrint().c_str());
}

You can also launch rtabmap node with --udebug argument or call rtabmap/rtabmap/log_debug service to enable debug log. If "newPose 2D..." log is not shown, check if the odometry topic produces exactly 3DoF transforms.

@matlabbe the landmark angular I have set to 9999, and the odometry coming is 3dof too, based on my launch file which is ICP with guess frame coming from EKF (which is in 2d mode too as per there parameter documentation.) and I see what you are saying this is what I get from newPose( looks 6dof) in the below log

Node(
        package='rtabmap_odom', executable='icp_odometry', output='screen',
        #prefix="xterm -e gdb -ex run --args",
        parameters=[{
            'frame_id':'base_link',
            'odom_frame_id':'odom_icp',
            'guess_frame_id':'odom_ekf',
            'publish_tf':True,
            'approx_sync':False,
            'tf_prefix':'', 
            'wait_for_transform':0.2,
            'queue_size':2,
            'publish_null_when_lost':False,
            'expected_update_rate':30.0,
            'use_sim_time':use_sim_time,
            'qos':qos,
            'deskewing':deskewing,
            'deskewing_slerp':deskewing_slerp,
            'wait_imu_to_init': False,
        }],
        remappings=[
            ('scan_cloud', '/hesai/points'),
            ('odom','/odom/icp')
        ],
        arguments=[
            #ICP
            #'--udebug',
            'Icp/CCFilterOutFarthestPoints', 'true',
            'Icp/PointToPlane', 'true',
            'Icp/VoxelSize', '0.15',
            'Icp/Epsilon', '0.001', 
            'Icp/DownsamplingStep','1',
            'Icp/PointToPlaneK', '20',
            'Icp/PointToPlaneRadius', '0',
            'Icp/MaxTranslation', '0',
            'Icp/Iterations','10', 
            'Icp/MaxCorrespondenceDistance', '0.3',
            'Icp/CorrespondenceRatio', '0.1',
            'Icp/PointToPlaneGroundNormalsUp','0.8',
            'Icp/Strategy', '1',
            'Icp/ReciprocalCorrespondences','False',
            'Icp/OutlierRatio', '0.85',
            #'Icp/Force4DoF','true',
            #Odom
            'Odom/ScanKeyFrameThr', '0.4',
            'OdomF2M/ScanSubtractRadius', '0.15', #same as voxel size
            'OdomF2M/ScanMaxSize', '20000',
            'OdomF2M/BundleAdjustment', 'false',
            'Odom/ResetCountdown','5',
            #optimizer
            'Optimizer/Strategy','2',
            'Optimizer/GravitySigma','0.1',
            'Reg/Strategy', '1',
            'Reg/Force3DoF','true',

        ]),

    Node(
        package='rtabmap_util', executable='point_cloud_assembler', output='screen',
        parameters=[{
            'max_clouds':10,
            'fixed_frame_id':'',
            'use_sim_time':use_sim_time,
            'qos':qos,
            'qos_odom':qos,

        }],
        remappings=[
            ('cloud', 'odom_filtered_input_scan'),
            ('odom', '/odom/icp')
        ]),

        Node(
        condition=IfCondition(localization),
        package='rtabmap_slam', executable='rtabmap', output='screen',
        #prefix="xterm -e gdb -ex run --args",
        parameters=[{
            'frame_id': 'base_link',
            'map_frame_id':'map',
            'publish_tf':True,
            'subscribe_depth':False,
            'subscribe_scan ':False,
            'subscribe_stereo ':False,
            'subscribe_scan_cloud':True,
            'subscribe_rgb':True,
            'subscribe_rgbd':False,
            'approx_sync':True,
            'queue_size':5,
            'wait_for_transform':0.2,
            'odom_frame_id':'odom_icp',
            'tf_prefix':'',
            'use_sim_time':use_sim_time,
            'qos_image':qos,
            'qos_camera_info':qos,
            'qos_odom':qos,
            'use_action_for_goal':False,
            'wait_imu_to_init': False,
            'landmark_angular_variance':float(9999),
            'landmark_linear_variance':0.01,
            'odom_tf_linear_variance':0.0005,
            'odom_tf_angular_variance':0.005,

        }],
        remappings=[
            ('scan_cloud', 'assembled_cloud'),
            ('tag_detections','/apriltag/detections'),
            ('grid_map','/map'),  # for Nav2 topic ease
            ('rgb/image','/slam_lidar_camera/color/image_raw'),
            ('rgb/camera_info','/slam_lidar_camera/color/camera_info')
        ],
        arguments=[
            #rtabmap
            '--udebug',
            'Rtabmap/DetectionRate','1',
            'Rtabmap/StartNewMapOnLoopClosure','true',
            'Rtabmap/TimeThr','700',
            #RGBD
            'RGBD/MarkerDetection','false',
            'RGBD/NeighborLinkRefining','false',
            'RGBD/ProximityBySpace','True',
            'RGBD/ProximityMaxGraphDepth', '0',
            'RGBD/ProximityPathMaxNeighbors', '3',
            'RGBD/AngularUpdate', '0.01',
            'RGBD/LinearUpdate', '0.01',
            'RGBD/Enabled','true',
            #'RGBD/OptimizeMaxError','0',
            'RGBD/ProximityPathFilteringRadius','0',
            'RGBD/MaxOdomCacheSize','30',
            'RGBD/StartAtOrigin','false',
            'RGBD/LocalizationPriorError','0.01',
            #'RGBD/OptimizeFromGraphEnd','true',
            #Grid for 2D
            'Reg/Strategy', '1',
            'Reg/Force3DoF','true',
            'Reg/RepeatOnce','False',
            'Grid/Sensor','0',
            'Grid/3D','false',
            'Grid/CellSize','0.05',
            'Grid/RangeMax','5',
            'Grid/ClusterRadius','1',
            'Grid/MinGroundHeight','0.0',
            'Grid/MaxGroundHeight','0.05',
            'Grid/MaxObstacleHeight','2',
            'Grid/RayTracing','true',
            'Grid/NoiseFilteringMinNeighbors','2',
            'Grid/NoiseFilteringRadius','0.05',
            'Grid/NormalsSegmentation','false',
            'Grid/FootprintHeight', '0.171',
            'Grid/FootprintLength','0.6604',
            'Grid/FootprintWidth','0.6096',
            'Grid/VoxelSize','0.05',
            #optimizer
            'Optimizer/GravitySigma','0.1',
            'Optimizer/Strategy','2',
            'Optimizer/Robust','false',
            'Optimizer/PriorsIgnored','true',
            'Optimizer/Iterations','5',
            'Optimizer/Epsilon','0.01',
            #ICP
            'Icp/VoxelSize', '0.15',
            'Icp/PointToPlaneK', '20',
            'Icp/PointToPlaneRadius', '0',
            'Icp/PointToPlane', 'true',
            'Icp/Epsilon', '0.001',
            'Icp/MaxTranslation', '0', #3.14
            'Icp/MaxCorrespondenceDistance', '0.3',
            'Icp/Strategy', '1',
            'Icp/OutlierRatio', '0.85',
            'Icp/CorrespondenceRatio', '0.1',
            'Icp/PointToPlaneGroundNormalsUp','0.8',
            'Icp/Iterations','10',
            'Icp/Force4DoF','true',
            #memory
            'Mem/NotLinkedNodesKept', 'false',
            'Mem/STMSize', '30',
            'Mem/LaserScanNormalK', '20',
            'Mem/IncrementalMemory','false',
            'Mem/InitWMWithAllNodes','true',
            'Mem/RehearsalSimilarity','0.25',
            #'Mem/BadSignaturesIgnored','true,'
            #loop closures
            'Rtabmap/LoopThr','0.11',
            'Kp/DetectorStrategy','11',
            'Vis/FeatureType','11',
            'Kp/MaxFeatures','800',
            #markers
            'Marker/Priors','"2 1.72 -10.06 0.59 -0.02 -0.02 -3.03|1 3.26 -0.24 0.49 0.00 0.03 -1.47"',
            #'Marker/Priors','"1 -0.49 -1.97 -0.05 -0.09 0.20 0.88"',

        ]),

        rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.981) Rtabmap.cpp:3227::process() Opt  -3 xyz=2.625630,-0.352895,0.559781 rpy=0.438645,-0.056381,-1.525544[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.981) Rtabmap.cpp:3227::process() Opt  143 xyz=0.044560,-0.149262,-0.000000 rpy=0.000000,0.000000,-0.008786[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.981) Rtabmap.cpp:3227::process() Opt  42728 xyz=0.043714,-0.152661,-0.000000 rpy=0.000000,-0.000000,0.005210[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.981) Rtabmap.cpp:3227::process() Opt  42729 xyz=0.047423,-0.149640,-0.000000 rpy=-0.000000,0.000000,-0.007630[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.981) Rtabmap.cpp:3227::process() Opt  42741 xyz=0.235458,-0.151073,-0.000000 rpy=-0.000000,-0.000000,-0.066563[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.981) Rtabmap.cpp:3227::process() Opt  42742 xyz=0.395813,-0.165179,0.000000 rpy=-0.000000,0.000000,-0.123367[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.981) Rtabmap.cpp:3227::process() Opt  42743 xyz=0.401860,-0.167802,0.000000 rpy=0.000000,-0.000000,-0.130278[0m
[rtabmap-3] [0m[ INFO] (2024-03-12 07:25:11.981) Rtabmap.cpp:3237::process() Compute max graph errors...[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.981) Graph.cpp:912::computeMaxGraphErrors() poses=7 links=8[0m
[rtabmap-3] [0m[ INFO] (2024-03-12 07:25:11.981) Rtabmap.cpp:3258::process() Max optimization linear error = 0.090396 m (link -3->42742, var=0.010000, ratio error/std=0.903963, thr=0.000000)[0m
[rtabmap-3] [0m[ INFO] (2024-03-12 07:25:11.981) Rtabmap.cpp:3300::process() Max optimization angular error = 0.244128 deg (link 42742->42743, var=0.005000, ratio error/std=0.060257, thr=0.000000)[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.981) Rtabmap.cpp:3545::process() Adjusted localization link 42743->-3 after optimization[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.981) Rtabmap.cpp:3546::process() from xyz=2.209498,0.107673,0.559781 rpy=0.438645,-0.056381,-1.363522[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.981) Rtabmap.cpp:3547::process()   to xyz=2.228971,0.105365,0.559781 rpy=0.438645,-0.056381,-1.395266[0m
[rtabmap-3] [0m[ INFO] (2024-03-12 07:25:11.981) Rtabmap.cpp:3563::process() Update localization[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.981) Rtabmap.cpp:3636::process() newPose=xyz=0.387270,-0.122695,1.024696 rpy=-0.063339,0.452936,-0.165002[0m
[rtabmap-3] [0m[ INFO] (2024-03-12 07:25:11.982) Rtabmap.cpp:3901::process() timeMapOptimization=0.002189s[0m
[rtabmap-3] [0m[ INFO] (2024-03-12 07:25:11.982) Rtabmap.cpp:3937::process() sending stats...[0m
[rtabmap-3] [0m[ INFO] (2024-03-12 07:25:11.982) Rtabmap.cpp:3950::process() send all stats...[0m
[rtabmap-3] [0m[ INFO] (2024-03-12 07:25:11.982) Rtabmap.cpp:4054::process() Localization pose = xyz=0.387270,-0.122695,1.024696 rpy=-0.063339,0.452936,-0.165002[0m
[rtabmap-3] [0m[ INFO] (2024-03-12 07:25:11.982) Rtabmap.cpp:4068::process() Set map correction = xyz=1.054000,-1.164457,0.686100 rpy=-0.402579,0.222550,-1.113676[0m
[rtabmap-3] [0m[ INFO] (2024-03-12 07:25:11.982) Rtabmap.cpp:4149::process() Time creating stats = 0.000844...[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.983) Memory.cpp:2788::removeRawData() id=42743 image=1 scan=0 userData=1[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.983) Memory.cpp:2075::cleanup() [0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.983) Memory.cpp:2387::moveToTrash() id=42743[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.983) Memory.cpp:6092::disableWordsRef() id=42743[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.984) Memory.cpp:6108::disableWordsRef() 800 words total ref removed from signature 42743... (total active ref = 84800)[0m
[rtabmap-3] [0m[ INFO] (2024-03-12 07:25:11.985) Rtabmap.cpp:4251::process() timeMemoryCleanup = 0.002434s... 1 signatures removed[0m
[rtabmap-3] [0m[ INFO] (2024-03-12 07:25:11.985) Rtabmap.cpp:4264::process() Total time processing = 0.212461s...[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.985) Rtabmap.cpp:4305::process() Refresh local map from 23[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.985) Rtabmap.cpp:4320::process() id=23 _optimizedPoses=109[0m
[rtabmap-3] [32m[DEBUG] (2024-03-12 07:25:11.985) Rtabmap.cpp:4332::process() Detected that only last signature has been removed (lastId=42743)[0m
[rtabmap-3] [0m[ INFO] (2024-03-12 07:25:11.985) Rtabmap.cpp:4403::process() Time limit reached processing = 0.000031...[0m
[rtabmap-3] [INFO] [1710242711.987702080] [rtabmap]: rtabmap (42743): Rate=1.00s, Limit=0.700s, Conversion=0.0008s, RTAB-Map=0.2145s, Maps update=0.0007s pub=0.0001s (local map=108, WM=106)
[icp_odometry-1] [INFO] [1710242712.069908045] [icp_odometry]: Odom: ratio=0.523766, std dev=0.026573m|0.008403rad, update time=0.027296s
[icp_odometry-1] [INFO] [1710242712.119646232] [icp_odometry]: Odom: ratio=0.528842, std dev=0.026684m|0.008438rad, update time=0.038601s
[icp_odometry-1] [INFO] [1710242712.164009582] [icp_odometry]: Odom: ratio=0.524074, std dev=0.026905m|0.008508rad, update time=0.031577s

Couldn't this function be added too in the wrapper to check also for landmark quality especially when using Apriltags ?

        if (tagDetections->detections[i].decision_margin > 75 && tagDetections->detections[i].decision_margin < 95)
        {
            geometry_msgs::msg::PoseWithCovarianceStamped p;
            rtabmap_conversions::transformToPoseMsg(camToTag, p.pose.pose);
            p.header = tagDetections->header;

            uInsert(landmarks_,
                    std::make_pair(tagDetections->detections[i].id,
                            std::make_pair(p, 0.0f)));
        }

This is my EKF configuration which comes as guess frame to slam node.

So this log is not shown, maybe the z, roll, pith values are not exactly 0, thus the check signature->getPose().is3DoF() is not true.

Could you show the value of odometry?

ros2 run tf2_ros tf2_echo odom_icp base_link

and

ros2 run tf2_ros tf2_echo odom_ekf base_link

Please see the screeshot for the log,

I closed the Python issue where I had asked this question to you as I had issue setting Vis/RoiRatios and Kp/RoiRatios, I get this error while launching,

Also I get parse error for Marker/Priors (I followed your example from your post in Forum page)

ros2 topic echo /odom/icp header: stamp: sec: 1710858136 nanosec: 504297972 frame_id: odom_icp child_frame_id: base_link pose: pose: position: x: -8.6744170403108e-05 y: -0.00036029890179634094 z: 0.0 orientation: x: 0.0 y: 0.0 z: -0.0008547164369826128 w: 0.9999996347298395 covariance:

5.786795404947043e-06
0.0
0.0
0.0
0.0
0.0
0.0
5.786795404947043e-06
0.0
0.0
0.0
0.0
0.0
0.0
5.786795404947043e-06
0.0
0.0
0.0
0.0
0.0
0.0
5.786795404947044e-07
0.0
0.0
0.0
0.0
0.0
0.0
5.786795404947044e-07
0.0
0.0
0.0
0.0
0.0
0.0
5.786795404947044e-07 twist: twist: linear: x: -0.003023546887561679 y: -0.005135967396199703 z: 0.0 angular: x: 0.0 y: 0.0 z: 0.0003145650262013078 covariance:
2.8933977024735215e-06
0.0
0.0
0.0
0.0
0.0
0.0
2.8933977024735215e-06
0.0
0.0
0.0
0.0
0.0
0.0
2.8933977024735215e-06
0.0
0.0
0.0
0.0
0.0
0.0
2.893397702473522e-07
0.0
0.0
0.0
0.0
0.0
0.0
2.893397702473522e-07
0.0
0.0
0.0
0.0
0.0
0.0
2.893397702473522e-07

ros2 topic echo /odom/ekf header: stamp: sec: 1710858164 nanosec: 565059328 frame_id: odom_ekf child_frame_id: base_link pose: pose: position: x: 8.612408770479605e-08 y: 7.332042422677922e-14 z: 4.134883667295682e-63 orientation: x: -1.4977284793912615e-21 y: 3.140082435846923e-21 z: 0.011680774730330676 w: 0.9999317774236897 covariance:

5.085171321131029
6.082102975258061e-26
-1.1757427433481247e-31
-3.2861493401236896e-66
7.531782427226329e-66
-8.938414156842505e-15
-3.810658241429041e-24
5.085171321131029
-5.790664156131194e-32
1.4104085166130766e-64
-3.2291178221499886e-64
1.0789363657706965e-10
-1.1757427433481245e-31
-5.790664156131197e-32
9.96690010411171e-07
3.1929418827212346e-56
4.7366571212576985e-51
-9.858415078331811e-69
-3.392912039126149e-66
1.4562308459710049e-64
3.192941882721237e-56
9.934233442690997e-07
6.965055138758977e-28
-6.241725878752542e-26
7.531782431517759e-66
-3.2291178239918594e-64
4.736657121257698e-51
6.964490537604171e-28
9.934233442690997e-07
1.3841618997203817e-25
-8.93841415684255e-15
1.0789363657706937e-10
-9.858424581960187e-69
-6.045308503548055e-26
1.3841618989390145e-25
6.907903441478748 twist: twist: linear: x: -5.717685206124895e-38 y: 3.3512984110455207e-82 z: -1.0201759776444732e-67 angular: x: -5.055980171791758e-20 y: 1.120950471275229e-19 z: 0.0012185752000484114 covariance:
0.00026883186367407874
-2.6142575481793614e-50
3.3334433827945544e-34
-1.232616520256144e-80
-1.7958735645535908e-75
-1.7132770161214414e-87
-2.6142575481793605e-50
0.00026883186367407874
1.546672726639966e-34
-5.719189947486493e-81
-8.332621530613514e-76
-7.950766715494549e-88
3.333443382794553e-34
1.546672726639966e-34
9.950513284601065e-07
7.420025420610877e-65
1.081523764866329e-59
8.811576524650287e-72
-1.2326165202561444e-80
-5.719189947486496e-81
7.420025420610881e-65
9.807694416823469e-07
6.680907496017666e-19
-3.910691064108379e-20
-1.79587356455359e-75
-8.332621530613512e-76
1.081523764866329e-59
6.680907496017664e-19
9.807694416823469e-07
8.670308906231857e-20
-1.713277789545682e-87
-7.950770304221433e-88
8.811580921906129e-72
-3.787652801375118e-20
8.670308906231854e-20
0.0009513916638092568

On the EKF side there is small e-^ component in z and other 3DoF, could this be causing issue ?

Yeah, a workaround would be to subscribe to /odom/icp topic (remap ('odom', '/odom/icp')) instead of using odom_frame_id parameter for rtabmap node.

I'll check later if I can add an epsilon when checking for 3DoF.

Thank you @matlabbe, I can try that out. Can you please advise on the minor 2 parameters setting when you can ? as I am getting parse error while setting them up in python launch file and I tried referencing your previous post in Forum page,

Current Setting: 'Vis/RoiRatios','0.0 0.0 0.2 0.3', 'Kp/RoiRatios','0.0 0.0 0.2 0.3', 'Marker/Priors','"1 2.97 -0.70 0.51 -0.03 -0.05 -1.54|2 0.30 -10.27 0.63 0.37 -0.06 -3.06"', (unless I cannot use this as I am using external Apriltag_ros library ?)

Also is this still good practice in code to have this check ?

    if (tagDetections->detections[i].decision_margin > 75 && tagDetections->detections[i].decision_margin < 95)
    {
        geometry_msgs::msg::PoseWithCovarianceStamped p;
        rtabmap_conversions::transformToPoseMsg(camToTag, p.pose.pose);
        p.header = tagDetections->header;

        uInsert(landmarks_,
                std::make_pair(tagDetections->detections[i].id,
                        std::make_pair(p, 0.0f)));
    }

Using topic instead of frame on rtabmap node comes with its own issue I get when launching Nav2 and subscribing /odom/icp there as well, ON Rtabmap side I get,

Can you please suggest, to troubleshoot I disabled Kp/MaxFeatures to -1 and then Nav2 and Rtabmap was fine, this looks like to me profiling issue, I am using superpoint + superglue method, is there any safe implementations which could help ? Image decimation ? or Mem/ side ? Kp/ByteToFloat ? queue size of 15 works but looks to me very high number on Rtabmap side, with loop closure enabled.
Apart from this on RQT for setting roi and Marker/Prior strings show this, something is not getting parsed the right way,

Launch file,

For the 3DoF odom issue, you can try to add odomPose = odomPose.to3DoF() here.

Not sure why it cannot sync the odometry topic. Check timestamps of all input topics, check at which rate they are published.

Not sure what issue 2. is referring to.

Are you on ros humble? I tried on ros foxy and added the following to this file and it seems to work:

          # RTAB-Map's parameters should be strings:
          'Vis/RoiRatios':'0.0 0.0 0.2 0.3',
          'Kp/RoiRatios':'0.0 0.0 0.2 0.3',
          'Marker/Priors':'1 2.97 -0.70 0.51 -0.03 -0.05 -1.54|2 0.30 -10.27 0.63 0.37 -0.06 -3.06',

[rtabmap-2] [INFO] [1710989345.646472360] [rtabmap]: Setting RTAB-Map parameter "Kp/RoiRatios"="0.0 0.0 0.2 0.3"
[rtabmap-2] [INFO] [1710989345.646743441] [rtabmap]: Setting RTAB-Map parameter "Marker/Priors"="1 2.97 -0.70 0.51 -0.03 -0.05 -1.54|2 0.30 -10.27 0.63 0.37 -0.06 -3.06"
[rtabmap-2] [INFO] [1710989345.647755689] [rtabmap]: Setting RTAB-Map parameter "Optimizer/GravitySigma"="0"
[rtabmap-2] [INFO] [1710989345.648615355] [rtabmap]: Setting RTAB-Map parameter "RGBD/NeighborLinkRefining"="True"
[rtabmap-2] [INFO] [1710989345.649152612] [rtabmap]: Setting RTAB-Map parameter "Reg/Force3DoF"="true"
[rtabmap-2] [INFO] [1710989345.649202580] [rtabmap]: Setting RTAB-Map parameter "Reg/Strategy"="1"
[rtabmap-2] [INFO] [1710989345.649827249] [rtabmap]: Setting RTAB-Map parameter "Rtabmap/TimeThr"="0"
[rtabmap-2] [INFO] [1710989345.652538858] [rtabmap]: Setting RTAB-Map parameter "Vis/RoiRatios"="0.0 0.0 0.2 0.3"

ros2 param get /rtabmap Marker/Priors
String value is: 1 2.97 -0.70 0.51 -0.03 -0.05 -1.54|2 0.30 -10.27 0.63 0.37 -0.06 -3.06

For the 3DoF odom issue, you can try to add odomPose = odomPose.to3DoF() here.

As per your suggestion of forcing 3Dof works great so far, no issues with tags in z direction.

Are you on ros humble? I tried on ros foxy and added the following to this file and it seems to work:

I am on galactic, and I was following this file and passing it like arguments which is what I see the issue to be, changing the file to your format as passing it as parameters fixed it, rqt shows the string now. Thank you for pointing that file, could be also good to fix that vlp_16 launch file.

Not sure why it cannot sync the odometry topic. Check timestamps of all input topics, check at which rate they are published.

This is fixed too, by using tf instead of topic and reducing the queue on odom to 3 and rtabmap node to 5.

I want to ask you two question specific to my Database for Memory management and then I can close this topic.

'Mem/RehearsalSimilarity':'0.63'
'Rtabmap/TimeThr':'700', for 1hz detection

Are these ok numbers for warehouse environment or can be there any optimization done in parameters to reduce the time for nodes ? Using ORB or SURF or SIFT reduces the time under 0.20, but as per your paper, superpoint is better with your dataset and even better is superglue technique, with just superpoint I got 0.51 secs as time and was curious to see if there could be anything used optimize ?

Is there any way for avoiding floor height for anomalies in warehouse to avoid any un-expected loop closures like ROI which does not look like is supported by superpoint as logger complains about that ?

Current Parameter setting,

     parameters = {
            'frame_id': 'base_link',
            'map_frame_id':'map',
            'publish_tf':True,
            'subscribe_depth':False,
            'subscribe_scan ':False,
            'subscribe_stereo ':False,
            'subscribe_scan_cloud':True,
            'subscribe_rgb':True,
            'subscribe_rgbd':False,
            'approx_sync':True,
            'queue_size':5,
            'wait_for_transform':0.2,
            'odom_frame_id':'odom_icp',
            'tf_prefix':'',
            'use_sim_time':use_sim_time,
            'qos_image':qos,
            'qos_camera_info':qos,
            'qos_odom':qos,
            'wait_imu_to_init': False,
            'odom_tf_linear_variance':0.0005,
            'odom_tf_angular_variance':0.005,
            #rtab
            'Rtabmap/DetectionRate':'1',
            #RGBD
            'RGBD/MarkerDetection':'false',
            'RGBD/NeighborLinkRefining':'false',
            'RGBD/ProximityBySpace':'True',
            'RGBD/ProximityMaxGraphDepth': '0',
            'RGBD/ProximityPathMaxNeighbors': '3',
            'RGBD/AngularUpdate': '0.01',
            'RGBD/LinearUpdate':'0.01',
            'RGBD/CreateOccupancyGrid':'true',
            'RGBD/Enabled':'true',
            'RGBD/OptimizeMaxError':'0',
            'RGBD/ProximityPathFilteringRadius':'0',
            'RGBD/MaxOdomCacheSize':'30',
            'RGBD/StartAtOrigin':'false',
            'RGBD/LocalizationPriorError':'0.01',
            #Grid for 2D
            'Reg/Strategy':'1',
            'Reg/Force3DoF':'true',
            'Reg/RepeatOnce':'False',
            'Grid/Sensor':'0',
            'Grid/3D':'false',
            'Grid/CellSize':'0.05',
            'Grid/RangeMax':'5',
            'Grid/ClusterRadius':'1',
            'Grid/MinGroundHeight':'0.0',
            'Grid/MaxGroundHeight':'0.1',
            'Grid/MaxObstacleHeight':'2',
            'Grid/RayTracing':'true',
            'Grid/NoiseFilteringMinNeighbors':'2',
            'Grid/NoiseFilteringRadius':'0.05',
            'Grid/NormalsSegmentation':'false',
            #'Grid/FootprintHeight':'0.171',
            #'Grid/FootprintLength':'0.6604',
            #'Grid/FootprintWidth':'0.6096',
            'Grid/VoxelSize':'0.05',
            #optimizer
            'Optimizer/GravitySigma':'0.1',
            'Optimizer/Strategy':'2',
            #ICP
            'Icp/VoxelSize':'0.15',
            'Icp/PointToPlaneK':'20',
            'Icp/PointToPlaneRadius':'0',
            'Icp/PointToPlane':'true',
            'Icp/Epsilon':'0.001',
            'Icp/MaxTranslation':'0',
            'Icp/MaxCorrespondenceDistance':'0.3',
            'Icp/Strategy':'1',
            'Icp/OutlierRatio':'0.85',
            'Icp/CorrespondenceRatio':'0.1',
            'Icp/PointToPlaneGroundNormalsUp':'0.8',
            'Icp/Iterations':'10',
            #memory
            'Mem/NotLinkedNodesKept':'false',
            'Mem/STMSize':'30',
            'Mem/LaserScanNormalK':'20',
            #loop closures
            'Kp/DetectorStrategy':'11',
            'Vis/FeatureType':'11',
            'Vis/CorGuessWinSize':'0', 
            'Vis/CorNNType':'6', 
            #'Vis/RoiRatios':'0.0 0.0 0.2 0.2',
            #'Kp/RoiRatios':'0.0 0.0 0.2 0.2',
            'Vis/CorNNDR':'0.6',
            'Vis/MaxDepth':'6',
            'Vis/MaxFeatures':'1000',
            #localization
            'SuperPoint/ModelPath':'/home/amr/AMRMAIN/src/ROS_DRIVERS_Readme/rtabmap/archive/2022-IlluminationInvariant/scripts/superpoint_v1.pt',
            'PyMatcher/Path':'/home/amr/AMRMAIN/src/ROS_DRIVERS_Readme/rtabmap/archive/2022-IlluminationInvariant/scripts/SuperGluePretrainedNetwork/rtabmap_superglue.py',
            'PyMatcher/Cuda':'true',
            'SuperPoint/Cuda':'true',
            'Mem/UseOdomFeatures':'false', 
            'PyMatcher/Cuda':'true',
            'SuperPoint/Cuda':'true',
            'PyMatcher/Iterations':'7',
    }

    Node(
        package='rtabmap_odom', executable='icp_odometry', output='screen',
        #prefix="xterm -e gdb -ex run --args",
        parameters=[{
            'frame_id':'base_link',
            'odom_frame_id':'odom_icp',
            'guess_frame_id':'odom_ekf',
            'publish_tf':True,
            'approx_sync':False,
            'tf_prefix':'', 
            'wait_for_transform':0.2,
            'queue_size':3,
            'publish_null_when_lost':False,
            'expected_update_rate':30.0,
            'use_sim_time':use_sim_time,
            'qos':qos,
            'deskewing':deskewing,
            'deskewing_slerp':deskewing_slerp,
            'wait_imu_to_init': False,
            'Icp/CCFilterOutFarthestPoints':'true',
            'Icp/PointToPlane':'true',
            'Icp/VoxelSize':'0.15',
            'Icp/Epsilon':'0.001', 
            'Icp/DownsamplingStep':'1',
            'Icp/PointToPlaneK':'20',
            'Icp/PointToPlaneRadius':'0',
            'Icp/MaxTranslation':'0',
            'Icp/Iterations':'10', 
            'Icp/MaxCorrespondenceDistance':'0.3',
            'Icp/CorrespondenceRatio':'0.1',
            'Icp/PointToPlaneGroundNormalsUp':'0.8',
            'Icp/Strategy':'1',
            'Icp/ReciprocalCorrespondences':'False',
            'Icp/OutlierRatio':'0.85',
            #Odom
            'Odom/ScanKeyFrameThr':'0.4',
            'OdomF2M/ScanSubtractRadius':'0.15', #same as voxel size
            'OdomF2M/ScanMaxSize':'20000',
            'OdomF2M/BundleAdjustment':'false',
            'Odom/ResetCountdown':'5',
            #optimizer
            'Optimizer/Strategy':'2',
            'Optimizer/GravitySigma':'0.1',
            'Reg/Strategy':'1',
            'Reg/Force3DoF':'true',

   Node(
        condition=IfCondition(localization),
        package='rtabmap_slam', executable='rtabmap', output='screen',
        #prefix="xterm -e gdb -ex run --args",
        parameters=[parameters,
        {
            'Rtabmap/StartNewMapOnLoopClosure':'true',
            #'Rtabmap/TimeThr':'700',
            #'RGBD/OptimizeFromGraphEnd':'true',
            'Optimizer/Iterations':'5',
            'Optimizer/PriorsIgnored':'true',
            'Optimizer/Epsilon':'0.01',
            #memory
            'Mem/IncrementalMemory':'false',
            'Mem/InitWMWithAllNodes':'true',
            #'Mem/RehearsalSimilarity':'0.80',
            #apriltags
            'Marker/Priors':'"1 3.02 -0.46 0.48 0.00 -0.03 -1.59|2 0.94 -10.20 0.61 0.04 0.00 3.13"',
            'landmark_angular_variance':float(9999),
            'landmark_linear_variance':0.01,
        }],
        remappings=remappings,

It makes sense for the map frame to shift vs robot pose, I was trying to achieve this but sometimes I see the map is never published by using the below parameters for tuning the weight and timing, the logger gives message the time limit has reached, How does one carefully tune timing for transfer of Nodes to LTM ?

            'Rtabmap/StartNewMapOnLoopClosure':'true',
            'Rtabmap/TimeThr':'700',
            'RGBD/OptimizeFromGraphEnd':'true',
            'Optimizer/Iterations':'10',
            'Optimizer/PriorsIgnored':'true',
            'Optimizer/Epsilon':'0.01',
            #memory
            'Mem/IncrementalMemory':'false',
            'Mem/InitWMWithAllNodes':'true',
            'Mem/RehearsalSimilarity':'0.63',

Hi,

Thanks to point out the difference how strings are handled between parameters and arguments in ros2 launch files. I updated the example.

For the 3DoF issue, I am still debating myself if I should add an option to force 3DoF odometry on rtabmap side, though it seems more an upstream issue (where the EKF filter should publish a perfect TF 3DoF transform and it doesn't in your setup). If more people have this issue, I could do something.

Do you run superpoint on CUDA? The timing results of 86 ms for superpoint in the paper were done on a 2019's laptop nvidia gpu.

For optimization, it looks like visual proximity detection/loop closure requires quite much time in comparison to proximity by space (lidar):

Not sure what happens on the first spike. Based on the time you get with superpoint (500 ms), it could make sens that it is caused by SuperGlue (done on CPU?). You may use default KNN matching approach instead.

For the similarity threshold, 0.63 can be okay based on that dataset.

@matlabbe thank you for your reply and the launch file commit. !

For the 3DoF issue, I am still debating myself if I should add an option to force 3DoF odometry on rtabmap side, though it seems more an upstream issue (where the EKF filter should publish a perfect TF 3DoF transform and it doesn't in your setup). If more people have this issue, I could do something.

Its up to your discretion, but just my suggestion if this could be added like, unless there could be fallback which I do not see in short sightedness but you know more better. if 'guess_frame' is being used and 'Reg/Force3DoF':'true', --> odomPose.to3DoF(); EKF package is anyways getting deprecated and fuse is going to be replacing that soon, hopefully this issue will not port over.

Do you run superpoint on CUDA? The timing results of 86 ms for superpoint in the paper were done on a 2019's laptop nvidia gpu.

Yes I am running on CUDA, but I will switch over to default KNN as the timing was better without using superglue and performance of that is still very good. .

Ultimately If you wanna close the topic, I leave it up to you as the original issue has been fixed for me using force function for odometry unless you plan another commit and If I have any question on the Mem/ side I can create one topic in the Nabble forum page.

introlab / rtabmap_ros

Apriltags_ros with RTABMAP Integration #1134

2.893397702473522e-07

0.0009513916638092568