introlab
ORB_SLAM3 camera configuration file
Hello,
I'm currently trying to integrate ORB_SLAM3 odometry into RtabMap. As of now I have successfully built and trying to run rtabmap_odom rgbd_odometry with ORB_SLAM strategy. But I saw that the odometry node auto generated a camera configuration file, which seems to be incompatible with my camera and the odometry node crashed. I tried to run the generated config file with my current ORB_SLAM3 code and it seems wrong too. So I want to use my camera config file that I have been using with ORB_SLAM3. It seems that the config file path can't be changed as a parameter in the launch file and when I change the content of the generated config file, it changes back when I run the odometry node. How can I use my camera configuration file?
Here's my current rtabmap launch file:
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
def generate_launch_description():
parameters={
'frame_id':'camera_link',
'subscribe_rgbd':True,
'subscribe_odom':True,
'approx_sync':True,
'approx_sync_max_interval': 0.01,}
remappings=[
('rgb/image', '/camera/color/image_raw'),
('rgb/camera_info', '/camera/color/camera_info'),
('depth/image', '/camera/depth/image_raw'),
('odom', '/odom')]
return LaunchDescription([
# # Optical rotation
Node(
package='tf2_ros', executable='static_transform_publisher', output='screen',
arguments=["0", "0", "0", "1.57", "0", "0", "odom", "camera_link"]),
Node(
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
parameters=[parameters,
{'Odom/Strategy':'5',
'OdomORBSLAM/VocPath' : '/home/trongdatlinux/ORB_SLAM3/Vocabulary/ORBvoc.txt',
'OdomORBSLAM/configPath' : '/home/trongdatlinux/orb_slam3_ws/src/orbslam3_ros2/config/rgbd/camera_rgbd.yaml' ,
'OdomORBSLAM/Bf' : '40.0',
'OdomORBSLAM/Fps' : "20.0",
'OdomORBSLAM/MaxFeatures' : "2000",
'OdomORBSLAM/ThDepth' : "40.0" }],
remappings=remappings,
namespace="rtabmap"),
Node(
package='rtabmap_slam', executable='rtabmap', output='screen',
parameters=[parameters],
remappings=remappings,
arguments=['-d'],
namespace="rtabmap"),
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
parameters=[parameters],
remappings=remappings,
namespace="rtabmap"),
])
Thank you in advance.
What kind of camera do you have? If the images are published in camera_link frame directly, you should apply the optical rotation like this:
# Optical rotation
Node(
package='tf2_ros', executable='static_transform_publisher', output='screen',
arguments=["0", "0", "0", "-1.5707", "0", "-1.5707", "base_link", "camera_link"]),
then use base_link as frame_id for rtabmap nodes.
For the config, can you show a comparison of the generated one and one that is working? Maybe the format changed over time and we would need to update it. This is where we generate the config: https://github.com/introlab/rtabmap/blob/802b8f9870e74cfa0197921db4a514473f585dec/corelib/src/odometry/OdometryORBSLAM3.cpp#L106-L341
Note that we do this so that we use camera calibration from the camera_info message and also use TF to set the appropriate transform between base frame and the camera optical frame.
Thanks for your reply. I use the Orbbec Astra Pro camera. Here is the RTAB-MAP generated config file:
%YAML:1.0
File.version: "1.0"
Camera.type: "PinHole"
Camera1.fx: 570.3422047415297
Camera1.fy: 570.3422047415297
Camera1.cx: 319.5000000000000
Camera1.cy: 239.5000000000000
Camera1.k1: 0.0000000000000
Camera1.k2: 0.0000000000000
Camera1.p1: 0.0000000000000
Camera1.p2: 0.0000000000000
Camera1.k3: 0.0000000000000
Camera.bf: 22813.6881896611885
Camera.width: 640
Camera.height: 480
Camera.RGB: 0
Camera.fps: 20
Stereo.ThDepth: 40.0000000000000
Stereo.b: 40.0000000000000
RGBD.DepthMapFactor: 1.0000000000000
ORBextractor.nFeatures: 2000
ORBextractor.scaleFactor: 2.0000000000000
ORBextractor.nLevels: 3
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7
loopClosing: 0
Viewer.KeyFrameSize: 0.0500000000000
Viewer.KeyFrameLineWidth: 1.0000000000000
Viewer.GraphLineWidth: 0.9000000000000
Viewer.PointSize: 2.0000000000000
Viewer.CameraSize: 0.0800000000000
Viewer.CameraLineWidth: 3.0000000000000
Viewer.ViewpointX: 0.0000000000000
Viewer.ViewpointY: -0.7000000000000
Viewer.ViewpointZ: -3.5000000000000
Viewer.ViewpointF: 500.0000000000000
And here is the config file that I've been using with ORB_SLAM3:
%YAML:1.0
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
Camera.type: "PinHole"
# Right Camera calibration and distortion parameters (OpenCV)
Camera.fx: 570.3422047415297
Camera.fy: 570.3422047415297
Camera.cx: 319.5
Camera.cy: 239.5
# distortion parameters
Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
# Camera resolution
Camera.width: 640
Camera.height: 480
# Camera frames per second
Camera.fps: 30.0
# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1
# Image scale, it changes the image size to be processed (<1.0: reduce, >1.0: increase)
# Camera.imageScale: 0.7071 # 0.5 #0.7071 # 1/sqrt(2)
# Close/Far threshold. Baseline times.
ThDepth: 40.0
# stereo baseline times fx
Camera.bf: 40.0
# Deptmap values factor
DepthMapFactor: 1000.0
#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------
# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 2000
# ORB Extractor: Scale factor between levels in the scale pyramid
ORBextractor.scaleFactor: 1.2
# ORB Extractor: Number of levels in the scale pyramid
ORBextractor.nLevels: 8
# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7
#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.1
Viewer.KeyFrameLineWidth: 1.0
Viewer.GraphLineWidth: 1.0
Viewer.PointSize: 2.0
Viewer.CameraSize: 0.15
Viewer.CameraLineWidth: 2.0
Viewer.ViewpointX: 0.0
Viewer.ViewpointY: -10.0
Viewer.ViewpointZ: -0.1
Viewer.ViewpointF: 2000.0
Update: I seems to have misunderstood some parameters in the node launch and changed so the generated config file becomes this:
%YAML:1.0
File.version: "1.0"
Camera.type: "PinHole"
Camera1.fx: 570.3422047415297
Camera1.fy: 570.3422047415297
Camera1.cx: 319.5000000000000
Camera1.cy: 239.5000000000000
Camera1.k1: 0.0000000000000
Camera1.k2: 0.0000000000000
Camera1.p1: 0.0000000000000
Camera1.p2: 0.0000000000000
Camera1.k3: 0.0000000000000
Camera.bf: 42.7756653556147
Camera.width: 640
Camera.height: 480
Camera.RGB: 0
Camera.fps: 20
Stereo.ThDepth: 40.0000000000000
Stereo.b: 0.0750000000000
RGBD.DepthMapFactor: 1000.0000000000000
ORBextractor.nFeatures: 2000
ORBextractor.scaleFactor: 2.0000000000000
ORBextractor.nLevels: 3
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7
loopClosing: 0
Viewer.KeyFrameSize: 0.0500000000000
Viewer.KeyFrameLineWidth: 1.0000000000000
Viewer.GraphLineWidth: 0.9000000000000
Viewer.PointSize: 2.0000000000000
Viewer.CameraSize: 0.0800000000000
Viewer.CameraLineWidth: 3.0000000000000
Viewer.ViewpointX: 0.0000000000000
Viewer.ViewpointY: -0.7000000000000
Viewer.ViewpointZ: -3.5000000000000
Viewer.ViewpointF: 500.0000000000000
This config works fine when I run ORB_SLAM3 alone, but when I tried to run ORB_SLAM3 with RTAB-MAP, I got segmentation fault: [ERROR] [rgbd_odometry-1]: process has died [pid 7627, exit code -11, cmd '/home/asus/rtab_ws/install/rtabmap_odom/lib/rtabmap_odom/rgbd_odometry --ros-args -r __ns:=/rtabmap --params-file /tmp/launch_params_1qcm0_6b --params-file /tmp/launch_params_cpar58sq -r rgb/image:=/camera/color/image_raw -r rgb/camera_info:=/camera/color/camera_info -r depth/image:=/camera/depth/image_raw -r odom:=/odom'].
I tried debugging with GDB and got the results:
Thread 19 "rgbd_odometry" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fffc0cca640 (LWP 7707)]
0x00007ffff24ae9f3 in ORB_SLAM3::Frame::Frame() () from /home/asus/ORB_SLAM3/lib/libORB_SLAM3.so
(gdb) bt
#0 0x00007ffff24ae9f3 in ORB_SLAM3::Frame::Frame() () at /home/asus/ORB_SLAM3/lib/libORB_SLAM3.so
#1 0x00007ffff23a1854 in ORB_SLAM3::Tracking::Tracking(ORB_SLAM3::System*, DBoW2::TemplatedVocabulary<cv::Mat, DBoW2::FORB>*, ORB_SLAM3::FrameDrawer*, ORB_SLAM3::MapDrawer*, ORB_SLAM3::Atlas*, ORB_SLAM3::KeyFrameDatabase*, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, ORB_SLAM3::Settings*, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) () at /home/asus/ORB_SLAM3/lib/libORB_SLAM3.so
#2 0x00007ffff234c9ab in ORB_SLAM3::System::System(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ORB_SLAM3::System::eSensor, bool, int, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) () at /home/asus/ORB_SLAM3/lib/libORB_SLAM3.so
#3 0x00007ffff655d215 in rtabmap::OdometryORBSLAM3::init(rtabmap::CameraModel const&, rtabmap::CameraModel const&, double, bool, double) () at /usr/local/lib/librtabmap_core.so.0.21
#4 0x00007ffff655f1ee in rtabmap::OdometryORBSLAM3::computeTransform(rtabmap::SensorData&, rtabmap::Transform const&, rtabmap::OdometryInfo*) () at /usr/local/lib/librtabmap_core.so.0.21
#5 0x00007ffff65148a3 in rtabmap::Odometry::process(rtabmap::SensorData&, rtabmap::Transform const&, rtabmap::OdometryInfo*) () at /usr/local/lib/librtabmap_core.so.0.21
#6 0x00007ffff52cd501 in rtabmap_odom::OdometryROS::mainLoop() () at /home/asus/rtab_ws/install/rtabmap_odom/lib/librtabmap_odom.so
#7 0x00007ffff7d6ec11 in UThread::ThreadMain() () at /usr/local/lib/librtabmap_utilite.so.0.21
#8 0x00007ffff7d6ef59 in UThreadC<void>::ThreadMainHandler(UThreadC<void>::Instance*) () at /usr/local/lib/librtabmap_utilite.so.0.21
#9 0x00007ffff5694ac3 in start_thread (arg=<optimized out>) at ./nptl/pthread_create.c:442
#10 0x00007ffff5726850 in clone3 () at ../sysdeps/unix/sysv/linux/x86_64/clone3.S:81
Basically, aligning similar parameters, the diff is (red=oirginal, green=rtabmap generated):
> File.version: "1.0"
>
5,13c7,16
< Camera.fx: 570.3422047415297
< Camera.fy: 570.3422047415297
< Camera.cx: 319.5
< Camera.cy: 239.5
<
< Camera.k1: 0.0
< Camera.k2: 0.0
< Camera.p1: 0.0
< Camera.p2: 0.0
---
> Camera1.fx: 570.3422047415297
> Camera1.fy: 570.3422047415297
> Camera1.cx: 319.5000000000000
> Camera1.cy: 239.5000000000000
>
> Camera1.k1: 0.0000000000000
> Camera1.k2: 0.0000000000000
> Camera1.p1: 0.0000000000000
> Camera1.p2: 0.0000000000000
> Camera1.k3: 0.0000000000000
17c20
< Camera.fps: 30.0
---
> Camera.fps: 20
19c22
< Camera.RGB: 1
---
> Camera.RGB: 0
21c24,25
< ThDepth: 40.0
---
> Stereo.ThDepth: 40.0000000000000
> Stereo.b: 0.0750000000000
23c27
< Camera.bf: 40.0
---
> Camera.bf: 42.7756653556147
25c29
< DepthMapFactor: 1000.0
---
> RGBD.DepthMapFactor: 1000.0000000000000
28,29c32,33
< ORBextractor.scaleFactor: 1.2
< ORBextractor.nLevels: 8
---
> ORBextractor.scaleFactor: 2.0000000000000
> ORBextractor.nLevels: 3
31a36,37
>
> loopClosing: 0
It looks like your original ORB_SLAM3 parameters are matching the old format from here https://github.com/UZ-SLAMLab/ORB_SLAM3/tree/master/Examples_old. The one from rtabmap is based on "latest" examples here: https://github.com/UZ-SLAMLab/ORB_SLAM3/tree/master/Examples
For the gdb log, it could be related to compile arguments used on ORB_SLAM3 side. Make sure to remove all -march=native in ORB_SLAM3 code. See this patch for example: https://gist.github.com/matlabbe/f5cb281304a1305b2824a6ce19792e13
Thanks, I managed to run the ORB-SLAM3 integrated RTAB-MAP successfully. Also is there a way to print out the generated 3D map or publish it to Rviz?
If you use rviz's rtabmap_rviz_plugins:MapCloud plugin, you could visualize the 3D point cloud by adding corresponding topic (/rtabmap/mapData).
Thanks, I managed to run the ORB-SLAM3 integrated RTAB-MAP successfully. Also is there a way to print out the generated 3D map or publish it to Rviz? @trongdathust Can you share how you use orbslam3 on rtabmap?
@trongdathust Can you share how you use orbslam3 on rtabmap
There is an option to change the Odom/Strategy in rtabmap to use orbslam3 for visual odometry calculation. You can refer to issue #1104 for guide on setting up the environment to use orbslam3 with rtabmap