ORB_SLAM3 на raspberry pi 4

Рассматриваются нюансы установки ORB_SLAM3 на одноплатном пк — raspberry pi 4 c ОС Raspbian buster, проводится поверхностный анализ возможностей алгоритма с учетом ограничений raspberry, показаны возможные пути оптимизации производительности, используется помимо прочего ROS noetic как связующее звено между imu, csi камерой raspberry pi и ORB_SLAM3. Статья не претендует на научность, излагается мнение автора с опорой на экспериментальную базу.

Вступление

Появилась интересная задача: построить маршрут в помещении, здании и т.п., да так, чтобы по этому маршруту можно было «пройти и назад вернуться». Сверхточность не важна, сколько понимание пространства и маршрута. Под «пройти» подразумевается перемещение слабовидящего человека с неким прибором в руке.

Первое что пришло на ум — это конечно же лидары и камеры глубины. Однако эти решения были отложены как дорогостоящие и/или неудобные.

Было решено посмотреть в сторону ORB_SLAM, у которого «поспела» 3-я версия (с обновлениями 2021г.).
Так как ранее доводилось работать с ORB_SLAM2, но производительность оставляла желать лучшего на слабом железе (raspberry pi3), то сложилось впечатление, что на raspberry pi4 обновленный ORB_SLAM3 должен завестись.

Посмотрим, что из этого получилось.

Но для начала —

Установим ROS noetic

ROS — robot operation system понадобится, чтобы связать данные, поступающие от гироскопа/акселерометра, камеры raspberry pi и ORB_SLAM.

Как установить ROS подробно здесь описываться не будет, приведем только ссылку на статью-источник, следуя которой можно установить ROS на raspbian buster — здесь,
а также краткое изложение команд из статьи по приведенной ссылке.

Небольшие уточнения, далее будут приводиться со знаком "*".

ROS различается наименованиями, каждое из которых «привязано» к определенным дистрибутивам. Так для raspbian buster подходит — ROS noetic.


*команда

rosinstall_generator ros_comm --rosdistro noetic --deps --wet-only --tar > noetic-ros_comm-wet.rosinstall

заменена на другую (предшествующую команду) и выполняться не должна.
**swap file можно не увеличивать, если у вас raspberry pi c 8 Гб RAM.

Также создадим рабочее окружение, в которое в дальнейшем будем устанавливать пакеты ROS:

cd ~
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/
catkin_make
source devel/setup.bash
echo $ROS_PACKAGE_PATH
*outputs: /home/pi/catkin_ws/src:/opt/ros/noetic/share

Usb camera node

Чтобы общаться с камерой на raspberry через ROS, нужна ее(камеры), так называемая, нода (node). Установим ее, хотя этот шаг можно и пропустить, так как в дальнейшем мы установим «специализированную» именно для raspberry ноду — raspicam node.
Однако usb camera node более универсальна, и, если не получится с raspicam node, эта точна должна заработать.

cd ~/catkin_ws/src
git clone https://github.com/bosch-ros-pkg/usb_cam.git
git clone https://github.com/ros-perception/image_common.git
git clone https://github.com/ros-perception/image_pipeline.git
git clone https://github.com/ros-perception/vision_opencv.git
cd ..
catkin_make

Пакетов несколько, так как нода требует дополнительные зависимости.

Imu node (mpu650)

Этот шаг можно пропустить, для ORB_SLAM будет достаточно и одной камеры (без параллельно работающего imu), однако точность будет хромать и невозможно будет запустить пример с суффиксом inertial.
На данном этапе подразумевается, что будет установлена нода, отвечающая за гироскоп/акселерометр GY-521 (mpu-650). Данный imu дешев и достаточно распространен. Его легко купить и он так же необязателен в своих показаниях. Этот факт мы учтем в дальнейшем. Хорош imu также тем, что неприхотлив и подключается напрямую к raspberry pi gpio, используя 5V, вместо положенных 3.

Чтобы imu заработал необходимо добавить

sudo nano /etc/modules

две строки:

i2c-bcm2708
i2c-dev

Перезагрузить систему и можно посмотреть на адрес imu:

sudo apt-get install i2c-tools python-smbus
i2cdetect -y -r 1
sudo i2cget -y 1 0x68 0x75

Как правило, это 0x68.

Также здесь пригодится статья, которая прояснит отдельные моменты, связанные с imu, а также позволит его проверить на работоспособность — здесь.

Случается иногда ошибка с imu:

Error: Could not set address to 0x68: Device or resource busy

Причины ее могут быть различны, но одна из них, может быть наличие rtc часов, которые когда-то были подключены и о которых «забыли». Чтобы выгрузить часы, пригодится:

lsmod | grep rtc
sudo rmmod rtc-ds1307

После этого, imu должен заработать.

Убедившись, что imu работает и может что-то отдавать в serial или иной порт, установим ROS ноду для него.

За основу будет взят пакет ноды mpu6050 — отсюда.
Внимание! Порядок установки будет отличаться от описанного в readme на github.
Далее по readme репо.

Здесь без изменений:

sudo mkdir -p /usr/share/arduino/libraries
cd /usr/share/arduino/libraries
sudo git clone https://github.com/chrisspen/i2cdevlib.git

*В этом шаге понадобится patch, который можно взять отсюда — скачать.

cd /tmp
wget http://www.airspayce.com/mikem/bcm2835/bcm2835-1.59.tar.gz
cd bcm2835-1.59
tar zxvf bcm2835-1.59.tar.gz
patch -lNp1 --input=bcm2835_rpi4.patch
./configure
make
make check
sudo make install

Далее непосредственно установим саму ноду для imu:

cd ~/catkin_ws
source devel/setup.bash
cd ~/catkin_ws/src
git clone https://github.com/chrisspen/ros_mpu6050_node
cd ..
catkin_make --pkg ros_mpu6050_node

Ошибки, которые могут поджидать:
—

Initializing I2C...
bcm2835_init: gpio mmap failed: Cannot allocate memory
[mpu6050_node-2] process has died [pid 3427, exit code -11,

Возможные пути решения изложены здесь и здесь.
—

error FIFO overflow!

Решение: увеличить параметр в launch файле imu:

<param name="frequency" type="int" value="10" />

Как правило, достаточно изменить на 100 или 200 с 10.

Надо ли дополнительно калибровать сам imu?

Здесь мнения в источниках расходятся, одни говорят, что достаточно взять показания из datasheet и для дешевых imu умножить на 10, другие предлагают пройти процесс калибровки. Для последних оставлю ссылку.

Теперь, когда ROS установлена, а также установлены ноды imu и камеры, можно проверить, что они работают.

Понадобится открыть 3и терминала, и выполнить отдельно команды:

bash -c "source /opt/ros/noetic/setup.bash; roscore"	

*запустили мастер ноду

bash -c "source /home/pi/catkin_ws/devel/setup.bash; roslaunch usb_cam usb_cam-test.launch"

*запустили ноду камеры

sudo bash -c "source /home/pi/catkin_ws/devel/setup.bash; roslaunch ros_mpu6050_node mpu6050.launch"

*запустили ноду imu

Если все работает, отлично! Движемся дальше.

Raspicam-node

Raspicam нода менее требовательна к ресурсам по сравнению с usb camera нодой, которую установили ранее. Поэтому ее рекомендуется (автором) использовать.

Вот сравнение потребления вычислительных мощностей нодами-побратимами при одинаковых настройках:





Установка базируется на репо — github.com/UbiquityRobotics/raspicam_node, однако отличается.

sudo apt-get install libogg-dev libvorbis-dev libtheora-dev
cd catkin_ws/src
git clone https://github.com/ros-perception/image_transport_plugins
git clone https://github.com/UbiquityRobotics/raspicam_node.git
cd ..
catkin_make

Нода запускается командой:

roslaunch raspicam_node camerav2_1280x960.launch

Картинку не выводит, т.к. в launch файле нет image_viewer, и, чтобы убедиться, что все работает, можно либо послушать топик

rostopic echo rostopic echo /raspicam_node/image_raw

либо посмотреть в rviz, подписавшись на соответствующий топик.

Установка ORB_SLAM3

Пакет устанавливается не в catkin_ws!

В целях экономии места и терпения опишем порядок установки тезисно:
---libraries---

sudo apt-get install libboost-all-dev libboost-dev libssl-dev libpython2.7-dev libeigen3-dev

---Pangolin---

cd ~
git clone https://github.com/stevenlovegrove/Pangolin
cd Pangolin
./scripts/install_prerequisites.sh recommended
cmake -B build -GNinja
cmake --build build

---opencv---
*https://qengineering.eu/install-opencv-4.5-on-raspberry-pi-4.html

wget https://github.com/Qengineering/Install-OpenCV-Raspberry-Pi-32-bits/raw/main/OpenCV-4-5-5.sh
sudo chmod 755 ./OpenCV-4-5-5.sh
./OpenCV-4-5-5.sh

---ORB_SLAM3---

git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git ORB_SLAM3
cd ORB_SLAM3
chmod +x build.sh
sed -i 's/++11/++14/g' CMakeLists.txt
./build.sh

ORB_SLAM установлен, но необходимо еще собрать для него ROS ноды, которые не устанавливаются автоматически при инсталляции (при запуске ./build.sh):

1. Изменим CMakeLists.txt:

cd /home/pi/ORB_SLAM3/Examples_old/ROS/ORB_SLAM3
sed -i 's/++11/++14/g' CMakeLists.txt

здесь же закомментируем AR nodes, т.к. они не собираются:

# Node for monocular camera (Augmented Reality Demo)
#rosbuild_add_executable(MonoAR
#src/AR/ros_mono_ar.cc
#src/AR/ViewerAR.h
#src/AR/ViewerAR.cc
#)
#target_link_libraries(MonoAR
#${LIBS}
#)

здесь же в начало файла CMakeLists.txt добавим:

project(ORB_SLAM3)

2. Изменим build_ros.sh (/home/pi/ORB_SLAM3):

echo "Building ROS nodes"
cd Examples_old/ROS/ORB_SLAM3
mkdir build
cd build
cmake .. -DROS_BUILD_TYPE=Release -DPYTHON_EXECUTABLE=/usr/bin/python3
make -j

3.установим библиотеки:

---Sophus---
cd ~
git clone https://github.com/strasdat/Sophus.git
cd Sophus
mkdir build && cd build && cmake .. && sudo make install
---fmt---
sudo apt install libfmt-dev

Заменим Sophus из /home/pi/ORB_SLAM3/Thirdparty/Sophus
на Sophus /home/pi/Sophus
*Удалить директорию и скопировать ту, что сбилдили выше.
4. Изменить топики, которые будет «слушать» ORB_SLAM3, чтобы камера(raspicam_node) и ORB_SLAM общались в одном топике:

nano /home/pi/ORB_SLAM3/Examples_old/ROS/ORB_SLAM3/src/ros_mono.cc
ros::Subscriber sub = nodeHandler.subscribe("/camera/image", 1, &ImageGrabber::GrabImage,&igb);

*это можно не делать именно здесь, но потом сделать remap в launch файле с камерой. Ниже будет пометка.
5. Собрать ORB_SLAM3 ноды:

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/pi/ORB_SLAM3/Examples_old/ROS/ORB_SLAM3
source /opt/ros/noetic/setup.bash
cd ~/ORB_SLAM3
./build_ros.sh

Теперь, когда все готово, можно запустить все необходимые ноды, совместно с нодами ORB_SLAM3, используя уже 4-е терминала:

bash -c "source /opt/ros/noetic/setup.bash; roscore"

bash -c "source /home/pi/catkin_ws/devel/setup.bash; roslaunch usb_cam usb_cam-test.launch"

*либо

bash -c "source /home/pi/catkin_ws/devel/setup.bash; roslaunch raspicam_node camerav2_1280x960.launch"

bash -c "source /home/pi/catkin_ws/devel/setup.bash;cd /home/pi/ORB_SLAM3/Examples_old/ROS/ORB_SLAM3;
./Mono /home/pi/ORB_SLAM3/Vocabulary/ORBvoc.txt /home/pi/ORB_SLAM3/Examples/Monocular/TUM1.yaml"

sudo bash -c "source /home/pi/catkin_ws/devel/setup.bash; roslaunch ros_mpu6050_node mpu6050.launch"

Строим карту с ORB_SLAM3

ORB_SLAM строит своеобразные карты и к ним поначалу сложно привыкнуть. Особенно, если вы привыкли к картам, построенным лидаром или иным «дорогостоящим» прибором.



На изображении видно несколько десятков «почтовых конвертов» — это камера. При ее перемещении также можно наблюдать облако точек (на картинке выше его нет), point cloud.
Однако, чтобы перемещаться по помещению, нужен худо-бедно какой-нибудь мобильный прибор.

Можно взять за образец следующую модель, собранной на фанере от мобильного робота:





Двух аккумуляторов 18650 хватает на 2-3 часа.

Перед тем как строить карту необходимо поработать с настройками launch файлов как самого ORB_SLAM3 так и нод камеры и imu.

Начнем с камеры.

nano /home/pi/catkin_ws/src/raspicam_node/launch/camerav2_512x512_fish.launch

Поправим launch файл raspicam_node, снизив fps до 20, и выставим параметр enable_raw в true, так как ORB_SLAM3 не переваривает сжатые (image_compressed) изображения, которые по умолчанию шлет raspicam_node.


Разрешение лучше не трогать, так как далее в ORB_SLAM файле параметров будет использоваться файл с настройками калибровки камеры именно под картинку 512х512.

*Теперь, если планируется использовать гироскоп/акселерометр, поправим соответствующую ноду.
nano /home/pi/catkin_ws/src/ros_mpu6050_node/launch/mpu6050.launch
Здесь надо выставить частоту — 100-200 Гц, а также сделать remap (перенаправление) топиков, чтобы imu публиковал данные в топик, который будет слушать ORB_SLAM3.


Остались настройки ORB_SLAM3

Здесь немного посложнее.

Параметры хранятся в файлах формата yaml, поэтому править необходимо их. Что мы и сделаем.
Если у вас камера типа Pinhole, то правим TUM1.yaml:

nano  /home/pi/ORB_SLAM3/Examples/Monocular/TUM1.yaml

если fish-eye, в понятиях ORB_SLAM3 это тип KannalaBrandt8, то TUM-VI.yaml:

nano  /home/pi/ORB_SLAM3/Examples/Monocular/TUM-VI.yaml

В целом у приведенных TUM файлов разницы немного в части содержимого.

Необходимо снизить ORBextractor.nLevels, обратить внимание на fps, разрешение 512х512, а также ORBextractor.nFeatures — количество извлекаемых фич (здесь необходим баланс производительности/ качества — меньше фич — быстрее работает, но хуже позиционируется и наоборот).



Запустим все (пока без ноды imu):

1-st terminal:

bash -c "source /opt/ros/noetic/setup.bash; roscore"

2-d:

bash -c "source /home/pi/catkin_ws/devel/setup.bash; roslaunch usb_cam usb_cam-test.launch"

либо

bash -c "source /home/pi/catkin_ws/devel/setup.bash; roslaunch raspicam_node camerav2_512x512_fish.launch"

3d:

bash -c "source /home/pi/catkin_ws/devel/setup.bash;cd /home/pi/ORB_SLAM3/Examples_old/ROS/ORB_SLAM3;
./Mono /home/pi/ORB_SLAM3/Vocabulary/ORBvoc.txt /home/pi/ORB_SLAM3/Examples/Monocular/TUM-VI.yaml"

В целом, raspberry неплохо справляется:



Однако, при резких поворотах мощностей явно не хватает.

Тем не менее, если двигаться неспеша и не дергать камерой, то можно построить «маршрут»:



Так выглядит пройтись туда и обратно с плавным разворотом камеры.

Сохранить карту, нажав на кнопку gui не получится. Такой кнопки нет. Чтобы карта записалась необходимо перед стартом ORB_SLAM3 добавить строку в TUM.yaml файл:

System.SaveAtlasToFile: "my_01"

, где my_01 — название карты.
При этом карта будет сохраняться в /home/pi/ORB_SLAM3/Examples_old/ROS/ORB_SLAM3 и называться my_01.osa.

Чтобы загрузить карту при старте ORB_SLAM, которая ранее была построена в том же TUM файле указать:

System.LoadAtlasFromFile: "my_01"

ORB_SLAM3 c imu.

Пример запуска с учетом imu будет выглядеть так:

bash -c -E "source /home/pi/catkin_ws/devel/setup.bash;cd /home/pi/ORB_SLAM3/Examples_old/ROS/ORB_SLAM3;
./Mono_Inertial /home/pi/ORB_SLAM3/Vocabulary/ORBvoc.txt /home/pi/ORB_SLAM3/Examples/Monocular-Inertial/TUM-VI_far.yaml"

*с учетом уже ранее запущенных нод в других терминалах roscore, raspicam_node и собственно imu:


Не лишним будет убедиться, что imu шлет сообщения в топик imu:

rostopic echo /imu

Содержимое
Здесь необходимо обратить внимание на IMU.Frequency: 100.0 и System.thFarPoints: 3.0.
Частоту imu необходимо будет поменять в launch файле imu ноды с 200 на 100.
System.thFarPoints — это расстояние, дальше которого ORB_SLAM не будет извлекать фичи из изображения.

На этом пока все.