Closed anath93 closed 11 months ago
In your experience, which would be better LeGO_LOAM (I do not have variable terrain, its a flat terrain) or ICP or FLOAM ? I tried to find out more on odometry benchmark testing for RTABMAP but did not find any on the internet except for KITTI dataset benchmarking.
Those approaches appeared relatively at the end of my PhD, didn't have time to really get in newer lidar's odometry approaches comparison (other than original LOAM).
For integration with rtabmap, if it is just the header's names that are changed, you may just have to update them in rtabmap code. Also in my patch on FLOAM for ros1, the
#add_library(floam src/laser_processing_class.cpp src/lidar.cpp src/laser_processing.cpp src/odom_estimation_class.cpp src/odom_estimation.cpp src/lidar_optimization.cpp src/laser_mapping_class.cpp src/laser_mapping_class.cpp)
was needed and rtabmap expects such floam library to exist to correctly link the code.
@thank you for your response, just a side note how will this be ported over to ROS2,
`<node pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
<remap from="imu/data_raw" to="$(arg imu_topic)"/>
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
</node>
<!-- This nodelet imu_filter_madgwick translates the IMU acceleration and angular velocity to
orientation -->
<node pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
<param name="use_mag" value="false"/>
<param name="world_frame" value="enu"/>
<param name="publish_tf" value="false"/>
</node>
<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_util/imu_to_tf imu_nodelet_manager">
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>`
ON the ROS2 side what I have for this
` Node(
package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
parameters=[{'use_mag': False,
'world_frame':'enu',
'publish_tf':False}],
remappings=[
('imu/data_raw', '/zed2i/zed_node/imu/data')
('imu/data', '/zed2i/zed_node/imu/data/filtered') #published after filtering, orientation
]),`
I do not see any documentation on imu_to_tf node ? Can you please clarify, I am trying to replicate vlp-16 with D435 lidar deskewing example with ICP odometry but with fusing with wheel encoders and IMU.
Also just another question, I do not want to take your time and put it in practice. Would it be better to fuse wheel encoders + IMU with EKF node and back to ICP odometry OR fuse wheel encoders + IMU + ICP using EKF node and back to RTAB-MAP slam node ?
Also can I change "Icp/CCSamplingLimit" to the number of points my LIDAR sends every frame rate which is 128 ring sensor and I see 230400 or is there any downside to this ? I want to utilize complete capability of my sensor.
So I tried to write a file using ICP with DE skewing and fusing EKF node ended up like below complaining about IMU orientation which is coming from Zed2i camera.
[INFO] [launch]: All log files can be found below /home/amr-1/.ros/log/2023-08-18-14-48-35-400793-amr-1-scs-2483204
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [imu_filter_madgwick_node-1]: process started with pid [2483649]
[INFO] [lidar_deskewing-2]: process started with pid [2483651]
[INFO] [icp_odometry-3]: process started with pid [2483655]
[INFO] [rtabmap-4]: process started with pid [2483657]
[INFO] [point_cloud_assembler-5]: process started with pid [2483660]
[imu_filter_madgwick_node-1] [INFO] [1692384515.701244321] [imu_filter_madgwick]: Starting ImuFilter
[imu_filter_madgwick_node-1] [INFO] [1692384515.701893828] [imu_filter_madgwick]: Using dt computed from message headers
[imu_filter_madgwick_node-1] [INFO] [1692384515.701935300] [imu_filter_madgwick]: The gravity vector is kept in the IMU message.
[imu_filter_madgwick_node-1] [INFO] [1692384515.702094533] [imu_filter_madgwick]: Imu filter gain set to 0.100000
[imu_filter_madgwick_node-1] [INFO] [1692384515.702125285] [imu_filter_madgwick]: Gyro drift bias set to 0.000000
[imu_filter_madgwick_node-1] [INFO] [1692384515.702136965] [imu_filter_madgwick]: Magnetometer bias values: 0.000000 0.000000 0.000000
[imu_filter_madgwick_node-1] [INFO] [1692384515.779648999] [imu_filter_madgwick]: First IMU message received.
[rtabmap-4] [INFO] [1692384517.494981993] [rtabmap]: rtabmap(maps): map_filter_radius = 0.000000
[rtabmap-4] [INFO] [1692384517.497216884] [rtabmap]: rtabmap(maps): map_filter_angle = 30.000000
[rtabmap-4] [INFO] [1692384517.497300628] [rtabmap]: rtabmap(maps): map_cleanup = true
[rtabmap-4] [INFO] [1692384517.497320436] [rtabmap]: rtabmap(maps): map_always_update = false
[rtabmap-4] [INFO] [1692384517.497334388] [rtabmap]: rtabmap(maps): map_empty_ray_tracing = true
[rtabmap-4] [INFO] [1692384517.497346868] [rtabmap]: rtabmap(maps): cloud_output_voxelized = true
[rtabmap-4] [INFO] [1692384517.497358292] [rtabmap]: rtabmap(maps): cloud_subtract_filtering = false
[rtabmap-4] [INFO] [1692384517.497369236] [rtabmap]: rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[rtabmap-4] [INFO] [1692384517.503405171] [rtabmap]: rtabmap(maps): octomap_tree_depth = 16
[rtabmap-4] [INFO] [1692384517.733458829] [rtabmap]: rtabmap: frame_id = base_link
[rtabmap-4] [INFO] [1692384517.733554286] [rtabmap]: rtabmap: map_frame_id = map
[rtabmap-4] [INFO] [1692384517.733593646] [rtabmap]: rtabmap: log_to_rosout_level = 4
[rtabmap-4] [INFO] [1692384517.734077168] [rtabmap]: rtabmap: initial_pose =
[rtabmap-4] [INFO] [1692384517.734102001] [rtabmap]: rtabmap: use_action_for_goal = false
[rtabmap-4] [INFO] [1692384517.734115569] [rtabmap]: rtabmap: tf_delay = 0.050000
[rtabmap-4] [INFO] [1692384517.734131409] [rtabmap]: rtabmap: tf_tolerance = 0.100000
[rtabmap-4] [INFO] [1692384517.734143889] [rtabmap]: rtabmap: odom_sensor_sync = false
[rtabmap-4] [INFO] [1692384517.734154481] [rtabmap]: rtabmap: pub_loc_pose_only_when_localizing = false
[rtabmap-4] [INFO] [1692384517.734171665] [rtabmap]: rtabmap: gen_scan = false
[rtabmap-4] [INFO] [1692384517.734182673] [rtabmap]: rtabmap: gen_depth = false
[rtabmap-4] [INFO] [1692384517.734195729] [rtabmap]: rtabmap: scan_cloud_max_points = 0
[rtabmap-4] [INFO] [1692384517.734215697] [rtabmap]: rtabmap: scan_cloud_is_2d = false
[point_cloud_assembler-5] [INFO] [1692384518.677047344] [point_cloud_assembler]: point_cloud_assembler: queue_size=10
[point_cloud_assembler-5] [INFO] [1692384518.677735828] [point_cloud_assembler]: point_cloud_assembler: qos=0
[point_cloud_assembler-5] [INFO] [1692384518.677785140] [point_cloud_assembler]: point_cloud_assembler: qos_odom=0
[point_cloud_assembler-5] [INFO] [1692384518.677803956] [point_cloud_assembler]: point_cloud_assembler: fixed_frame_id=
[point_cloud_assembler-5] [INFO] [1692384518.677815892] [point_cloud_assembler]: point_cloud_assembler: frame_id=
[point_cloud_assembler-5] [INFO] [1692384518.677826452] [point_cloud_assembler]: point_cloud_assembler: max_clouds=10
[point_cloud_assembler-5] [INFO] [1692384518.677836948] [point_cloud_assembler]: point_cloud_assembler: assembling_time=0.000000s
[point_cloud_assembler-5] [INFO] [1692384518.677851796] [point_cloud_assembler]: point_cloud_assembler: skip_clouds=0
[point_cloud_assembler-5] [INFO] [1692384518.677861812] [point_cloud_assembler]: point_cloud_assembler: circular_buffer=false
[point_cloud_assembler-5] [INFO] [1692384518.677872628] [point_cloud_assembler]: point_cloud_assembler: linear_update=0.000000 m
[point_cloud_assembler-5] [INFO] [1692384518.677884276] [point_cloud_assembler]: point_cloud_assembler: angular_update=0.000000 rad
[point_cloud_assembler-5] [INFO] [1692384518.677895348] [point_cloud_assembler]: point_cloud_assembler: wait_for_transform=0.100000
[point_cloud_assembler-5] [INFO] [1692384518.677909268] [point_cloud_assembler]: point_cloud_assembler: range_min=0.000000
[point_cloud_assembler-5] [INFO] [1692384518.677919348] [point_cloud_assembler]: point_cloud_assembler: range_max=0.000000
[point_cloud_assembler-5] [INFO] [1692384518.677929172] [point_cloud_assembler]: point_cloud_assembler: voxel_size=0.000000m
[point_cloud_assembler-5] [INFO] [1692384518.677939381] [point_cloud_assembler]: point_cloud_assembler: noise_radius=0.000000m
[point_cloud_assembler-5] [INFO] [1692384518.677949589] [point_cloud_assembler]: point_cloud_assembler: noise_min_neighbors=5
[point_cloud_assembler-5] [INFO] [1692384518.677960309] [point_cloud_assembler]: point_cloud_assembler: remove_z=false
[rtabmap-4] [INFO] [1692384518.688505545] [rtabmap]: Update RTAB-Map parameter "Grid/CellSize"="0.01" from arguments
[rtabmap-4] [INFO] [1692384518.688628618] [rtabmap]: Update RTAB-Map parameter "Grid/ClusterRadius"="1" from arguments
[rtabmap-4] [INFO] [1692384518.688655562] [rtabmap]: Update RTAB-Map parameter "Grid/GroundIsObstacle"="True" from arguments
[rtabmap-4] [INFO] [1692384518.688673066] [rtabmap]: Update RTAB-Map parameter "Grid/RangeMax"="20" from arguments
[rtabmap-4] [INFO] [1692384518.688686154] [rtabmap]: Update RTAB-Map parameter "Grid/Sensor"="1" from arguments
[rtabmap-4] [INFO] [1692384518.688697866] [rtabmap]: Update RTAB-Map parameter "Icp/CorrespondenceRatio"="0.2" from arguments
[rtabmap-4] [INFO] [1692384518.688710154] [rtabmap]: Update RTAB-Map parameter "Icp/Epsilon"="0.001" from arguments
[rtabmap-4] [INFO] [1692384518.688722474] [rtabmap]: Update RTAB-Map parameter "Icp/Iterations"="10" from arguments
[rtabmap-4] [INFO] [1692384518.688733450] [rtabmap]: Update RTAB-Map parameter "Icp/MaxCorrespondenceDistance"="1" from arguments
[rtabmap-4] [INFO] [1692384518.688744906] [rtabmap]: Update RTAB-Map parameter "Icp/MaxTranslation"="3" from arguments
[rtabmap-4] [INFO] [1692384518.688756075] [rtabmap]: Update RTAB-Map parameter "Icp/OutlierRatio"="0.1" from arguments
[rtabmap-4] [INFO] [1692384518.688767915] [rtabmap]: Update RTAB-Map parameter "Icp/PointToPlane"="true" from arguments
[rtabmap-4] [INFO] [1692384518.688779179] [rtabmap]: Update RTAB-Map parameter "Icp/PointToPlaneK"="20" from arguments
[rtabmap-4] [INFO] [1692384518.688791147] [rtabmap]: Update RTAB-Map parameter "Icp/PointToPlaneRadius"="0" from arguments
[rtabmap-4] [INFO] [1692384518.688803339] [rtabmap]: Update RTAB-Map parameter "Icp/Strategy"="1" from arguments
[rtabmap-4] [INFO] [1692384518.688816427] [rtabmap]: Update RTAB-Map parameter "Icp/VoxelSize"="0.1" from arguments
[rtabmap-4] [INFO] [1692384518.688828875] [rtabmap]: Update RTAB-Map parameter "Mem/LaserScanNormalK"="20" from arguments
[rtabmap-4] [INFO] [1692384518.688841643] [rtabmap]: Update RTAB-Map parameter "Mem/NotLinkedNodesKept"="false" from arguments
[rtabmap-4] [INFO] [1692384518.688852875] [rtabmap]: Update RTAB-Map parameter "Mem/STMSize"="30" from arguments
[rtabmap-4] [INFO] [1692384518.688864299] [rtabmap]: Update RTAB-Map parameter "Optimizer/GravitySigma"="0.3" from arguments
[rtabmap-4] [INFO] [1692384518.688875467] [rtabmap]: Update RTAB-Map parameter "RGBD/AngularUpdate"="0.05" from arguments
[rtabmap-4] [INFO] [1692384518.688886059] [rtabmap]: Update RTAB-Map parameter "RGBD/LinearUpdate"="0.05" from arguments
[rtabmap-4] [INFO] [1692384518.689028972] [rtabmap]: Update RTAB-Map parameter "RGBD/NeighborLinkRefining"="False" from arguments
[rtabmap-4] [INFO] [1692384518.689068012] [rtabmap]: Update RTAB-Map parameter "RGBD/ProximityBySpace"="True" from arguments
[rtabmap-4] [INFO] [1692384518.689091564] [rtabmap]: Update RTAB-Map parameter "RGBD/ProximityMaxGraphDepth"="0" from arguments
[rtabmap-4] [INFO] [1692384518.689109548] [rtabmap]: Update RTAB-Map parameter "RGBD/ProximityPathMaxNeighbors"="1" from arguments
[rtabmap-4] [INFO] [1692384518.689122444] [rtabmap]: Update RTAB-Map parameter "Reg/Strategy"="1" from arguments
[rtabmap-4] [INFO] [1692384518.689133740] [rtabmap]: Update RTAB-Map parameter "Rtabmap/DetectionRate"="1" from arguments
[rtabmap-4] [INFO] [1692384518.690277298] [rtabmap]: RTAB-Map detection rate = 1.000000 Hz
[rtabmap-4] [INFO] [1692384518.690679860] [rtabmap]: rtabmap: Deleted database "/home/amr-1/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[rtabmap-4] [INFO] [1692384518.690736308] [rtabmap]: rtabmap: Using database from "/home/amr-1/.ros/rtabmap.db" (0 MB).
[icp_odometry-3] [INFO] [1692384518.721855344] [icp_odometry]: Odometry: frame_id = base_link
[icp_odometry-3] [INFO] [1692384518.726148293] [icp_odometry]: Odometry: odom_frame_id = odom
[icp_odometry-3] [INFO] [1692384518.726216037] [icp_odometry]: Odometry: publish_tf = false
[icp_odometry-3] [INFO] [1692384518.726233350] [icp_odometry]: Odometry: wait_for_transform = 0.200000
[icp_odometry-3] [INFO] [1692384518.726253350] [icp_odometry]: Odometry: log_to_rosout_level = 4
[icp_odometry-3] [INFO] [1692384518.726339014] [icp_odometry]: Odometry: initial_pose = xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000
[icp_odometry-3] [INFO] [1692384518.726367910] [icp_odometry]: Odometry: ground_truth_frame_id =
[icp_odometry-3] [INFO] [1692384518.726383206] [icp_odometry]: Odometry: ground_truth_base_frame_id = base_link
[icp_odometry-3] [INFO] [1692384518.726400838] [icp_odometry]: Odometry: config_path =
[icp_odometry-3] [INFO] [1692384518.726412486] [icp_odometry]: Odometry: publish_null_when_lost = false
[icp_odometry-3] [INFO] [1692384518.726422310] [icp_odometry]: Odometry: guess_frame_id =
[icp_odometry-3] [INFO] [1692384518.726432711] [icp_odometry]: Odometry: guess_min_translation = 0.000000
[icp_odometry-3] [INFO] [1692384518.726444071] [icp_odometry]: Odometry: guess_min_rotation = 0.000000
[icp_odometry-3] [INFO] [1692384518.726455111] [icp_odometry]: Odometry: guess_min_time = 0.000000
[icp_odometry-3] [INFO] [1692384518.726465927] [icp_odometry]: Odometry: expected_update_rate = 10.000000 Hz
[icp_odometry-3] [INFO] [1692384518.726479719] [icp_odometry]: Odometry: max_update_rate = 0.000000 Hz
[icp_odometry-3] [INFO] [1692384518.726490407] [icp_odometry]: Odometry: min_update_rate = 0.000000 Hz
[icp_odometry-3] [INFO] [1692384518.726500263] [icp_odometry]: Odometry: wait_imu_to_init = true
[icp_odometry-3] [INFO] [1692384518.726573031] [icp_odometry]: Odometry: stereoParams_=0 visParams_=0 icpParams_=1
[icp_odometry-3] [INFO] [1692384518.849305740] [icp_odometry]: Odometry: Update parameter "Icp/CorrespondenceRatio"="0.01" from arguments
[icp_odometry-3] [INFO] [1692384518.849428204] [icp_odometry]: Odometry: Update parameter "Icp/DownsamplingStep"="1" from arguments
[icp_odometry-3] [INFO] [1692384518.849452716] [icp_odometry]: Odometry: Update parameter "Icp/Epsilon"="0.001" from arguments
[icp_odometry-3] [INFO] [1692384518.849469356] [icp_odometry]: Odometry: Update parameter "Icp/Iterations"="10" from arguments
[icp_odometry-3] [INFO] [1692384518.849482380] [icp_odometry]: Odometry: Update parameter "Icp/MaxCorrespondenceDistance"="1" from arguments
[icp_odometry-3] [INFO] [1692384518.849495436] [icp_odometry]: Odometry: Update parameter "Icp/MaxTranslation"="0.2" from arguments
[icp_odometry-3] [INFO] [1692384518.849506189] [icp_odometry]: Odometry: Update parameter "Icp/OutlierRatio"="0.1" from arguments
[icp_odometry-3] [INFO] [1692384518.849518925] [icp_odometry]: Odometry: Update parameter "Icp/PointToPlane"="true" from arguments
[icp_odometry-3] [INFO] [1692384518.849530957] [icp_odometry]: Odometry: Update parameter "Icp/PointToPlaneGroundNormalsUp"="0.8" from arguments
[icp_odometry-3] [INFO] [1692384518.849542733] [icp_odometry]: Odometry: Update parameter "Icp/PointToPlaneK"="20" from arguments
[icp_odometry-3] [INFO] [1692384518.849554093] [icp_odometry]: Odometry: Update parameter "Icp/PointToPlaneRadius"="0" from arguments
[icp_odometry-3] [INFO] [1692384518.849566285] [icp_odometry]: Odometry: Update parameter "Icp/ReciprocalCorrespondences"="False" from arguments
[icp_odometry-3] [INFO] [1692384518.849577581] [icp_odometry]: Odometry: Update parameter "Icp/Strategy"="1" from arguments
[icp_odometry-3] [INFO] [1692384518.849589421] [icp_odometry]: Odometry: Update parameter "Icp/VoxelSize"="0.1" from arguments
[icp_odometry-3] [INFO] [1692384518.849603341] [icp_odometry]: Odometry: Update parameter "Odom/ScanKeyFrameThr"="0.4" from arguments
[icp_odometry-3] [INFO] [1692384518.849617357] [icp_odometry]: Odometry: Update parameter "Odom/Strategy"="0" from arguments
[icp_odometry-3] [INFO] [1692384518.849628621] [icp_odometry]: Odometry: Update parameter "OdomF2M/BundleAdjustment"="false" from arguments
[icp_odometry-3] [INFO] [1692384518.849640269] [icp_odometry]: Odometry: Update parameter "OdomF2M/ScanMaxSize"="15000" from arguments
[icp_odometry-3] [INFO] [1692384518.849651917] [icp_odometry]: Odometry: Update parameter "OdomF2M/ScanSubtractRadius"="0.01" from arguments
[icp_odometry-3] [WARN] [1692384518.850052047] [icp_odometry]: IcpOdometry: Transferring value 0.1 of "Icp/VoxelSize" to ros parameter "scan_voxel_size" for convenience. "Icp/VoxelSize" is set to 0.
[icp_odometry-3] [WARN] [1692384518.850206224] [icp_odometry]: IcpOdometry: Transferring value 20 of "Icp/PointToPlaneK" to ros parameter "scan_normal_k" for convenience.
[icp_odometry-3] [WARN] [1692384518.850249136] [icp_odometry]: IcpOdometry: Transferring value 0.8 of "Icp/PointToPlaneGroundNormalsUp" to ros parameter "scan_normal_ground_up" for convenience.
[point_cloud_assembler-5] [INFO] [1692384518.871664539] [point_cloud_assembler]:
[point_cloud_assembler-5] point_cloud_assembler subscribed to (exact sync):
[point_cloud_assembler-5] cloud,
[point_cloud_assembler-5] odom
[lidar_deskewing-2] [INFO] [1692384518.900542955] [lidar_deskewing]: fixed_frame_id:
[lidar_deskewing-2] [INFO] [1692384518.908342994] [lidar_deskewing]: wait_for_transform: 0.010000s
[lidar_deskewing-2] [INFO] [1692384518.908424723] [lidar_deskewing]: slerp: false
[lidar_deskewing-2] [FATAL] [1692384518.908441779] [lidar_deskewing]: fixed_frame_id parameter cannot be empty!
[icp_odometry-3] [INFO] [1692384518.979276500] [icp_odometry]: odometry: Subscribing to IMU topic /imu
[icp_odometry-3] [INFO] [1692384518.979367124] [icp_odometry]: odometry: qos_imu = 0
[icp_odometry-3] [INFO] [1692384518.996422602] [icp_odometry]: IcpOdometry: qos = 0
[icp_odometry-3] [INFO] [1692384518.998101362] [icp_odometry]: IcpOdometry: scan_cloud_max_points = -1
[icp_odometry-3] [INFO] [1692384518.998233939] [icp_odometry]: IcpOdometry: scan_cloud_is_2d = false
[icp_odometry-3] [INFO] [1692384518.998259667] [icp_odometry]: IcpOdometry: scan_downsampling_step = 1
[icp_odometry-3] [INFO] [1692384518.998273683] [icp_odometry]: IcpOdometry: scan_range_min = 0.000000 m
[icp_odometry-3] [INFO] [1692384518.998293651] [icp_odometry]: IcpOdometry: scan_range_max = 0.000000 m
[icp_odometry-3] [INFO] [1692384518.998305555] [icp_odometry]: IcpOdometry: scan_voxel_size = 0.100000 m
[icp_odometry-3] [INFO] [1692384518.998321651] [icp_odometry]: IcpOdometry: scan_normal_k = 20
[icp_odometry-3] [INFO] [1692384518.998332499] [icp_odometry]: IcpOdometry: scan_normal_radius = 0.000000 m
[icp_odometry-3] [INFO] [1692384518.998343443] [icp_odometry]: IcpOdometry: scan_normal_ground_up = 0.800000
[icp_odometry-3] [INFO] [1692384518.998354323] [icp_odometry]: IcpOdometry: deskewing = true
[icp_odometry-3] [INFO] [1692384518.998364467] [icp_odometry]: IcpOdometry: deskewing_slerp = false
[rtabmap-4] [INFO] [1692384519.022833741] [rtabmap]: rtabmap: Database version = "0.21.2".
[rtabmap-4] [INFO] [1692384519.024498614] [rtabmap]: rtabmap: SLAM mode (Mem/IncrementalMemory=true)
[rtabmap-4] [INFO] [1692384519.618438603] [rtabmap]: Setup callbacks
[rtabmap-4] [INFO] [1692384519.619946707] [rtabmap]: rtabmap: subscribe_depth = false
[rtabmap-4] [INFO] [1692384519.620032115] [rtabmap]: rtabmap: subscribe_rgb = true
[rtabmap-4] [INFO] [1692384519.620053268] [rtabmap]: rtabmap: subscribe_stereo = false
[rtabmap-4] [INFO] [1692384519.620069620] [rtabmap]: rtabmap: subscribe_rgbd = false (rgbd_cameras=1)
[rtabmap-4] [INFO] [1692384519.620087732] [rtabmap]: rtabmap: subscribe_odom_info = false
[rtabmap-4] [INFO] [1692384519.620103604] [rtabmap]: rtabmap: subscribe_user_data = false
[rtabmap-4] [INFO] [1692384519.620118996] [rtabmap]: rtabmap: subscribe_scan = false
[rtabmap-4] [INFO] [1692384519.620133812] [rtabmap]: rtabmap: subscribe_scan_cloud = true
[rtabmap-4] [INFO] [1692384519.620148948] [rtabmap]: rtabmap: subscribe_scan_descriptor = false
[rtabmap-4] [INFO] [1692384519.620164852] [rtabmap]: rtabmap: queue_size = 10
[rtabmap-4] [INFO] [1692384519.620180372] [rtabmap]: rtabmap: qos_image = 0
[rtabmap-4] [INFO] [1692384519.620195476] [rtabmap]: rtabmap: qos_camera_info = 0
[rtabmap-4] [INFO] [1692384519.620210292] [rtabmap]: rtabmap: qos_scan = 0
[rtabmap-4] [INFO] [1692384519.620225716] [rtabmap]: rtabmap: qos_odom = 0
[rtabmap-4] [INFO] [1692384519.620240276] [rtabmap]: rtabmap: qos_user_data = 0
[rtabmap-4] [INFO] [1692384519.620254965] [rtabmap]: rtabmap: approx_sync = false
[rtabmap-4] [INFO] [1692384519.620281557] [rtabmap]: Setup rgb-only callback
[rtabmap-4] [INFO] [1692384519.703688981] [rtabmap]:
[rtabmap-4] rtabmap subscribed to (exact sync):
[rtabmap-4] /odom/ekf \
[rtabmap-4] /zed2i/zed_node/rgb/image_rect_color \
[rtabmap-4] /zed2i/zed_node/rgb/camera_info \
[rtabmap-4] /lidar_points3/deskewed
My launch file is below
`from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition, UnlessCondition from launch_ros.actions import Node
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time')
deskewing = LaunchConfiguration('deskewing')
return LaunchDescription([
# Launch arguments
DeclareLaunchArgument(
'use_sim_time', default_value='false',
description='Use simulation (Gazebo) clock if true'),
DeclareLaunchArgument(
'deskewing', default_value='true',
description='Enable lidar deskewing'),
# Compute quaternion of the IMU
Node(
package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
parameters=[{'use_mag': False,
'world_frame':'enu',
'publish_tf':False}],
remappings=[
('imu/data_raw', '/zed2i/zed_node/imu/data'),
('imu/data', '/zed2i/zed_node/imu/data/filtered') #published after filtering, orientation
]),
Node(
package='rtabmap_util', executable='lidar_deskewing', output='screen',
parameters=[{
'fixed_frame_id':'/zed2i/zed_node/imu/data/filtered',
'slerp':False,
'wait_for_transform':0.01,
}],
remappings=[
('input_cloud', '/lidar_points3')
]),
Node(
package='rtabmap_odom', executable='icp_odometry', output='screen',
parameters=[{
'frame_id':'base_link',
'odom_frame_id':'odom',
'publish_tf':False,
'approx_sync':False,
'wait_for_transform':0.2,
'queue_size':1,
'publish_null_when_lost':False,
'wait_imu_to_init':True,
'expected_update_rate':10.0,
'deskewing':deskewing,
'use_sim_time':use_sim_time,
'deskewing_slerp':False,
}],
remappings=[
('scan_cloud', '/lidar_points3/deskewed'),
('odom','/odom/icp')
],
arguments=[
#ICP Parameters
'Icp/PointToPlane', 'true',
'Icp/Iterations', '10',
'Icp/VoxelSize', '0.1', #default 0.05, wanna see precision
'Icp/Epsilon', '0.001',
'Icp/DownsamplingStep','1',
'Icp/PointToPlaneK', '20',
'Icp/PointToPlaneRadius', '0',
'Icp/MaxTranslation', '0.2', #check with 2 also
'Icp/MaxCorrespondenceDistance', '1', #voxel size res*10
'Icp/CorrespondenceRatio', '0.01',
'Icp/PointToPlaneGroundNormalsUp','0.8',
'Icp/Strategy', '1',
#'Icp/CCSamplingLimit','230400' not sure on computation resources
'Icp/ReciprocalCorrespondences','False',
'Icp/OutlierRatio', '0.1',
#'Icp/PM','True',
#'Icp/PMOutlierRatio','0.1',
#Odom Para
'Odom/ScanKeyFrameThr', '0.4',
'Odom/Strategy', '0', #(F2M)
'OdomF2M/ScanSubtractRadius', '0.01', #resolution
'OdomF2M/ScanMaxSize', '15000',
'OdomF2M/BundleAdjustment', 'false',
#'Odom/FilteringStrategy','2',
]),
Node(
package='rtabmap_slam', executable='rtabmap', output='screen',
parameters=[{
'frame_id':'base_link',
'map_frame_id':'map',
'publish_tf':True,
'subscribe_depth':False,
'subscribe_scan ':False,
'subscribe_scan_cloud':True,
'subscribe_stereo':False, #subscribe_rgb deprecated ?
'subscribe_rgb':True,
'approx_sync':False,
'wait_for_transform':0.2,
'use_sim_time':use_sim_time,
}],
remappings=[
('scan_cloud', '/lidar_points3/deskewed'),
('rgb/image', '/zed2i/zed_node/rgb/image_rect_color'),
('rgb/camera_info', '/zed2i/zed_node/rgb/camera_info'),
('imu', '/zed2i/zed_node/imu/data/filtered'),
('odom', '/odom/ekf') # check
],
arguments=[
'-d', # This will delete the previous database (~/.ros/rtabmap.db)
#RGBD
'Rtabmap/DetectionRate','1',
'RGBD/NeighborLinkRefining','False',
'RGBD/ProximityBySpace','True',
'RGBD/ProximityMaxGraphDepth', '0',
'RGBD/ProximityPathMaxNeighbors', '1',
'RGBD/AngularUpdate', '0.05',
'RGBD/LinearUpdate', '0.05',
'Mem/NotLinkedNodesKept', 'false',
'Mem/STMSize', '30',
'Mem/LaserScanNormalK', '20',
#Grid for 2D Occupancy, Nav2 will use this
'Reg/Strategy', '1',
'Grid/Sensor','1',
'Grid/CellSize','0.01',
'Grid/RangeMax','20',
'Grid/ClusterRadius','1',
'Grid/GroundIsObstacle','True',
'Optimizer/GravitySigma','0.3',
#ICP
'Icp/VoxelSize', '0.1',
'Icp/PointToPlaneK', '20',
'Icp/PointToPlaneRadius', '0',
'Icp/PointToPlane', 'true',
'Icp/Iterations', '10',
'Icp/Epsilon', '0.001',
'Icp/MaxTranslation', '3',
'Icp/MaxCorrespondenceDistance', '1',
#'Icp/PM','True',
#'Icp/PMOutlierRatio','0.7',
'Icp/Strategy', '1',
'Icp/OutlierRatio', '0.1',
'Icp/CorrespondenceRatio', '0.2',
#'Icp/CCFilterOutFarthestPoints','True',
]),
Node(
package='rtabmap_util', executable='point_cloud_assembler', output='screen',
parameters=[{
'queue_size':10,
'max_clouds':10, #if spin rate is changed on LIDAR to 1200 RPM, change this to 20
'fixed_frame_id':'',
'use_sim_time':use_sim_time,
'approx_sync':False,
}],
remappings=[
('cloud', 'odom_filtered_input_scan'),
('odom', '/odom/ekf')
]),
# Node(
# package='rtabmap_viz', executable='rtabmap_viz', output='screen',
# parameters=[{
# 'frame_id':'base_link',
# 'odom_frame_id':'/odom/ekf',
# 'subscribe_odom_info':True,
# 'subscribe_scan_cloud':True,
# 'approx_sync':False,
# 'use_sim_time':use_sim_time,
# }],
# remappings=[
# ('scan_cloud', 'odom_filtered_input_scan')
# ]),
])`
All the data is getting published as on topic echo, and on rate side zed is at 20 hz, EKF is at 10 and desked topic is at 1 hz.
Thank you again !
It looks like imu_to_tf
has not been converted yet to ROS2: https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_util/src/nodelets/imu_to_tf.cpp
I would try "fuse wheel encoders + IMU with EKF node and back to ICP odometry" first.
Changing Icp/CCSamplingLimit
will do nothing if you are not using CloudCompare ICP strategy (Icp/Strategy=2
).
Back to original deskewing example with IMU, you could use your combined IMU+Wheel odometry as fixed frame to do the deskewing instead of using imu_to_tf
. Your EKF filter is probably giving you a TF frame for odometry. Set guess_frame_id
to your wheelodom+IMU TF frame so that icp_odometry publishes TF correctly.
On rtabmap node, it is unlikely that all the input topics are exactly sync, so set approx_sync:=true
.
@matlabbe Thank you for the response, I did all the changes as you mentioned. I am just left with this error on icp_odometry which I feel is something to do my LIDAR header stamp is split into secs and nano sec and your wrapper needs one timestamp but can you confirm as I was looking into Msgconversion.cpp file if there is a way around ?
@matlabbe On the LIDAR, SDK I changed the label to "time" , ended up in next error Time Datatype which on my end is configured as Float64 and wrapper needs Float32, can I add the datatype on the wrapper end to consume datatype of 8 too ? As that is more precise right ?
Yeah, could could change every references of datatype=7 to 8 in deskewing function, by changing also float to double when getting time field in the point cloud.
The header's timestamp (sec,nanosec) is not the same stamp than the one set in the fields, which are time difference from header's stamp. Along fields x
, y
and z
, there should be a field called time
or t
.
Hi Mathieu,
As you said above, If I just do ICP odom, I see odom computed in the logger, without guess_frame_id and not publishing ekf node . But once the ekf and deskewing comes into factor, things go haywire as shown in the tf. map -> odom -> odom_combined -> base_link -> sensors, wheel, camera ? Isn't this correct ?
Node(
package='rtabmap_odom', executable='icp_odometry', output='screen',
parameters=[{
'frame_id':'base_footprint',
'odom_frame_id':'odom',
'guess_frame_id':'',
'publish_tf':True,
'approx_sync':True,
'tf_prefix':'', # add this when you have multiple robots on the same domain
'wait_for_transform':0.2,
'queue_size':2,
'publish_null_when_lost':False,
'expected_update_rate':20.0,
'deskewing':deskewing,
'use_sim_time':use_sim_time,
'deskewing_slerp':False,
}],
remappings=[
('scan_cloud', '/lidar_points3'),
('odom','odom')
],
arguments=[
#ICP
'Icp/PointToPlane', 'true',
'Icp/Iterations', '10',
'Icp/VoxelSize', '0.1', #for 3d lidar, 0.2 is good,
'Icp/Epsilon', '0.001',
'Icp/DownsamplingStep','1',
'Icp/PointToPlaneK', '20',
'Icp/PointToPlaneRadius', '0',
'Icp/MaxTranslation', '2', #check with 2 also
'Icp/MaxCorrespondenceDistance', '1', #voxel size res*10
'Icp/CorrespondenceRatio', '0.01',
'Icp/PointToPlaneGroundNormalsUp','0.8',
'Icp/Strategy', '1',
'Icp/ReciprocalCorrespondences','False',
'Icp/OutlierRatio', '0.7',
#Odom
'Odom/ScanKeyFrameThr', '0.4',
'Odom/Strategy', '0', #(F2M)
'OdomF2M/ScanSubtractRadius', '0.1', #resolution
'OdomF2M/ScanMaxSize', '15000',
'OdomF2M/BundleAdjustment', 'false',
#'Odom/FilteringStrategy','2',
]),
# Node(
# package='rtabmap_sync', executable='rgb_sync', output='screen',
# remappings=[
# ('rgb/image', '/zed2i/zed_node/rgb/image_rect_color'),
# ('rgb/camera_info','/zed2i/zed_node/rgb/camera_info')
# ],),
Node(
package='rtabmap_util', executable='point_cloud_assembler', output='screen',
parameters=[{
'queue_size':10,
'max_clouds':5, #if spin rate is changed on LIDAR to 1200 RPM, change this to 20
'fixed_frame_id':'',
'use_sim_time':use_sim_time,
'approx_sync':True,
}],
remappings=[
('cloud', 'odom_filtered_input_scan'),
('odom', 'odom')
]),
Node(
package='rtabmap_slam', executable='rtabmap', output='screen',
parameters=[{
'frame_id':'base_footprint',
'map_frame_id':'map',
'publish_tf':True,
'subscribe_depth':False,
'subscribe_scan ':False,
'subscribe_scan_cloud':True,
'subscribe_stereo':False,
'subscribe_rgb':False,
'approx_sync':True,
'wait_for_transform':0.2,
'odom_frame_id':'odom',
'queue_size':10,
'tf_prefix':'', #use this for multiple robots
'use_sim_time':use_sim_time,
}],
remappings=[
('scan_cloud', 'assembled_cloud'),
('odom','odom')
# ('rgb/image', '/zed2i/zed_node/rgb/image_rect_color'),
# ('rgb/camera_info', '/zed2i/zed_node/rgb/camera_info'),
# ('grid_map','/map') # for Nav2
],
arguments=[
'-d',
'--udebug', # This will delete the previous database (~/.ros/rtabmap.db)
#RGBD
#'Rtabmap/DetectionRate','1',
#'RGBD/NeighborLinkRefining','False',
#'RGBD/ProximityBySpace','True',
'RGBD/ProximityMaxGraphDepth', '0',
'RGBD/ProximityPathMaxNeighbors', '1',
'RGBD/AngularUpdate', '0.05',
'RGBD/LinearUpdate', '0.05',
'Mem/NotLinkedNodesKept', 'false',
'Mem/STMSize', '30',
'Mem/LaserScanNormalK', '20',
#Grid for 2D Occupancy, Nav2 will use this
'Reg/Strategy', '1',
'Grid/FromDepth','true',
'Grid/Sensor','1',
'Grid/CellSize','0.01',
'Grid/RangeMax','20',
'Grid/ClusterRadius','1',
'Grid/GroundIsObstacle','True',
'Optimizer/GravitySigma','0.3',
#ICP
'Icp/VoxelSize', '0.1',
'Icp/PointToPlaneK', '20',
'Icp/PointToPlaneRadius', '0',
'Icp/PointToPlane', 'true',
'Icp/Iterations', '10',
'Icp/Epsilon', '0.001',
'Icp/MaxTranslation', '3',
'Icp/MaxCorrespondenceDistance', '1',
'Icp/Strategy', '1',
'Icp/OutlierRatio', '0.7',
'Icp/CorrespondenceRatio', '0.2',
]),
above is without setting reset,
After setting Odom/ResetCountdown = 1, I started seeing Odom being computed.
Also on a side note, is ICP odometry estimation better with wheel encoders + IMU OR using ORB_SLAM2 odom better to feed to SLAM node in your experience ?
Also Reg/Strategy = "0" [0=Vis, 1=Icp, 2=VisIcp], if set to 1, does loop closure happen with Vis-> ICP or do i need to set it to "3" ?
Hi Mathieu,
I tried just to launch lidar deskewing and icp_odomery and I get these 2 errors,
Just ICP works fine as mentioned above, any inputs please ?
Below is lidar data
Thanks
For that post:
'guess_frame_id':''
, so if icp_odometry gets lost, it won't recover unless you enable auto reset. If guess_frame_id
is set to odom_combined
, you will get indeed odom -> odom_combined
published by icp_odometry.Reg/Strategy=1
will use vision for initial guess for icp for global loop closure detection. For proximity detection, it will be done locally only with lidar. I never use Reg/Strategy=2
.For that post:
timestamp
should be the offset in seconds from the stamp in the header. In the error msg, the resulting stamps are negative. I would try to print the resulting offset read from the PointCloud data here.
Hello,
I went through the code, and this was my finding. For ROS2 side where the FLOAM was ported over to foxy by Chris, did git clone
https://github.com/Chris7462/floam/tree/master/src
In RTABMAP I got error for not finding files, as the ROS1 side the original FLOAM had no "_" in naming convention for src/*files.
My cmake at FLOAM side