introlab / rtabmap_ros

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

Monocular camera and imu #1162

Open SaiPuneeth666 opened 5 months ago

SaiPuneeth666 commented 5 months ago

Hi, I would like to use monocular camera with imu data. I do not want to use stereo cameras. Is there a way to use them without using third party SLAM algorithms. And when using IMU data, what is this data used for in stereo setting. Is it used in just the filter or used in creating depth information?

Thanks in advance.

matlabbe commented 5 months ago

We don't provide a VIO solution out-of-the-box in this repo. To do IMU+Monocular, you will ned to find a package doing it and feed the pose to rtabmap. Rtabmap can subscribe to only a monocular camera with a pose, but Mem/StereoFromMotion should be enabled to close loop closures.

For stereo odometry + IMU, we do a loosely coupled VIO, using IMU mostly to help feature matching (rotation) and to add gravity constraints to graph. If visual features cannot be matched, odometry will be lost (there is no pose estimation with only IMU). Odometry output frequency is limited to image rate, not imu rate.

gjcliff commented 2 weeks ago

@matlabbe Can you elaborate more on how to set up rtabmap to be used with only a monocular camera and a pose? Do you mean a pose as in position and orientation, or pose as in a nav_msgs/msg/Odometry message with linear velocity and angular velocity?

matlabbe commented 1 week ago

By pose, I mean an odometry topic containing a pose of the corresponding frame.

Node(
      package='rtabmap_slam', executable='rtabmap', output='screen',
      parameters=[{ 'frame_id':'base_link',
                    'subscribe_rgb':True,
                    'subscribe_depth':False,
                    'Mem/StereoFromMotion':'true'}],
      remappings=[('rgb/image', '/camera/image_raw'),
                  ('rgb/camera_info', '/camera/camera_info'),
                  ('odom', '/camera/odom')],
      arguments=['--delete_db_on_start']
)
gjcliff commented 1 week ago

Thanks for your reply. I've set up my project with orb_slam3 generating the camera's odom for rtabmap along with a raw monocular image from a realsense d435i. Things seem to work well and rtabmap_viz shows the image, some detected features overlaid on top of the image, and a pose graph:

Screenshot from 2024-11-01 23-04-30

I noticed that there's no point cloud being generated in the 3D map, and none of the map-related topics (e.g. /map, /cloud_map, /octomap_grid, etc.) have messages published to them. Is that by design, or am I doing something wrong? Here's how I'm launching my rtabmap nodes, I can send more information if needed. Nothing crazy, pretty much exactly what you sent in your last message:

Node(
    package="rtabmap_slam",
    executable="rtabmap",
    output="screen",
    parameters=[
        {
            "frame_id": "base_link",
            "subscribe_rgb": True,
            "subscribe_depth": False,
            "Mem/StereoFromMotion": "true",
        }
    ],
    remappings=[
        ("rgb/image", "/orb_camera/image"),
        ("rgb/camera_info", "/orb_camera/info"),
        ("odom", "/orb_odom"),
    ],
    arguments=["--delete_db_on_start"],
),
Node(
    package="rtabmap_viz",
    executable="rtabmap_viz",
    output="screen",
    parameters=[
        {
            "frame_id": "base_link",
            "odom_frame_id": "odom",
            "subscribe_rgb": True,
            "approx_sync": False,  # False,
            "use_sim_time": LaunchConfiguration("use_sim_time"),
        },
    ],
    remappings=[
        ("rgb/image", "/orb_camera/image"),
        ("rgb/camera_info", "/orb_camera/info"),
    ],
),
matlabbe commented 1 week ago

Without depth or stereo, no dense point cloud will be generated. You could see however the 3D estimated features by enabling it under Preferences->3D Renderings, scroll down to Miscellaneous section, then check "Show 3D features." under Map column.

Note that without depth or stereo or laser scans, as you are already using ORB-SLAM3, there is no advantage to use RTAB-Map. You should already have a feature map from ORB-SLAM3 (probably more accurate than the 3D features that RTAB-Map can compute from monocular images).

Note also that using ORB-SLAM3 "as is" as input to RTAB-Map would be not compatible if loop closure detection of ORB-SLAM3 is not disabled. That's why we provide an OdometryORBSLAM3 implementation inside RTAB-Map where we explicitly ignore loop closure detector of ORB-SLAM3. The internal version is only supporting RGB-D or stereo data though.

EDIT: To correct my last point, when using ORB-SLAM3 externally, it seems we can disable loop closure detector by setting orbslam config "loopClosing" to 0, so it is possible to use external ORB-SLAM3 node as input to rtabmap.