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}