search

introlab / rtabmap_ros

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

The frame convention of the latest realsense SDK is not the same as the config in vins-fusion reposirory. #517

Open TouchDeeper opened 3 years ago

TouchDeeper commented 3 years ago

Hi, Mathieu

I used the test_d435i_vio.launch to run vins-fusion supported rtabmap. An error occured: [ERROR] [1608127730.914892966]: The stereo baseline (-0.049985) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo setup where the Tx (or P(0,3)) is negative in the right camera info msg.

I find the realsense_stereo_imu_config.yaml in VINS-FUSION has a different frame convention with my realsense d435i, the default frame convention of realsense_stereo_imu_config.yaml is: QQ图片20201216222431 The frame convention of my realsense d435i is below: QQ图片20201216222803 Screenshot from 2020-12-16 21-49-11 you can note that the body frame convention is different with the config in VINS-FUSION. I can run VINS-FUSION well with my config calibrated by Kalibr. Then I use my config to run test_d435i_vio.launch, but errors about stereo baseline occur. My config realsense_stereo_imu_config_me.yaml is (omit the same content with the default config):

body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 0.999917,   -0.0127019,   0.0020851,  -0.00182752,
           0.0126912,   0.999907,    0.00506861,  0.0093345,
          -0.00214928, -0.00504173,  0.999985,    0.0200701,
             0,          0,          0,           1 ]

body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 0.999921 ,  -0.0123262,   0.0025727,    0.048313,
           0.0123166,   0.999917,    0.00371045,   0.0101984,
          -0.00261822, -0.00367847,  0.99999,      0.0210006,
             0,          -0,          -0,           1 ]

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.1         # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 0.05        # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 7.1765713730075628e-04         # accelerometer bias random work noise standard deviation.  #0.002
gyr_w: 4.0e-05       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.805         # gravity magnitude

The test_d435i_vio.launch is the same as yours except I use my own config realsense_stereo_imu_config_me.yaml

The SDK information is : Required Info
Camera Model D400
Firmware Version 05.12.09.00
Operating System & Version Ubuntu 16
Kernel Version (Linux Only) 4.15.0-128-generic
ros Version kinetic
Platform PC
SDK Version 2.40.0
Language C
Segment Robot
installation Method backend installation refer to https://github.com/IntelRealSense/librealsense/blob/master/doc/libuvc_installation.md

Could you please provide some advice? Thank you a lot! The output information of test_d435i_vio.launch is below:

PARAMETERS
 * /camera/realsense2_camera/accel_fps: 250
 * /camera/realsense2_camera/accel_frame_id: camera_accel_frame
 * /camera/realsense2_camera/accel_optical_frame_id: camera_accel_opti...
 * /camera/realsense2_camera/align_depth: True
 * /camera/realsense2_camera/aligned_depth_to_color_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_fisheye1_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_fisheye2_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_fisheye_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_infra1_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_infra2_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/allow_no_texture_points: False
 * /camera/realsense2_camera/base_frame_id: camera_link
 * /camera/realsense2_camera/calib_odom_file: 
 * /camera/realsense2_camera/clip_distance: -2.0
 * /camera/realsense2_camera/color_fps: 30
 * /camera/realsense2_camera/color_frame_id: camera_color_frame
 * /camera/realsense2_camera/color_height: 480
 * /camera/realsense2_camera/color_optical_frame_id: camera_color_opti...
 * /camera/realsense2_camera/color_width: 640
 * /camera/realsense2_camera/depth_fps: 30
 * /camera/realsense2_camera/depth_frame_id: camera_depth_frame
 * /camera/realsense2_camera/depth_height: 480
 * /camera/realsense2_camera/depth_optical_frame_id: camera_depth_opti...
 * /camera/realsense2_camera/depth_width: 640
 * /camera/realsense2_camera/device_type: 
 * /camera/realsense2_camera/enable_accel: True
 * /camera/realsense2_camera/enable_color: True
 * /camera/realsense2_camera/enable_depth: True
 * /camera/realsense2_camera/enable_fisheye1: False
 * /camera/realsense2_camera/enable_fisheye2: False
 * /camera/realsense2_camera/enable_fisheye: False
 * /camera/realsense2_camera/enable_gyro: True
 * /camera/realsense2_camera/enable_infra1: True
 * /camera/realsense2_camera/enable_infra2: True
 * /camera/realsense2_camera/enable_infra: False
 * /camera/realsense2_camera/enable_pointcloud: False
 * /camera/realsense2_camera/enable_pose: False
 * /camera/realsense2_camera/enable_sync: True
 * /camera/realsense2_camera/filters: 
 * /camera/realsense2_camera/fisheye1_frame_id: camera_fisheye1_f...
 * /camera/realsense2_camera/fisheye1_optical_frame_id: camera_fisheye1_o...
 * /camera/realsense2_camera/fisheye2_frame_id: camera_fisheye2_f...
 * /camera/realsense2_camera/fisheye2_optical_frame_id: camera_fisheye2_o...
 * /camera/realsense2_camera/fisheye_fps: 30
 * /camera/realsense2_camera/fisheye_frame_id: camera_fisheye_frame
 * /camera/realsense2_camera/fisheye_height: 480
 * /camera/realsense2_camera/fisheye_optical_frame_id: camera_fisheye_op...
 * /camera/realsense2_camera/fisheye_width: 640
 * /camera/realsense2_camera/gyro_fps: 400
 * /camera/realsense2_camera/gyro_frame_id: camera_gyro_frame
 * /camera/realsense2_camera/gyro_optical_frame_id: camera_gyro_optic...
 * /camera/realsense2_camera/imu_optical_frame_id: camera_imu_optica...
 * /camera/realsense2_camera/infra1_frame_id: camera_infra1_frame
 * /camera/realsense2_camera/infra1_optical_frame_id: camera_infra1_opt...
 * /camera/realsense2_camera/infra2_frame_id: camera_infra2_frame
 * /camera/realsense2_camera/infra2_optical_frame_id: camera_infra2_opt...
 * /camera/realsense2_camera/infra_fps: 30
 * /camera/realsense2_camera/infra_height: 480
 * /camera/realsense2_camera/infra_rgb: False
 * /camera/realsense2_camera/infra_width: 640
 * /camera/realsense2_camera/initial_reset: False
 * /camera/realsense2_camera/json_file_path: /home/td/realsens...
 * /camera/realsense2_camera/linear_accel_cov: 0.01
 * /camera/realsense2_camera/odom_frame_id: camera_odom_frame
 * /camera/realsense2_camera/pointcloud_texture_index: 0
 * /camera/realsense2_camera/pointcloud_texture_stream: RS2_STREAM_COLOR
 * /camera/realsense2_camera/pose_frame_id: camera_pose_frame
 * /camera/realsense2_camera/pose_optical_frame_id: camera_pose_optic...
 * /camera/realsense2_camera/publish_odom_tf: True
 * /camera/realsense2_camera/publish_tf: True
 * /camera/realsense2_camera/rosbag_filename: 
 * /camera/realsense2_camera/serial_no: 
 * /camera/realsense2_camera/tf_publish_rate: 0.0
 * /camera/realsense2_camera/topic_odom_in: odom_in
 * /camera/realsense2_camera/unite_imu_method: copy
 * /camera/realsense2_camera/usb_port_id: 
 * /imu_filter_node/publish_tf: False
 * /imu_filter_node/use_mag: False
 * /imu_filter_node/world_frame: enu
 * /rosdistro: kinetic
 * /rosversion: 1.12.17
 * /rtabmap/rtabmap/Mem/IncrementalMemory: true
 * /rtabmap/rtabmap/Mem/InitWMWithAllNodes: false
 * /rtabmap/rtabmap/RGBD/OptimizeMaxError: 0
 * /rtabmap/rtabmap/RGBD/OptimizeRobust: true
 * /rtabmap/rtabmap/RGBD/OptimizeStrategy: 1
 * /rtabmap/rtabmap/approx_sync: False
 * /rtabmap/rtabmap/config_path: 
 * /rtabmap/rtabmap/database_path: ~/.ros/rtabmap.db
 * /rtabmap/rtabmap/frame_id: camera_link
 * /rtabmap/rtabmap/ground_truth_base_frame_id: 
 * /rtabmap/rtabmap/ground_truth_frame_id: 
 * /rtabmap/rtabmap/landmark_angular_variance: 9999.0
 * /rtabmap/rtabmap/landmark_linear_variance: 0.0001
 * /rtabmap/rtabmap/map_frame_id: map
 * /rtabmap/rtabmap/odom_frame_id: 
 * /rtabmap/rtabmap/odom_sensor_sync: False
 * /rtabmap/rtabmap/odom_tf_angular_variance: 1.0
 * /rtabmap/rtabmap/odom_tf_linear_variance: 1.0
 * /rtabmap/rtabmap/publish_tf: True
 * /rtabmap/rtabmap/queue_size: 10
 * /rtabmap/rtabmap/scan_cloud_max_points: 0
 * /rtabmap/rtabmap/subscribe_depth: True
 * /rtabmap/rtabmap/subscribe_rgb: True
 * /rtabmap/rtabmap/subscribe_rgbd: False
 * /rtabmap/rtabmap/subscribe_scan: False
 * /rtabmap/rtabmap/subscribe_scan_cloud: False
 * /rtabmap/rtabmap/subscribe_scan_descriptor: False
 * /rtabmap/rtabmap/subscribe_stereo: False
 * /rtabmap/rtabmap/subscribe_user_data: False
 * /rtabmap/rtabmap/wait_for_transform_duration: 0.2
 * /rtabmap/rtabmapviz/approx_sync: False
 * /rtabmap/rtabmapviz/frame_id: camera_link
 * /rtabmap/rtabmapviz/odom_frame_id: 
 * /rtabmap/rtabmapviz/queue_size: 10
 * /rtabmap/rtabmapviz/subscribe_depth: True
 * /rtabmap/rtabmapviz/subscribe_rgbd: False
 * /rtabmap/rtabmapviz/subscribe_scan: False
 * /rtabmap/rtabmapviz/subscribe_scan_cloud: False
 * /rtabmap/rtabmapviz/subscribe_scan_descriptor: False
 * /rtabmap/rtabmapviz/subscribe_stereo: False
 * /rtabmap/rtabmapviz/wait_for_transform_duration: 0.2
 * /rtabmap/stereo_odometry/frame_id: camera_link
 * /rtabmap/stereo_odometry/wait_imu_to_init: True

NODES
  /camera/
    realsense2_camera (nodelet/nodelet)
    realsense2_camera_manager (nodelet/nodelet)
  /rtabmap/
    rtabmap (rtabmap_ros/rtabmap)
    rtabmapviz (rtabmap_ros/rtabmapviz)
    stereo_odometry (rtabmap_ros/stereo_odometry)
  /
    imu_filter_node (imu_filter_madgwick/imu_filter_node)

auto-starting new master
process[master]: started with pid [19683]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 368f3800-3fa8-11eb-bfeb-2016b9896a4d
process[rosout-1]: started with pid [19699]
started core service [/rosout]
process[camera/realsense2_camera_manager-2]: started with pid [19716]
[ INFO] [1608127728.555721775]: Initializing nodelet with 6 worker threads.
process[camera/realsense2_camera-3]: started with pid [19717]
[ INFO] [1608127728.923209415]: RealSense ROS v2.2.20
[ INFO] [1608127728.923233592]: Built with LibRealSense v2.40.0
[ INFO] [1608127728.923243330]: Running with LibRealSense v2.40.0
[ INFO] [1608127728.934877688]:  
[ INFO] [1608127729.045865157]: Device with serial number 040322070519 was found.

[ INFO] [1608127729.045890048]: Device with physical ID 2-1-8 was found.
[ INFO] [1608127729.045897415]: Device with name Intel RealSense D435I was found.
[ INFO] [1608127729.046261027]: Device with port number 2-1 was found.
[ INFO] [1608127729.046277217]: Device USB type: 3.2
[ INFO] [1608127729.047755570]: getParameters...
[ INFO] [1608127729.101817494]: setupDevice...
process[imu_filter_node-4]: started with pid [19739]
process[rtabmap/stereo_odometry-5]: started with pid [19998]
 16/12 22:08:49,752 WARNING [140340849788672] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: Resource temporarily unavailable, number: b
[ INFO] [1608127729.868391432]: JSON file is loaded! (/home/td/realsense/realsense_ws/src/realsense-ros-2.2.20/realsense2_camera/conifg/kalibr.json)
[ INFO] [1608127729.868451263]: ROS Node Namespace: camera
[ INFO] [1608127729.868474298]: Device Name: Intel RealSense D435I
[ INFO] [1608127729.868493570]: Device Serial No: 040322070519
[ INFO] [1608127729.868512455]: Device physical port: 2-1-8
[ INFO] [1608127729.868532557]: Device FW version: 05.12.09.00
[ INFO] [1608127729.868552290]: Device Product ID: 0x0B3A
[ INFO] [1608127729.868570020]: Enable PointCloud: Off
[ INFO] [1608127729.868588365]: Align Depth: On
[ INFO] [1608127729.868599804]: Sync Mode: On
[ INFO] [1608127729.868629094]: Device Sensors: 
[ INFO] [1608127729.883112899]: Stereo Module was found.
[ INFO] [1608127729.890862880]: RGB Camera was found.
[ INFO] [1608127729.891049118]: Motion Module was found.
[ INFO] [1608127729.891076461]: num_filters: 0
[ INFO] [1608127729.891086791]: Setting Dynamic reconfig parameters.
 16/12 22:08:49,922 WARNING [140340849788672] (messenger-libusb.cpp:42) control_transfer returned error, index: 300, error: No data available, number: 3d
process[rtabmap/rtabmap-6]: started with pid [20071]
[ INFO] [1608127729.993450636]: Initializing nodelet with 6 worker threads.
[ INFO] [1608127730.064012752]: Odometry: frame_id               = camera_link
[ INFO] [1608127730.064040520]: Odometry: odom_frame_id          = odom
[ INFO] [1608127730.064051858]: Odometry: publish_tf             = true
[ INFO] [1608127730.064062042]: Odometry: wait_for_transform     = true
[ INFO] [1608127730.064076749]: Odometry: wait_for_transform_duration  = 0.100000
[ INFO] [1608127730.064101946]: Odometry: initial_pose           = xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000
[ INFO] [1608127730.064112931]: Odometry: ground_truth_frame_id  = 
[ INFO] [1608127730.064121910]: Odometry: ground_truth_base_frame_id = camera_link
[ INFO] [1608127730.064130652]: Odometry: config_path            = 
[ INFO] [1608127730.064138896]: Odometry: publish_null_when_lost = true
[ INFO] [1608127730.064147416]: Odometry: guess_frame_id         = 
[ INFO] [1608127730.064156585]: Odometry: guess_min_translation  = 0.000000
[ INFO] [1608127730.064165867]: Odometry: guess_min_rotation     = 0.000000
[ INFO] [1608127730.064175057]: Odometry: guess_min_time         = 0.000000
[ INFO] [1608127730.064184198]: Odometry: expected_update_rate   = 0.000000 Hz
[ INFO] [1608127730.064193391]: Odometry: max_update_rate        = 0.000000 Hz
[ INFO] [1608127730.064202470]: Odometry: wait_imu_to_init       = true
[ INFO] [1608127730.064223160]: Odometry: stereoParams_=1 visParams_=1 icpParams_=0
[ INFO] [1608127730.296584738]: Starting node...
process[rtabmap/rtabmapviz-7]: started with pid [20115]
[ INFO] [1608127730.381199662]: Done Setting Dynamic reconfig parameters.
[ INFO] [1608127730.381621524]: depth stream is enabled - width: 640, height: 480, fps: 30, Format: Z16
[ INFO] [1608127730.381768812]: infra1 stream is enabled - width: 640, height: 480, fps: 30, Format: Y8
[ INFO] [1608127730.381907462]: infra2 stream is enabled - width: 640, height: 480, fps: 30, Format: Y8
[ INFO] [1608127730.382334232]: color stream is enabled - width: 640, height: 480, fps: 30, Format: RGB8
[ INFO] [1608127730.383562542]: Initializing nodelet with 6 worker threads.
[ INFO] [1608127730.384139468]: setupPublishers...
[ INFO] [1608127730.389034567]: Expected frequency for depth = 30.00000
[ INFO] [1608127730.431958160]: Expected frequency for infra1 = 30.00000
[ INFO] [1608127730.470312512]: Expected frequency for aligned_depth_to_infra1 = 30.00000
[ INFO] [1608127730.504345550]: Update odometry parameter "Odom/Strategy"="9" from arguments
[ INFO] [1608127730.504373118]: Update odometry parameter "OdomVINS/ConfigPath"="/home/td/slam/rtabmap_ws/src/VINS-Fusion/vins_estimator/../config/realsense_d435i/realsense_stereo_imu_config_me.yaml" from arguments
[ INFO] [1608127730.504387143]: Update odometry parameter "Optimizer/GravitySigma"="0.3" from arguments
[ INFO] [1608127730.510748231]: /rtabmap/rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1608127730.510795924]: /rtabmap/rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1608127730.510816783]: /rtabmap/rtabmap(maps): map_cleanup                = true
[ INFO] [1608127730.510835899]: /rtabmap/rtabmap(maps): map_always_update          = false
[ INFO] [1608127730.510855140]: /rtabmap/rtabmap(maps): map_empty_ray_tracing      = true
[ INFO] [1608127730.510873027]: /rtabmap/rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1608127730.510889797]: /rtabmap/rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1608127730.510906786]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1608127730.534283257]: /rtabmap/rtabmap(maps): octomap_tree_depth         = 16
[ INFO] [1608127730.543323570]: Expected frequency for infra2 = 30.00000
[ INFO] [1608127730.591656062]: Expected frequency for color = 30.00000
[ INFO] [1608127730.601775963]: rtabmap: frame_id      = camera_link
[ INFO] [1608127730.601802550]: rtabmap: map_frame_id  = map
[ INFO] [1608127730.601814216]: rtabmap: use_action_for_goal  = false
[ INFO] [1608127730.601824951]: rtabmap: tf_delay      = 0.050000
[ INFO] [1608127730.601836666]: rtabmap: tf_tolerance  = 0.100000
[ INFO] [1608127730.601847550]: rtabmap: odom_sensor_sync   = false
[ INFO] [1608127730.642053250]: Expected frequency for aligned_depth_to_color = 30.00000
USE_IMU: 1
IMU_TOPIC: /camera/imu
result path /home/td/slam/vins_fusion_ws/src/VINS-Fusion/output/vio.csv
[ WARN] [1608127730.669305779]:  Optimize extrinsic param around initial guess!
camera number 2
[ INFO] [1608127730.669423733]: Unsynchronized sensors, online estimate time offset, initial td: 0
[ INFO] [1608127730.669437619]: ROW: 480 COL: 640 
[ INFO] [1608127730.687784348]: odometry: Subscribing to IMU topic /rtabmap/imu
[ INFO] [1608127730.692689953]: StereoOdometry: approx_sync = false
[ INFO] [1608127730.692714741]: StereoOdometry: queue_size = 5
[ INFO] [1608127730.692729559]: StereoOdometry: subscribe_rgbd = false
[ INFO] [1608127730.692739656]: StereoOdometry: keep_color = false
[ INFO] [1608127730.697749070]: Start publisher IMU
[ INFO] [1608127730.702883051]: setupStreams...
[ INFO] [1608127730.724685166]: insert Depth to Stereo Module
[ INFO] [1608127730.724730635]: 
/rtabmap/stereo_odometry subscribed to (exact sync):
   /camera/infra1/image_rect_raw \
   /camera/infra2/image_rect_raw \
   /camera/infra1/camera_info \
[ INFO] [1608127730.724741269]: insert Color to RGB Camera
   /camera/infra2/camera_info
[ INFO] [1608127730.724762433]: insert Infrared to Stereo Module
[ INFO] [1608127730.724780561]: insert Infrared to Stereo Module
[ INFO] [1608127730.724795938]: insert Gyro to Motion Module
[ INFO] [1608127730.724811301]: insert Accel to Motion Module
[ INFO] [1608127730.747917004]: Starting node...
 16/12 22:08:50,782 WARNING [140340849788672] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61
[ INFO] [1608127730.832630283]: SELECTED BASE:Depth, 0
[ INFO] [1608127730.850485688]: RealSense Node Is Up!
 16/12 22:08:50,882 WARNING [140340849788672] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61
[ INFO] [1608127730.908857332]: rtabmapviz: Using configuration from "/home/td/.ros/rtabmap_gui.ini"
[ERROR] [1608127730.914892966]: The stereo baseline (-0.049985) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo setup where the Tx (or P(0,3)) is negative in the right camera info msg.
[ INFO] [1608127730.921717093]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="true"
[ INFO] [1608127730.922213178]: Setting RTAB-Map parameter "Mem/InitWMWithAllNodes"="false"
 16/12 22:08:50,932 WARNING [140340849788672] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61
[ERROR] [1608127730.946915072]: The stereo baseline (-0.049985) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo setup where the Tx (or P(0,3)) is negative in the right camera info msg.
[ERROR] [1608127730.980049023]: The stereo baseline (-0.049985) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo setup where the Tx (or P(0,3)) is negative in the right camera info msg.
[ INFO] [1608127731.007483194]: Setting RTAB-Map parameter "RGBD/OptimizeMaxError"="0"
[ERROR] [1608127731.013762099]: The stereo baseline (-0.049985) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo setup where the Tx (or P(0,3)) is negative in the right camera info msg.
[ERROR] [1608127731.047054873]: The stereo baseline (-0.049985) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo setup where the Tx (or P(0,3)) is negative in the right camera info msg.
[ERROR] [1608127731.080353738]: The stereo baseline (-0.049985) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo setup where the Tx (or P(0,3)) is negative in the right camera info msg.
 16/12 22:08:51,083 WARNING [140340849788672] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61
[ERROR] [1608127731.113618788]: The stereo baseline (-0.049985) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo setup where the Tx (or P(0,3)) is negative in the right camera info msg.
[ INFO] [1608127731.189205273]: Update RTAB-Map parameter "Optimizer/GravitySigma"="0.3" from arguments
[ERROR] [1608127731.196835159]: The stereo baseline (-0.049985) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo setup where the Tx (or P(0,3)) is negative in the right camera info msg.
[ERROR] [1608127731.305269935]: hwmon command 0x75( 0 1df 0 27f ) failed (response -6= Invalid parameter)
[ERROR] [1608127731.329564498]: The stereo baseline (-0.049985) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo setup where the Tx (or P(0,3)) is negative in the right camera info msg.
[ WARN] [1608127731.334585573]: Rtabmap: Parameter name changed: "RGBD/OptimizeRobust" -> "Optimizer/Robust". Please update your launch file accordingly. Value "true" is still set to the new parameter name.
[ WARN] [1608127731.337611182]: Rtabmap: Parameter name changed: "RGBD/OptimizeStrategy" -> "Optimizer/Strategy". Please update your launch file accordingly. Value "1" is still set to the new parameter name.
[ERROR] [1608127731.362870489]: The stereo baseline (-0.049985) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo setup where the Tx (or P(0,3)) is negative in the right camera info msg.
[ INFO] [1608127731.381390969]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1608127731.381527394]: rtabmap: Deleted database "/home/td/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[ INFO] [1608127731.381553667]: rtabmap: Using database from "/home/td/.ros/rtabmap.db" (0 MB).
[ERROR] [1608127731.396163788]: The stereo baseline (-0.049985) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo setup where the Tx (or P(0,3)) is negative in the right camera info msg.
[ERROR] [1608127731.563181463]: The stereo baseline (-0.049985) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo setup where the Tx (or P(0,3)) is negative in the right camera info msg.
matlabbe commented 3 years ago

This error is independent of TF, it is coming from the camera _info topics. The camera projection matrix for the right camera_info should have P(0,3) negative. Can you show the camera_info of the left and right cameras?

TouchDeeper commented 3 years ago

@matlabbe Thank you for your reply.

The camera_info of left_infra is:

header: 
  seq: 867
  stamp: 
    secs: 1608946479
    nsecs: 473160331
  frame_id: "camera_infra1_optical_frame"
height: 480
width: 640
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [383.15728759765625, 0.0, 319.8735046386719, 0.0, 383.15728759765625, 241.6318359375, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [383.15728759765625, 0.0, 319.8735046386719, 0.0, 0.0, 383.15728759765625, 241.6318359375, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

the camera_info of right_infra is:

header: 
  seq: 1473
  stamp: 
    secs: 1608946499
    nsecs: 644384161
  frame_id: "camera_infra2_optical_frame"
height: 480
width: 640
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [383.15728759765625, 0.0, 319.8735046386719, 0.0, 383.15728759765625, 241.6318359375, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [383.15728759765625, 0.0, 319.8735046386719, 19.151988983154297, 0.0, 383.15728759765625, 241.6318359375, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
matlabbe commented 3 years ago

19.151988983154297 should be -19.151988983154297

TouchDeeper commented 3 years ago

@matlabbe Thank you for your reply. But it is not convenient to modify the camera_info, this should be caused by realsense's firmware, and other people will also encounter this problem. What's more, I think the positive 19.151988983154297 makes sense in the frame convention of my realsense d435i. Could you please provide some advice to modify rtabmap to adapt to this situation?

TouchDeeper commented 3 years ago

@matlabbe please help~, someone encountered the same problem as me, here is the issue.