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

RTABMAP_FLOAM Integration for ROS2 #1013

Closed anath93 closed 11 months ago

anath93 commented 1 year ago

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.

image

My cmake at FLOAM side 

cmake_minimum_required(VERSION 3.5)
project(floam)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread")

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(PCL REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(Ceres REQUIRED)

include_directories(
  include
  ${PCL_INCLUDE_DIRS}
  ${EIGEN3_INCLUDE_DIR}
  ${CERES_INCLUDE_DIRS}
)

link_directories(
  ${PCL_LIBRARY_DIRS}
  ${CERES_LIBRARY_DIRS}
)

#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)

add_executable(laser_processing_node
  src/laser_processing_node.cpp
  src/laser_processing.cpp
  src/laser_processing_class.cpp
  src/lidar.cpp
)

ament_target_dependencies(laser_processing_node
  rclcpp
  sensor_msgs
  pcl_conversions
  Eigen3
  CERES
)

target_link_libraries(laser_processing_node
  ${PCL_LIBRARIES}
)

add_executable(odom_estimation_node
  src/odom_estimation_node.cpp
  src/odom_estimation.cpp
  src/odom_estimation_class.cpp
  src/lidar_optimization.cpp
  src/lidar.cpp
)

ament_target_dependencies(odom_estimation_node
  rclcpp
  sensor_msgs
  nav_msgs
  geometry_msgs
  tf2
  tf2_ros
  pcl_conversions
  Eigen3
  CERES
)

target_link_libraries(odom_estimation_node
  ${PCL_LIBRARIES}
  ${EIGEN3_LIBRARIES}
  ${CERES_LIBRARIES}
)

add_executable(laser_mapping_node
  src/laser_mapping_node.cpp
  src/laser_mapping.cpp
  src/laser_mapping_class.cpp
  src/lidar.cpp
)

ament_target_dependencies(laser_mapping_node
  rclcpp
  sensor_msgs
  nav_msgs
  pcl_conversions
  tf2
  tf2_ros
  tf2_geometry_msgs
  Eigen3
  CERES
)

target_link_libraries(laser_mapping_node
  ${PCL_LIBRARIES}
)

install(TARGETS laser_processing_node odom_estimation_node laser_mapping_node
  DESTINATION lib/${PROJECT_NAME}
)

install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)

install(DIRECTORY rviz
  DESTINATION share/${PROJECT_NAME}
)

install(DIRECTORY params
  DESTINATION share/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()
anath93 commented 1 year 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.

matlabbe commented 1 year ago

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.

anath93 commented 1 year ago

@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 !

matlabbe commented 1 year ago

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.

anath93 commented 1 year ago

@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 ?

image

image

image

image

anath93 commented 1 year ago

@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 ?

matlabbe commented 1 year ago

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.

anath93 commented 1 year ago

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 ?

image

image

  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. image

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" ?

anath93 commented 12 months ago

Hi Mathieu,

I tried just to launch lidar deskewing and icp_odomery and I get these 2 errors,

image

image

image

Just ICP works fine as mentioned above, any inputs please ? Below is lidar data image image

Thanks

matlabbe commented 11 months ago

For that post:

For that post: