Lab 07 - ORB-SLAM3 cz. 1
Lab. 07 - SLAM Wizyjny (ORB-SLAM3) cz. 1
1. ORB-SLAM3
ORB-SLAM3 jest trzecim z serii systemem z grupy prof. Tardosa opartym na wykrywaniu cech charakterystycznych ORB na obrazie, ich dopasowywaniu za pomocą opisu każdej cechy (deskryptory) oraz ostatecznej optymalizacji pozy z wykorzystaniem błędu reprojekcji.
Różnice pomiędzy systemami ORB-SLAM w skrócie:
- 2015, ORB-SLAM - wersja monokularowa
- 2017, ORB-SLAM2 - wersja monokularowa, stereowizyjna, RGB-D
- 2020, ORB-SLAM3 - wersja monokularowa, stereowizyjna, RGB-D. Każda z wersji bez lub z pomiarami z IMU. Łączenie wielu map w celu budowy mapy świata.
2. Cel zajęć
Celem zajęć jest uruchomienie lokalizacji 3D z wykorzystaniem systemu ORB-SLAM3 na danych z eksperymentów przeprowadzonych na terenie Politechniki Poznańskiej.
3. Przygotowanie środowiska
Wersja 1: Obraz (większy rozmiar)
UWAGA: Wersja z obrazem może nie działać na części komputerów z uwagi na brak kompatybilności budowanych bibliotek. Sugerowana jest opcja samodzielnego zbudowania obrazu z wykorzystaniem Dockerfile'a!
Przed przystąpieniem do pracy należy przygotować środowisko: pobrać obraz dockera i utworzyć na jego podstawie kontener.
Obraz konieczny do wykonywania dzisiejszych zajęć opiera się o osrf/ros:humble-desktop-full, ale zawiera paczki, które są niezbędne do prawidłowego wykonania zadań. Obraz można pobrać z tego linku.
Pobrany obraz należy wczytać, korzystając z polecenia, podając odpowiednią ścieżkę:
docker load < path/to/file.tar.gz
Następnie można przejść do tworzenia kontenera korzystając ze skryptów przygotowanych dla osób korzystających wyłącznie z CPU lub wyposażonych w GPU Nvidia. Można pobrać odpowiedni skrypt korzystając np. z wget.
Domyślnie kontener nosi nazwę ARM_07. UWAGA! Skrypty po uruchomieniu usuwają kontener o takiej nazwie przed utworzeniem nowego.
Wersja 2: Dockerfile
Plik Dockerfile jest dostępny tutaj. Można go pobrać poleceniem:
wget https://raw.githubusercontent.com/kamilmlodzikowski/LabARM/main/Lab07-ORB-SLAM/Dockerfile
Skrypt budujący jest dostępny tutaj. Można go pobrać poleceniem:
wget https://raw.githubusercontent.com/kamilmlodzikowski/LabARM/main/Lab07-ORB-SLAM/arm_07_build.sh
Następnie można przejść do tworzenia kontenera korzystając ze skryptów przygotowanych dla osób korzystających wyłącznie z CPU lub wyposażonych w GPU Nvidia. Można pobrać odpowiedni skrypt korzystając np. z wget.
Domyślnie kontener nosi nazwę ARM_07. UWAGA! Skrypty po uruchomieniu usuwają kontener o takiej nazwie przed utworzeniem nowego.
Korzystanie z kontenera
Po każdym ponownym uruchomieniu komputera (oraz w przypadku problemów z wyświetlaniem aplikacji okienkowych w dockerze), proszę pamiętać o wywoływaniu:
xhost +local:root
Nowy terminal można dołączyć do kontenera korzystając z polecenia:
docker exec -it ARM_07 bash
ROS_DOMAIN_ID
W przypadku pracy na komputerze w laboratorium może okazać się konieczne ustawienie ROS_DOMAIN_ID. Domyślnie, ROS2 rozgłasza wszystko innym komputerom w sieci z tym samym ROS_DOMAIN_ID. Wartość tej zmiennej można ustawić korzystając z:
export ROS_DOMAIN_ID = <wybrana_wartość>
Proszę wybrać losową wartość.
Wygodne może być dodanie tego exportu do bashrc:
echo 'export ROS_DOMAIN_ID=<wybrana_wartość>' >> ~/.bashrc
4. Uruchomienie ORB-SLAM3
Konfigruacja i budowanie
W pliku /arm_ws/src/ORB_SLAM3_ROS2/CMakeLists.txt usuwamy linijkę set(ENV{PYTHONPATH} "/opt/ros/foxy/lib/python3.8/site-packages/"). Dodajemy również find_package(OpenCV REQUIRED) koło innych find_package oraz
target_link_libraries(mono ${OpenCV_LIBS})
target_link_libraries(rgbd ${OpenCV_LIBS})
target_link_libraries(stereo ${OpenCV_LIBS})
target_link_libraries(stereo-inertial ${OpenCV_LIBS})tuż przed ament_package()
Następnie w pliku ORB_SLAM3_ROS2/MakeModules/FindORB_SLAM3.cmake linijkę set(ORB_SLAM3_ROOT_DIR "~/Install/ORB_SLAM/ORB_SLAM3") zmieniamy na set(ORB_SLAM3_ROOT_DIR "/ORB_SLAM3").
Możemy przejść teraz do budowania paczki:
cd /arm_ws
source /opt/ros/humble/setup.bash
colcon build --symlink-install
Uruchamianie
W tej instrukcji będziemy wykorzystywać paczkę ORB_SLAM3_ROS2. Jest to wrapper paczki ORB_SLAM3 do ROS2. Uruchomić system można następującym poleceniem:
cd /arm_ws
source install/setup.bash
ros2 run orbslam3 stereo-inertial /arm_ws/src/orbslam3_ros2/vocabulary/ORBvoc.txt /arm_ws/src/orbslam3_ros2/config/stereo-inertial/EuRoC.yaml true
Przekazywane argumenty to:
ros2 run orbslam3 stereo-inertial PATH_TO_VOCABULARY PATH_TO_YAML_CONFIG_FILE BOOL_RECTIFY
Po załadowaniu słownika, powinno pojawić się okno, w którym będzie wizualizowany przebieg lokalizacji i budowy mapy.
EuRoC
Kolejnym krokiem (w osobnym terminalu) będzie odtworzenie baga zawierającego dane zarejestrowane na potrzeby EuRoC Dataset:
ros2 bag play -r 0.5 bags/MH_02_easy -p --remap /cam0/image_raw:=/camera/left /cam1/image_raw:=/camera/right /imu0:=/imu
Bag zostanie odtworzony zapauzowany i z rate wynoszącym 0.5 normalnego tempa.
Uruchomienie odtwarzania nastąpi po wciśnięciu klawisza spacja z aktywnym terminalem, w którym odtwarzany jest bag.
Po zakończeniu baga można kliknąć przycisk Stop z lewej strony okna ORB-SLAM3: Map Viewer. Spowoduje to zapisanie trajektorii do pliku. 
Następnie można zamknąć program w terminalu korzystając z Ctrl+C.
PUT Car
Na potrzeby zajęć zostały przygotowane również dane zebrane z samochodu, który zrobił okrążenie kampusu Piotrowo na Politechnice Poznańskiej. Sam samochód był wyposażony dodatkowo w DGPS, który dostarczył informacji o referencyjnym położenia co umożliwia ocenę dokładności lokalizacji ORB-SLAM3.
Baga można uruchomić poleceniem:
ros2 bag play -r 0.25 bags/put_car -p --remap /pylon_stereo_node/left/image_raw:=/camera/left /pylon_stereo_node/right/image_raw:=/camera/right
a ORB-SLAM3:
ros2 run orbslam3 stereo /arm_ws/src/orbslam3_ros2/vocabulary/ORBvoc.txt /arm_ws/src/orbslam3_ros2/config/stereo/PP.yaml true
Sam proces nie będzie działał jednak poprawnie (nie jest to potrzebne na dzisiejsze zajęcia). Konieczne jest wprowadzenie poprawek do pliku PP.yaml, więcej informacji można znaleźć w instrukcji do kolejnych zajęć.
Dane z DGPS
Lokalizacja z DGPS znajduje się w topicu /dgps_ublox/fix w polach latitude, longitude, altitude w wiadomości o typie sensor_msgs/NavSatFix. W tej wiadomości znajduje się podwiadomość sensor_msgs/NavSatStatus zawierająca pole status określające aktualny status GPS. Wartości pola status 2 oznacza działanie w trybie FLOAT i FIXED o dokładności kilkunastu i kilku centymetrów, a więc wystarczające na potrzeby ewaluacji. Wiadomości o innym statusie pomijamy.
Zadaniem na dziś jest pobranie tych wiadomości i wygenerowanie danych w układzie kartezjańskim (metrycznym) w pliku w formacie:
timestamp x y z 0 0 0 1
gdzie każda linia oznacza jeden pomiar z DGPS. Timestamp powinien być podany w sekundach (nanosekundy powinny być odpowiednio przemnożone i dodane do sekund) oraz nie powinien być podawany względem pierwszej klatki (tylko taki jaki jest w headerze).
5. Zadanie do samodzielnej realizacji
Efekt dzisiejszego zadania będzie niezbędny na następne zajęcia, dlatego wyjątkowo jest na jego wykonanie 1 tydzień.
Konieczne będzie przeprowadzenie konwersji danych DGPS do układu kartezjańskiego. W tym celu należy:
- Napisać node ROS do subskrypcji danych z DGPS (łatwiejsza opcja) lub napisać skrypt do parsowania danych zapisanych w plikach .bag (link).
- Uznać pierwszy pomiar o statusie 2 lub 3 jako pomiar początkowy (początek układu współrzędnych).
- Z wykorzystaniem z zewnętrznej biblioteki utm obliczyć początkową pozycję w układzie UTM. W c++ można wykorzystać tę funkcję.
- Wykorzystać bibliotekę do konwersji każdego następnego pomiaru do układu metrycznego, a następnie wyrażenie wyniku metrycznego względem pomiaru początkowego.
- Zebranie wszystkich pomiarów i zapisanie ich do pliku.
Uzyskany plik zawiera dane referencyjne do oceny działania systemu ORB-SLAM3. Nazwijmy go dgps.txt.
Proszę o przesłanie skryptu konwertującego i pliku dgps.txt.
Autorzy: Michał Nowicki, Kamil Młodzikowski