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.

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu buster main" > /etc/apt/sources.list.d/ros-noetic.list' sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 sudo apt update sudo apt-get install -y python-rosdep python-rosinstall-generator python-wstool python-rosinstall build-essential cmake sudo rosdep init rosdep update mkdir ~/ros_catkin_ws && cd ~/ros_catkin_ws sudo apt install -y python3-rosdep python3-rosinstall-generator python3-wstool python3-rosinstall build-essential cmake rosinstall_generator desktop --rosdistro noetic --deps --wet-only --tar > noetic-ros_comm-wet.rosinstall *rosinstall_generator ros_comm --rosdistro noetic --deps --wet-only --tar > noetic-ros_comm-wet.rosinstall wstool init src noetic-ros_comm-wet.rosinstall rosdep install -y --from-paths src --ignore-src --rosdistro noetic -r --os=debian:buster sudo src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --install-space /opt/ros/noetic -j1 -DPYTHON_EXECUTABLE=/usr/bin/python3  check: source /opt/ros/noetic/setup.bash roscd roscore

*команда

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.

 <launch>   <arg name="enable_raw" default="true"/>   <arg name="enable_imv" default="false"/>   <arg name="camera_id" default="0"/>   <arg name="camera_frame_id" default="camera"/>   <arg name="camera" default="camerav2_640x480"/>    <node type="raspicam_node" pkg="raspicam_node" name="camera" output="screen">     <param name="private_topics" value="true"/>      <param name="camera_frame_id" value="$(arg camera_frame_id)"/>     <param name="enable_raw" value="$(arg enable_raw)"/>     <param name="enable_imv" value="$(arg enable_imv)"/>     <param name="camera_id" value="$(arg camera_id)"/>      <param name="camera_info_url" value="package://raspicam_node/camera_info/camerav2_640x480.yaml"/>     <param name="camera" value="$(arg camera)"/>     <param name="width" value="512"/>     <param name="height" value="512"/>      <param name="framerate" value="20"/>     <!-- <remap from="/camera/image" to="/camera/image_raw"/> -->     <!-- <remap from="/camera/image/compressed" to="/camera/image_raw"/> -->   </node> </launch> 

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

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

<!-- http://wiki.ros.org/roslaunch/XML/node --> <launch>    <node name="mpu6050_node" pkg="ros_mpu6050_node" type="mpu6050_node" output="screen">       <!--<rosparam file="$(find mypackage)/config/example.yaml" command="load" />-->       <param name="frequency" type="int" value="200" />       <param name="frame_id" type="str" value="base_imu" />       <param name="ax" type="int" value="0" />       <param name="ay" type="int" value="0" />       <param name="az" type="int" value="0" />       <param name="gx" type="int" value="0" />       <param name="gy" type="int" value="0" />       <param name="gz" type="int" value="0" />       <param name="ado" type="bool" value="false" />       <param name="debug" type="bool" value="false" />       <remap from="/imu/data" to="/imu"/>    </node> </launch>

Остались настройки 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 — количество извлекаемых фич (здесь необходим баланс производительности/ качества — меньше фич — быстрее работает, но хуже позиционируется и наоборот).

 %YAML:1.0  #-------------------------------------------------------------------------------------------- # Camera Parameters. Adjust them! #-------------------------------------------------------------------------------------------- File.version: "1.0"  Camera.type: "KannalaBrandt8"  # Camera calibration and distortion parameters (OpenCV)  Camera1.fx: 190.978477 Camera1.fy: 190.973307 Camera1.cx: 254.931706 Camera1.cy: 256.897442  # Equidistant distortion 0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.0002029$ Camera1.k1: 0.003482389402 Camera1.k2: 0.000715034845 Camera1.k3: -0.002053236141 Camera1.k4: 0.000202936736  # Camera resolution Camera.width: 512 Camera.height: 512  # Camera frames per second  Camera.fps: 20  # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) Camera.RGB: 1 #-------------------------------------------------------------------------------------------- # ORB Parameters #-------------------------------------------------------------------------------------------- # ORB Extractor: Number of features per image ORBextractor.nFeatures: 1000 # Tested with 1250 # ORB Extractor: Scale factor between levels in the scale pyramid       ORBextractor.nLevels: 3 #8  # ORB Extractor: Fast threshold # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST # You can lower these values if your images have low contrast ORBextractor.iniThFAST: 20 ORBextractor.minThFAST: 7  #-------------------------------------------------------------------------------------------- # Viewer Parameters #-------------------------------------------------------------------------------------------- Viewer.KeyFrameSize: 0.05 Viewer.KeyFrameLineWidth: 1.0 Viewer.GraphLineWidth: 0.9 Viewer.PointSize: 2.0 Viewer.CameraSize: 0.08 Viewer.CameraLineWidth: 3.0 Viewer.ViewpointX: 0.0 Viewer.ViewpointY: -0.7 Viewer.ViewpointZ: -3.5 Viewer.ViewpointF: 500.0 Viewer.imageViewScale: 0.3 

Запустим все (пока без ноды 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"

Продолжение следует.