D435I IMU ISSUE
When you install the d435i camera package and run roslaunch realsense2_camera opensource_tracking.launch, the mapping is not smooth and it is too slow.
I don't think I can catch the imu there, but I'm not sure if it's because of that, so I'm leaving the terminal log below.
setting /run_id to 1ae7c65a-12e5-11ed-bd9f-292678edc280
process[rosout-1]: started with pid [44067]
started core service [/rosout]
process[camera/realsense2_camera_manager-2]: started with pid [44075]
process[camera/realsense2_camera-3]: started with pid [44076]
process[ImuFilter-4]: started with pid [44077]
process[rtabmap/rgbd_odometry-5]: started with pid [44078]
process[rtabmap/rtabmap-6]: started with pid [44088]
process[rviz-7]: started with pid [44090]
process[points_xyzrgb-8]: started with pid [44091]
process[ukf_se-9]: started with pid [44093]
type is rtabmap_ros/point_cloud_xyzrgb
[ WARN] [1659501073.472999052]: Both imu0_differential and imu0_relative were set to true. Using differential mode.
[ WARN] [1659501073.475375549]: Warning: Some linear velocity entries in parameter imu0_config are listed true, but an sensor_msgs/Imu contains no information about linear velocities
[ INFO] [1659501073.521826902]: Initializing nodelet with 8 worker threads.
[ WARN] [1659501073.578257700]: Failed to meet update rate! Took 0.0082085680000000011552
[ INFO] [1659501073.586326436]: Starting node...
[ WARN] [1659501073.600582700]: Failed to meet update rate! Took 0.01722254299999999988
[ WARN] [1659501073.600652061]: Failed to meet update rate! Took 0.013985231000000000745
[ INFO] [1659501073.619516946]: RealSense ROS v2.3.2
[ INFO] [1659501073.619565730]: Built with LibRealSense v2.50.0
[ INFO] [1659501073.619586787]: Running with LibRealSense v2.50.0
[ INFO] [1659501073.646878534]:
03/08 13:31:13,711 WARNING [140227212867328] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: Resource temporarily unavailable, number: b
[ INFO] [1659501073.725127025]: Initializing nodelet with 8 worker threads.
[ INFO] [1659501073.772116836]: Initializing nodelet with 8 worker threads.
[ INFO] [1659501073.863081337]: /rtabmap/rtabmap(maps): map_filter_radius = 0.000000
[ INFO] [1659501073.863120978]: /rtabmap/rtabmap(maps): map_filter_angle = 30.000000
[ INFO] [1659501073.863133690]: /rtabmap/rtabmap(maps): map_cleanup = true
[ INFO] [1659501073.863144224]: /rtabmap/rtabmap(maps): map_always_update = false
[ INFO] [1659501073.863156939]: /rtabmap/rtabmap(maps): map_empty_ray_tracing = true
[ INFO] [1659501073.863166718]: /rtabmap/rtabmap(maps): cloud_output_voxelized = true
[ INFO] [1659501073.863177131]: /rtabmap/rtabmap(maps): cloud_subtract_filtering = false
[ INFO] [1659501073.863189745]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1659501073.864169937]: Odometry: frame_id = camera_link
[ INFO] [1659501073.864193523]: Odometry: odom_frame_id = odom
[ INFO] [1659501073.864202910]: Odometry: publish_tf = true
[ INFO] [1659501073.864211341]: Odometry: wait_for_transform = true
[ INFO] [1659501073.864225656]: Odometry: wait_for_transform_duration = 0.200000
[ INFO] [1659501073.864253512]: Odometry: initial_pose = xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000
[ INFO] [1659501073.864262775]: Odometry: ground_truth_frame_id =
[ INFO] [1659501073.864271294]: Odometry: ground_truth_base_frame_id =
[ INFO] [1659501073.864279674]: Odometry: config_path =
[ INFO] [1659501073.864288306]: Odometry: publish_null_when_lost = true
[ INFO] [1659501073.864298940]: Odometry: guess_frame_id =
[ INFO] [1659501073.864307282]: Odometry: guess_min_translation = 0.000000
[ INFO] [1659501073.864315449]: Odometry: guess_min_rotation = 0.000000
[ INFO] [1659501073.864323445]: Odometry: guess_min_time = 0.000000
[ INFO] [1659501073.864332531]: Odometry: expected_update_rate = 0.000000 Hz
[ INFO] [1659501073.864340590]: Odometry: max_update_rate = 0.000000 Hz
[ INFO] [1659501073.864348385]: Odometry: wait_imu_to_init = false
[ INFO] [1659501073.864370486]: Odometry: stereoParams_=0 visParams_=1 icpParams_=0
[ INFO] [1659501073.865009498]: /rtabmap/rtabmap(maps): octomap_tree_depth = 16
[ INFO] [1659501073.879839789]: Approximate time sync = true
[ INFO] [1659501073.908822114]: Device with serial number 040322073715 was found.
[ INFO] [1659501073.908893001]: Device with physical ID 3-2-5 was found.
[ INFO] [1659501073.908916777]: Device with name Intel RealSense D435I was found.
[ INFO] [1659501073.909327005]: Device with port number 3-2 was found.
[ INFO] [1659501073.909382698]: Device USB type: 2.1
[ WARN] [1659501073.909398538]: Device 040322073715 is connected using a 2.1 port. Reduced performance is expected.
[ INFO] [1659501073.914606252]: getParameters...
[ INFO] [1659501073.927851254]: rtabmap: frame_id = camera_link
[ INFO] [1659501073.927886942]: rtabmap: map_frame_id = map
[ INFO] [1659501073.927900627]: rtabmap: use_action_for_goal = false
[ INFO] [1659501073.927916449]: rtabmap: tf_delay = 0.050000
[ INFO] [1659501073.927926817]: rtabmap: tf_tolerance = 0.100000
[ INFO] [1659501073.927936041]: rtabmap: odom_sensor_sync = false
[ INFO] [1659501073.928807361]: rtabmap: gen_scan = false
[ INFO] [1659501073.928835051]: rtabmap: gen_depth = false
[ INFO] [1659501074.004999010]: setupDevice...
[ INFO] [1659501074.005018932]: JSON file is not provided
[ INFO] [1659501074.005026479]: ROS Node Namespace: camera
[ INFO] [1659501074.005036858]: Device Name: Intel RealSense D435I
[ INFO] [1659501074.005052499]: Device Serial No: 040322073715
[ INFO] [1659501074.005058749]: Device physical port: 3-2-5
[ INFO] [1659501074.005067390]: Device FW version: 05.13.00.50
[ INFO] [1659501074.005077388]: Device Product ID: 0x0B3A
[ INFO] [1659501074.005085824]: Enable PointCloud: Off
[ INFO] [1659501074.005099639]: Align Depth: On
[ INFO] [1659501074.005108114]: Sync Mode: On
[ INFO] [1659501074.005149424]: Device Sensors:
[ INFO] [1659501074.070161615]: Stereo Module was found.
[ INFO] [1659501074.079177907]: RGB Camera was found.
[ INFO] [1659501074.079295572]: Motion Module was found.
[ INFO] [1659501074.079330229]: (Confidence, 0) sensor isn't supported by current device! -- Skipping...
[ INFO] [1659501074.079610319]: num_filters: 1
[ INFO] [1659501074.079632366]: Setting Dynamic reconfig parameters.
[ INFO] [1659501074.207949131]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="true"
[ INFO] [1659501074.208212529]: Setting RTAB-Map parameter "Mem/InitWMWithAllNodes"="false"
[ INFO] [1659501074.470123315]: RGBDOdometry: approx_sync = true
[ INFO] [1659501074.470151945]: RGBDOdometry: queue_size = 10
[ INFO] [1659501074.470158520]: RGBDOdometry: subscribe_rgbd = false
[ INFO] [1659501074.470181676]: RGBDOdometry: rgbd_cameras = 1
[ INFO] [1659501074.470194725]: RGBDOdometry: keep_color = false
[ INFO] [1659501074.477510948]:
/rtabmap/rgbd_odometry subscribed to (approx sync):
/camera/color/image_raw \
/camera/aligned_depth_to_color/image_raw \
/camera/color/camera_info
[ INFO] [1659501074.643682093]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1659501074.644061822]: rtabmap: Deleted database "/home/aidl/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[ INFO] [1659501074.644096161]: rtabmap: Using database from "/home/aidl/.ros/rtabmap.db" (0 MB).
[ INFO] [1659501074.903346382]: rtabmap: Database version = "0.20.18".
[ INFO] [1659501074.903374508]: rtabmap: SLAM mode (Mem/IncrementalMemory=true)
[ INFO] [1659501074.917838868]: /rtabmap/rtabmap: subscribe_depth = true
[ INFO] [1659501074.917867095]: /rtabmap/rtabmap: subscribe_rgb = true
[ INFO] [1659501074.917881246]: /rtabmap/rtabmap: subscribe_stereo = false
[ INFO] [1659501074.917891296]: /rtabmap/rtabmap: subscribe_rgbd = false (rgbd_cameras=1)
[ INFO] [1659501074.917899422]: /rtabmap/rtabmap: subscribe_odom_info = true
[ INFO] [1659501074.917907693]: /rtabmap/rtabmap: subscribe_user_data = false
[ INFO] [1659501074.917916457]: /rtabmap/rtabmap: subscribe_scan = false
[ INFO] [1659501074.917926038]: /rtabmap/rtabmap: subscribe_scan_cloud = false
[ INFO] [1659501074.917933337]: /rtabmap/rtabmap: subscribe_scan_descriptor = false
[ INFO] [1659501074.917941242]: /rtabmap/rtabmap: queue_size = 10
[ INFO] [1659501074.917947789]: /rtabmap/rtabmap: approx_sync = true
[ INFO] [1659501074.917983310]: Setup depth callback
[ INFO] [1659501074.925377198]:
/rtabmap/rtabmap subscribed to (approx sync):
/rtabmap/odom \
/camera/color/image_raw \
/camera/aligned_depth_to_color/image_raw \
/camera/color/camera_info \
/rtabmap/odom_info
[ INFO] [1659501075.001063525]: rtabmap 0.20.18 started...
03/08 13:31:15,911 WARNING [140227212867328] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: Resource temporarily unavailable, number: b
03/08 13:31:16,155 WARNING [140227212867328] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: Resource temporarily unavailable, number: b
03/08 13:31:16,483 WARNING [140227212867328] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: Resource temporarily unavailable, number: b
[ INFO] [1659501076.914902247]: Done Setting Dynamic reconfig parameters.
[ INFO] [1659501076.915277823]: depth stream is enabled - width: 640, height: 480, fps: 15, Format: Z16
[ INFO] [1659501076.915577235]: color stream is enabled - width: 640, height: 480, fps: 15, Format: RGB8
[ INFO] [1659501076.916491261]: setupPublishers...
[ INFO] [1659501076.918430633]: Expected frequency for depth = 15.00000
[ INFO] [1659501076.942619820]: Expected frequency for color = 15.00000
[ INFO] [1659501076.959201378]: Expected frequency for aligned_depth_to_color = 15.00000
[ INFO] [1659501076.974834013]: setupStreams...
03/08 13:31:17,137 WARNING [140227212867328] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[ INFO] [1659501077.188326884]: SELECTED BASE:Depth, 0
[ INFO] [1659501077.195569029]: RealSense Node Is Up!
03/08 13:31:18,036 WARNING [140227212867328] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: Resource temporarily unavailable, number: b
03/08 13:31:18,087 WARNING [140227212867328] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: Resource temporarily unavailable, number: b
03/08 13:31:18,137 WARNING [140227212867328] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: Resource temporarily unavailable, number: b
[ WARN] [1659501078.155695972]:
03/08 13:31:18,188 WARNING [140227212867328] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
03/08 13:31:18,240 WARNING [140227212867328] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
03/08 13:31:18,291 WARNING [140227212867328] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
03/08 13:31:18,341 WARNING [140227212867328] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[ INFO] [1659501078.379051835]: Odom: quality=0, std dev=99.995000m|99.995000rad, update time=0.008447s
Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame camera_link at time 1659501078.103517 according to authority unknown_publisher
at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp
Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame camera_link at time 1659501078.103517 according to authority unknown_publisher
at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp
Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame camera_link at time 1659501078.103517 according to authority unknown_publisher
at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp
[ WARN] [1659501078.380979953]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame camera_link at time 1659501078.103517 according to authority unknown_publisher
03/08 13:31:18,391 WARNING [140227212867328] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[ INFO] [1659501078.398662862]: rtabmap (1): Rate=1.00s, Limit=0.000s, Conversion=0.0014s, RTAB-Map=0.0174s, Maps update=0.0002s pub=0.0000s (local map=1, WM=1)
03/08 13:31:18,442 WARNING [140227212867328] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
03/08 13:31:18,492 WARNING [140227212867328] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: Resource temporarily unavailable, number: b
[ WARN] (2022-08-03 13:31:18.524) OdometryF2M.cpp:525::computeTransform() Registration failed:
-
Hi Kimda0 The log states that your camera is being detected as being on a USB 2.1 connection instead of USB 3, which may reduce performance as a USB 2.1 connection is slower.
[INFO] [1659501073.909382698]: Device USB type: 2.1
[ WARN] [1659501073.909398538]: Device 040322073715 is connected using a 2.1 port. Reduced performance is expected.
The documentation of Intel's D435i SLAM guide for ROS that uses opensource_tracking.launch also cautions against making fast turns or movement.
https://github.com/IntelRealSense/realsense-ros/wiki/SLAM-with-D435i
**********
The built-in IMU can only keep track for a very short time. Moving or turning too quickly will break the sequence of successful point cloud matches and will result in the system losing track. It could happen that the system will recover immediately if stopped moving but if not, the longer the time passed since the break, the farther away it will drift from the correct position. The odds for recovery get very slim, very quickly. The parameters set in the launch file are most likely not ideal but this is a good starting point for calibrating
-
Thank you for your answer. But even when the cam is not moving at all, the current position on the rviz rotates here and there.
And is it an error in rtabmap that I wrote terminal warning below?
May I know the solution?03/08 17:38:47,127 WARNING [140051023390464] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: Resource temporarily unavailable, number: b
[ WARN] [1659515927.558956940]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10).
/rtabmap/rtabmap subscribed to (approx sync):
/rtabmap/odom \
/camera/color/image_raw \
/camera/aligned_depth_to_color/image_raw \
/camera/color/camera_info \
/rtabmap/odom_info
[ WARN] [1659515932.078102902]: /rtabmap/rgbd_odometry: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set.
/rtabmap/rgbd_odometry subscribed to (approx sync):
/camera/color/image_raw \
/camera/aligned_depth_to_color/image_raw \
/camera/color/camera_info
[ WARN] [1659515932.559120776]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10).
/rtabmap/rtabmap subscribed to (approx sync):
/rtabmap/odom \
/camera/color/image_raw \
/camera/aligned_depth_to_color/image_raw \
/camera/color/camera_info \
/rtabmap/odom_info
[ WARN] [1659515936.199965931]: Still waiting for data on topics /imu/data_raw and /imu/mag...
[ WARN] [1659515937.078309989]: /rtabmap/rgbd_odometry: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set.
/rtabmap/rgbd_odometry subscribed to (approx sync):
/camera/color/image_raw \
/camera/aligned_depth_to_color/image_raw \
/camera/color/camera_info
[ WARN] [1659515937.559328997]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10).
/rtabmap/rtabmap subscribed to (approx sync):
/rtabmap/odom \
/camera/color/image_raw \
/camera/aligned_depth_to_color/image_raw \
/camera/color/camera_info \
/rtabmap/odom_info
[ WARN] [1659515942.078490458]: /rtabmap/rgbd_odometry: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set.
/rtabmap/rgbd_odometry subscribed to (approx sync):
/camera/color/image_raw \
/camera/aligned_depth_to_color/image_raw \
/camera/color/camera_info
[ WARN] [1659515942.559533702]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10).
/rtabmap/rtabmap subscribed to (approx sync):
/rtabmap/odom \
/camera/color/image_raw \
/camera/aligned_depth_to_color/image_raw \
/camera/color/camera_info \
/rtabmap/odom_info -
Hello Kimda0 , I'm having the same problem with a d455, that the rviz position moves even with a still camera. How did you manage to solve that problem? Thank you.
-
Hi Ian Riera IMU data can be 'noisy' and prone to fluctuation. You can use a Python-based IMU calibration tool (at the link below) to calibrate the IMU and save the result to the camera hardware.
https://github.com/IntelRealSense/librealsense/tree/master/tools/rs-imu-calibration
-
Hi MartyG, thanks for the tutorial, it is well explained and quite smart solution (using the box). However, the rtabmap keeps moving even with the still camera, specially when scanning flat homogeneous surfaces (where it is expected to struggle).
-
If the camera is observing a low-detail scene with rtabmap_ros then the warning "Not enough inliers" may occur. Have you experienced that warning, please? If you have then stability of tracking may improve if you provide the camera with an "interesting" scene to look at (i.e one that is not too empty of objects and surfaces) and to keep the camera looking at such a scene.
A RealSense ROS wrapper developer also advised switching to rtabmap's pointcloud and using topic /voxel_cloud and frame_id map
An rtabmap_ros guide at the link below provides an example of how to switch to /voxel_cloud.
http://wiki.ros.org/rtabmap_ros/Tutorials/SetupOnYourRobot#rviz-1
Please sign in to leave a comment.
Comments
8 comments