Modification of repository so that this ROS2 wrapper can be used in navigation.

[jnskkmhr]

Hi, people.
I modified the original codebase so that I could use it inside navigation.
https://github.com/jnskkmhr/orbslam3/tree/main

Here is what I did:

• Modified the node to provide topics (tf2, tracked image, pose topics)
• The stereo-inertial node did not work because of QOS setting in IMU subscriber, so I fixed this.
• SLAM pose was an optical frame pose expressed in OpenCV frame, but I needed camera frame pose in ROS FLU coordinate.
• The node provide map to odom (localization) transform.
• Docker for easy development.

Currently, unfortunately, the stereo-inertial node does not work properly.
I see IMU initialization takes a lot of time, and the pose sometimes jumps.
This might be because of the wrong calibration configuration.
Please send PR if you know how to solve issues.

[NilsRublein]

I tried to install your version, but I am stuck at a dependency problem: It seems that CmakeLists.txt cannot find the epoxy library.

colcon build --cmake-args -DCMAKE_CXX_FLAGS="-w" --symlink-install --packages-select orbslam3
Starting >>> orbslam3
--- stderr: orbslam3                         
CMake Error at CMakeLists.txt:33 (find_package):
  By not providing "Findepoxy.cmake" in CMAKE_MODULE_PATH this project has
  asked CMake to find a package configuration file provided by "epoxy", but
  CMake did not find one.

  Could not find a package configuration file provided by "epoxy" with any of
  the following names:

    epoxyConfig.cmake
    epoxy-config.cmake

  Add the installation prefix of "epoxy" to CMAKE_PREFIX_PATH or set
  "epoxy_DIR" to a directory containing one of the above files.  If "epoxy"
  provides a separate development package or SDK, be sure it has been
  installed.


gmake: *** [Makefile:528: cmake_check_build_system] Error 1
---
Failed   <<< orbslam3 [0.69s, exited with code 2]
                                
Summary: 0 packages finished [1.09s]
  1 package failed: orbslam3
  1 package had stderr output: orbslam3

[jnskkmhr]

@NilsRublein
Did you use docker to build the package ?
Could you also check if this path is same in your case? (Path to ORB-SLAM3/CMakeModules)
If you are using docker, there might be other issues.

[hermanndererdmann]

not sure where i'm supposed to run colcon build, but neither on the machine nor in the container it works.

[jnskkmhr]

@hermanndererdmann
Hey, apologies for the confusion.
I modified README so that it is clear where to run the colcon build.

By the way, I recommend that you use docker as it is difficult for me to figure out some dependency issues that some people may face.
I did a clean install of orbslam3 for sanity check again, and everything works just fine.

[hermanndererdmann]

This is what i get. (no realsense connected, just trying to launch the package)

ubuntu@hp:~/Downloads/test/ros/src/orbslam3$ ./docker/build_image.sh
docker build /home/ubuntu/Downloads/test/ros/src/orbslam3/docker --tag orbslam3-ros2 -f /home/ubuntu/Downloads/test/ros/src/orbslam3/docker/dockerfile-foxy
[+] Building 1.8s (23/23) FINISHED docker:default
=> [internal] load build definition from dockerfile-foxy 0.0s
=> => transferring dockerfile: 2.32kB 0.0s
=> [internal] load metadata for docker.io/osrf/ros:foxy-desktop 1.4s
=> [internal] load .dockerignore 0.0s
=> => transferring context: 2B 0.0s
=> [ 1/19] FROM docker.io/osrf/ros:foxy-desktop@sha256:ce6126b2edd2231ed 0.0s
=> CACHED [ 2/19] RUN apt-get update && apt-get install -y --no-inst 0.0s
=> CACHED [ 3/19] WORKDIR /home/thirdparty 0.0s
=> CACHED [ 4/19] RUN git clone https://github.com/stevenlovegrove/Pango 0.0s
=> CACHED [ 5/19] RUN git clone https://github.com/jnskkmhr/ORB-SLAM3-ST 0.0s
=> CACHED [ 6/19] RUN cd ORB_SLAM3 && git pull origin && chmod +x build. 0.0s
=> CACHED [ 7/19] RUN cd ORB_SLAM3/Thirdparty/Sophus/build && make insta 0.0s
=> CACHED [ 8/19] RUN mkdir -p /etc/apt/keyrings && curl -sSf https://li 0.0s
=> CACHED [ 9/19] RUN apt-get update && apt-get install -y apt-transport 0.0s
=> CACHED [10/19] RUN echo "deb [signed-by=/etc/apt/keyrings/librealsens 0.0s
=> CACHED [11/19] RUN apt-get update && apt-get install -y librealsense2 0.0s
=> CACHED [12/19] WORKDIR /home/thirdparty/ros2_ws/src 0.0s
=> CACHED [13/19] RUN git clone https://github.com/IntelRealSense/realse 0.0s
=> CACHED [14/19] WORKDIR /home/thirdparty/ros2_ws 0.0s
=> CACHED [15/19] RUN rosdep install -i --from-path src --rosdistro foxy 0.0s
=> CACHED [16/19] RUN /bin/bash -c "source /opt/ros/foxy/setup.sh && col 0.0s
=> CACHED [17/19] WORKDIR /home/ros2_ws 0.0s
=> CACHED [18/19] RUN echo "source /opt/ros/foxy/setup.bash" >> /root/.b 0.0s
=> CACHED [19/19] RUN echo "source /home/thirdparty/ros2_ws/install/setu 0.0s
=> exporting to image 0.0s
=> => exporting layers 0.0s
=> => writing image sha256:8b7cfc5238fd6f98958b8ac986b628dea4df460e4f8df 0.0s
=> => naming to docker.io/library/orbslam3-ros2 0.0s

1 warning found (use --debug to expand):

• LegacyKeyValueFormat: "ENV key=value" should be used instead of legacy "ENV key value" format (line 2)
  ubuntu@hp:/Downloads/test/ros/src/orbslam3$ ./docker/run_container.sh
  non-network local connections being added to access control list
  docker: Error response from daemon: Conflict. The container name "/orbslam3-ros2-container" is already in use by container "81fae4d3b3135d56929da1b9794d5d98fcae8c52f3c9d1a51a0c5a5092858ee3". You have to remove (or rename) that container to be able to reuse that name.
  See 'docker run --help'.
  ubuntu@hp:/Downloads/test/ros/src/orbslam3$ ./docker/run_container.sh
  non-network local connections being added to access control list
  root@hp:/home/ros2_ws# colcon build --cmake-args -DCMAKE_CXX_FLAGS="-w" --symlink-install --packages-select orbslam3
  [0.335s] WARNING:colcon.colcon_core.package_selection:ignoring unknown package 'orbslam3' in --packages-select

Summary: 0 packages finished [0.12s]
root@hp:/home/ros2_ws# ls
build install log
root@hp:/home/ros2_ws# source install/setup.bash
root@hp:/home/ros2_ws# ros2 launch realsense2_camera rs_launch.py enable_infra1:=true enable_infra2:=true enable_accel:=true enable_gyro:=true unite_imu_method:=2 infra_width:=640 infra_height:=480 camera_name:=d455 camera_namespace:=d455
[INFO] [launch]: All log files can be found below /root/.ros/log/2024-07-18-08-56-26-586723-hp-62
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [realsense2_camera_node-1]: process started with pid [64]
[realsense2_camera_node-1] [INFO] [1721292986.869427926] [d455.d455]: RealSense ROS v4.55.1
[realsense2_camera_node-1] [INFO] [1721292986.869600847] [d455.d455]: Built with LibRealSense v2.55.1
[realsense2_camera_node-1] [INFO] [1721292986.869615324] [d455.d455]: Running with LibRealSense v2.55.1
[realsense2_camera_node-1] [WARN] [1721292986.876328754] [d455.d455]: No RealSense devices were found!
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[realsense2_camera_node-1] [INFO] [1721292989.137370221] [rclcpp]: signal_handler(signal_value=2)
[INFO] [realsense2_camera_node-1]: process has finished cleanly [pid 64]
root@hp:/home/ros2_ws# ros2 launch orbslam3 -
--debug --print-description --show-arguments -n
--noninteractive --show-all-subprocesses-output -a -p
--print --show-args -d -s
root@hp:/home/ros2_ws# ros2 launch orbslam3 stereo_inertial_d455.launch.yaml
Package 'orbslam3' not found: "package 'orbslam3' not found, searching: ['/home/thirdparty/ros2_ws/install/realsense2_description', '/home/thirdparty/ros2_ws/install/realsense2_camera', '/home/thirdparty/ros2_ws/install/realsense2_camera_msgs', '/opt/ros/foxy']"
root@hp:/home/ros2_ws#

[jnskkmhr]

@hermanndererdmann
Please clone this orbslam3 repository under ~/ros2_ws/src (It's written in README).
It seems you cloned the repository in /Downloads/test/ros/src/orbslam3.
This is important since the docker/run_container.sh shell file mounts your host's ~/ros2_ws directory to /home/ros2_ws directory in container.

[NilsRublein]

@NilsRublein Did you use docker to build the package ? Could you also check if this path is same in your case? (Path to ORB-SLAM3/CMakeModules) If you are using docker, there might be other issues.

@jnskkmhr I just tried to build it myself without the docker. In my case the path was not correct to the ORBSLAM3 repo, As you forked from this repo, I expected you also used this version of ORBSLAM3. After fixing the path and some dependencies (sophus & OpenCV) I managed to build and run it!

[jnskkmhr]

@NilsRublein
That's good to hear.
On my side, I am using modified ORB-SLAM3-Stereo-Fixed repo as I failed to build it.
https://github.com/jnskkmhr/ORB-SLAM3-STEREO-FIXED
I remember it was something related to find_package(epoxy) and find_package(OpenGL).

I did not explain this in README as OS3 is automatically built inside docker file.
https://github.com/jnskkmhr/orbslam3/blob/e9e05bc96cf8572e117b73fda14054e7d3bb2bda/docker/dockerfile-foxy#L33C15-L33C69

Btw, can you also let me know if you face a pose scale issue?
I think it is related to calibration, and I found out this when I compared OS3 with T265 VIO.

[NamTruongTran]

@jnskkmhr Thank you for your great contributions.

Can I also use your repository (https://github.com/jnskkmhr/orbslam3/tree/main) with Humble?

[jnskkmhr]

@NamTruongTran
Hi, I have not tested the repository with Humble yet.
You need to change the version of OpenCV here (I think apt in Ubuntu22.04 installs 4.5.4) or source build OpenCV4.2 on Ubuntu22.04 with humble.

[NamTruongTran]

@jnskkmhr

Thank you so much for you reply i really appreciate your time.
I want to ./build.sh this:
https://github.com/zang09/ORB-SLAM3-STEREO-FIXED

I got the following error message:

I setup:
-Ubuntu 20.04 ROS2 foxy
-libopenCV version 4.2.0 (build from source)

"I have been stuck for almost 2 days now."
Do you have any solution for that?

Thank you !

[jnskkmhr]

@NamTruongTran
Please use this: https://github.com/jnskkmhr/ORB-SLAM3-STEREO-FIXED instead of https://github.com/zang09/ORB-SLAM3-STEREO-FIXED
I modified couple lines in source code and CMakeLists.txt as there were dependency errors.

Also please refer to this: jnskkmhr#1
I do not want to deal with host install issues.
That is why I highly recommend that you use docker.
In your case, you need to base your docker image on jetson ubuntu 20.04.

[adamanov]

@jnskkmhr

first of all thanks for your contribution,

I just tested on Ubuntu 22.04 with ros2 humble, the package is built (here is my CMakeLists.txt, and it is needed to set correct path of ORBSLAM3 lib folder location from CMakeModules/FindORB_SLAM3.cmake to set(ORB_SLAM3_ROOT_DIR "~/thirdparty/ORB_SLAM3")). After that the nodes are started and working, however, have not conducted a deep test on if the published odometry is correct, (I will be back soon), I will push to your repo under branch humble version

[jnskkmhr]

@adamanov
Hi, thanks for the contribution.
I think the current odometry has some scale issues.
I still have not figured out this problem yet, so I highly welcome your PR in the future.

[irfanalimd]

@jnskkmhr Does the stereo-inertial mode work in your modified repo now ? (I am looking to run it on Ubuntu 22.04 and ROS 2 humble). I have tried running it using @zang09's version but I haven't been able to.

[jnskkmhr]

@irfanalimd
Hey, I haven't be able to test stereo-inertial SLAM since June.
The current issue is that pose jumps many times probably because of stereo-imu calibration.
Stereo SLAM runs, but the scale of pose seems not correct (relatively big gap from T265 odometry).
My repository is here: https://github.com/jnskkmhr/orbslam3
It comes with docker, so that I do not need with build issue.

[SrOdranoel]

Hello @irfanalimd and @jnskkmhr.
I have been trying for the past few weeks to run the stereo-inertial system using two Point Grey cameras from FLIR and the STIM 300 IMU. I have already used Kalibr to calibrate my system, which generates a rotation matrix that makes sense. I also used CAD to obtain the equipment positions, and the result from Kalibr makes sense. However, I am still encountering the same problem as mentioned by @jnskkmhr, as detailed below:

big time difference
big time difference
big time difference
big time difference
big time difference
big time difference
big time difference
big time difference
big time difference
big time difference
big time difference
big time difference
big time difference
[ros2run]: Segmentation fault

I believe the issue is likely related to some change that might have occurred in the repository in recent months. I will try to contact @zang09 and attempt to find a solution. If any of you have found a solution, I would appreciate it if you could get in touch with me. If you'd like to collaborate directly and share information, feel free to reach out to me via email at: leonardo.sales@labmetro.ufsc.br.

If I find a solution, I will reply here or in another issue, and if an urgent solution is needed, I can directly contact you if required.

[jnskkmhr]

@SrOdranoel
Hi!
Are you now using this repo? https://github.com/jnskkmhr/orbslam3
In my case, this big time difference came from imu subscriber qos setting.

https://github.com/jnskkmhr/orbslam3/blob/74d31fda8d80c03d9e4b1c97c7392426c3bcbc49/src/stereo-inertial/stereo-inertial-node.cpp#L61-L64

[SrOdranoel]

Hello @jnskkmhr,

No, I'm using this repo: https://github.com/zang09/ORB_SLAM3_ROS2

I had to change a few things in the stereo setup to make it work. The modifications I made are discussed in other threads of this repository. But anyway, I can't get it to work. I just tried to apply the change you mentioned in your last message by modifying the node.cpp for the inertial, but I still haven't had success. I'm still getting the message and eventually, the system freezes and closes ORB-SLAM3:

big time difference
big time difference
big time difference
big time difference
big time difference
big time difference
big time difference
big time difference
big time difference
big time difference
big time difference
[ros2run]: Segmentation fault

The strange thing is that it works, but it shows results as if the calibration were wrong, always losing and restarting the map, and not capturing many scenes.

I briefly looked at your repository, but I’m looking for a solution that doesn’t require using a Docker container, and that I could run everything on Ubuntu 22 while using ROS. Did you have fix this problem with other solution???

[jnskkmhr]

@SrOdranoel
Hi, it is certainly possible to use my repo without docker.
You just need to modify cmakemodule path here to your own path:
https://github.com/jnskkmhr/orbslam3/blob/74d31fda8d80c03d9e4b1c97c7392426c3bcbc49/CMakeLists.txt#L11
https://github.com/jnskkmhr/orbslam3/blob/74d31fda8d80c03d9e4b1c97c7392426c3bcbc49/CMakeLists.txt#L5

Also you might need to deal with different OpenCV version if you work on Ubuntu22.04.

Btw, I have not solved the lost pose problem as you mentioned, I thought this was due to calibration issue.
As far as I know, "big time difference" is corrected in my repo (this was due to imu subscriber qos setting).
The problem may persist if your imu and stereo has big time difference.

[SrOdranoel]

Hi Again @jnskkmhr.
After reading and installing your repository, I'm having some doubts and difficulties creating the launch and yaml files (not the yaml files with camera parameters, I already got them from Kalibr without any problems).

Could you help me create the files? I didn't understand the issue of the odometry used. Do you use the IMU as an odometry frame? Why not use remap in the ROS topics?

If you don't mind scheduling a conversation via meetup or discord, my time zone is GMT -3.

[jnskkmhr]

Sure, @SrOdranoel
My time zone is in EST.
Let me know what time works for you.
My email is jkamohara3@gatech.edu
You can share me your discord username there.

[SrOdranoel]

Hi @jnskkmhr,
I have sent you an email using the address leonardo.sales@labmetro.ufsc.br. It might have ended up in your spam folder, but I included my Discord and some of my questions in the message. Please check when you have a chance.

Thank you!