Калибровка камеры-imu с Kalibr

Предисловие.

Данная статья будет перекликаться с предыдущей публикаций, в которой был рассмотрен процесс установки и использования ORB_SLAM3.

Как выяснилось, несмотря на наличие в ORB_SLAM3 нескольких файлов с настройками калибровки камеры для различных типов камер (fish-eye, pinhole), настроек в связке imu-camera в пакете ORB_SLAM3 ограниченное количество. Кроме того, возник вопрос — будет ли работать алгоритм быстрее на raspberry pi, чьи вычислительные ресурсы ограничены, если уменьшить разрешение кадра хотя бы в два раза.

К сожалению, установить Kalibr на Raspbian buster не удалось, поэтому далее будет использоваться один из рекомендованных самим Kalibroм дистрибутивов, а именно — Ubuntu 20.04 в связке с ROS Noetic.

О Kalibr.

Что такое Kalibr и чем он лучше/хуже других пакетов калибровки?

Если заглянуть на официальный репозиторий Kalibra, то можно выяснить, что он позволяет проводить калибровку в нескольких направлениях:

• Multi-Camera Calibration: Intrinsic and extrinsic calibration of a camera-systems with non-globally shared overlapping fields of view with support for a wide range of camera models.
• Visual-Inertial Calibration (CAM-IMU): Spatial and temporal calibration of an IMU w.r.t a camera-system along with IMU intrinsic parameters
• Multi-Inertial Calibration (IMU-IMU): Spatial and temporal calibration of an IMU w.r.t a base inertial sensor along with IMU intrinsic parameters (requires 1-aiding camera sensor).
• Rolling Shutter Camera Calibration: Full intrinsic calibration (projection, distortion and shutter parameters) of rolling shutter cameras.

Как правило, большинство калибровок камер ограничивается первым и последним пунктами, не обращая внимание на imu.

В принципе, этих двух калибровок вполне достаточно, чтобы запустить и поработать с ORB_SLAM3 например. Однако, если требуется точность, то придется выполнить CAM_IMU calibration.

Из плюсов kalibr также необходимо отметить подробную wiki с инструкциями и дополнительные инструменты, такие как настройка фокусного расстояния камеры, проверка параметров калибровки и т.д.

Нюансы установки Kalibr на ubuntu 20.04 raspberry pi.

В общении с ubuntu на raspberry из под windows рекомендую использовать x2go:

sudo apt-get install x2goserver x2goserver-xsession sudo systemctl status x2goserver

В репозитории Kalibra установке посвящена страничка wiki.

Однако, воспользоваться docker на raspberry не получится и придется все собирать из исходников.

Чтобы получать информацию с камеры и imu, потребуются также их ROS nodы, установка которых описана в статье про ORB_SLAM3, а также дополнительные ROS пакеты:

Установка этих пакетов не замысловата — скачиваем их в workspace/src и далее из workspace выполняем:

 cd ~/kalibr_workspace catkin build -DCMAKE_BUILD_TYPE=Release -j4

Все должно пройти штатно… кроме raspicam_node и imu node, установка и использование которых немного отличаются.

Raspicam node на ubuntu.

Конечно, возможно, отказаться от использования raspicam node и использовать usb camera node, так как в целом последняя повторяет функционал. Однако, как упоминалось в статье про ORB_SLAM3 между нодами есть отличия.

Этапы установки следующие.

Добавляем обнаружение камеры в системе и проверяем после перезагрузки, что камера определилась:

sudo nano /boot/firmware/config.txt start_x=1 gpu_mem=128 vcgencmd get_camera supported=1 detected=1

Устанавливаем зависимости:

sudo apt-get install libogg-dev libvorbis-dev libtheora-dev

Движемся по инструкции репо raspicam node от userland:

---userland--- *https://github.com/raspberrypi/userland/issues/597 git clone https://github.com/raspberrypi/userland.git cd userland sed -i 's/__bitwise/FDT_BITWISE/'  /home/ubuntu/userland/opensrc/helpers/libfdt/libfdt_env.h sed -i 's/__force/FDT_FORCE/'  /home/ubuntu/userland/opensrc/helpers/libfdt/libfdt_env.h ./buildme --aarch64

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

LD_PRELOAD="/home/ubuntu/userland/build/lib/libmmal_vc_client.so /home/ubuntu/userland/build/lib/libvcsm.so /home/ubuntu/userland/build/lib/libmmal_core.so /home/ubuntu/userland/build/lib/libmmal_util.so" /opt/vc/bin/raspistill -o cam.jpg

Либо так:

export LD_PRELOAD="/home/ubuntu/userland/build/lib/libmmal_vc_client.so /home/ubuntu/userland/build/lib/libvcsm.so /home/ubuntu/userland/build/lib/libmmal_core.so /home/ubuntu/userland/build/lib/libmmal_util.so" cd /opt/vc/bin ./raspistill -k

Imu node на ubuntu.

Чтобы заставить работать imu, также придется выполнить определённые шаги, а именно:

Для начала небходимо попытаться запустить

 '''         Read Gyro and Accelerometer by Interfacing Raspberry Pi with MPU6050 using Python http://www.electronicwings.com ''' import smbus#import SMBus module of I2C from time import sleep          #import  #some MPU6050 Registers and their Address PWR_MGMT_1   = 0x6B SMPLRT_DIV   = 0x19 CONFIG       = 0x1A GYRO_CONFIG  = 0x1B INT_ENABLE   = 0x38 ACCEL_XOUT_H = 0x3B ACCEL_YOUT_H = 0x3D ACCEL_ZOUT_H = 0x3F GYRO_XOUT_H  = 0x43 GYRO_YOUT_H  = 0x45 GYRO_ZOUT_H  = 0x47   def MPU_Init(): #write to sample rate register bus.write_byte_data(Device_Address, SMPLRT_DIV, 7)  #Write to power management register bus.write_byte_data(Device_Address, PWR_MGMT_1, 1)  #Write to Configuration register bus.write_byte_data(Device_Address, CONFIG, 0)  #Write to Gyro configuration register bus.write_byte_data(Device_Address, GYRO_CONFIG, 24)  #Write to interrupt enable register bus.write_byte_data(Device_Address, INT_ENABLE, 1)  def read_raw_data(addr): #Accelero and Gyro value are 16-bit         high = bus.read_byte_data(Device_Address, addr)         low = bus.read_byte_data(Device_Address, addr+1)              #concatenate higher and lower value         value = ((high << 8) | low)                  #to get signed value from mpu6050         if(value > 32768):                 value = value - 65536         return value   bus = smbus.SMBus(1) # or bus = smbus.SMBus(0) for older version boards Device_Address = 0x68   # MPU6050 device address  MPU_Init()  print (" Reading Data of Gyroscope and Accelerometer")  while True:  #Read Accelerometer raw value acc_x = read_raw_data(ACCEL_XOUT_H) acc_y = read_raw_data(ACCEL_YOUT_H) acc_z = read_raw_data(ACCEL_ZOUT_H)  #Read Gyroscope raw value gyro_x = read_raw_data(GYRO_XOUT_H) gyro_y = read_raw_data(GYRO_YOUT_H) gyro_z = read_raw_data(GYRO_ZOUT_H)  #Full scale range +/- 250 degree/C as per sensitivity scale factor Ax = acc_x/16384.0 Ay = acc_y/16384.0 Az = acc_z/16384.0  Gx = gyro_x/131.0 Gy = gyro_y/131.0 Gz = gyro_z/131.0   print ("Gx=%.2f" %Gx, u'\u00b0'+ "/s", "\tGy=%.2f" %Gy, u'\u00b0'+ "/s", "\tGz=%.2f" %Gz, u'\u00b0'+ "/s", "\tAx=%.2f g" %Ax, "\tAy=%.2f g" %Ay, "\tAz=%.2f g" %Az)  sleep(1) 

Если imu шлет данные, двигаемся дальше, если нет, то:

pip3 install smbus sudo nano /lib/udev/rules.d/60-i2c-tools.rules KERNEL=="i2c-0"     , GROUP="i2c", MODE="0660" KERNEL=="i2c-[1-9]*", GROUP="i2c", MODE="0666"  sudo groupadd gpio sudo usermod -a -G gpio ubuntu sudo grep gpio /etc/group sudo chown ubuntu.gpio /dev/gpiomem sudo chmod g+rw /dev/gpiomem

Также в процессе установки imu node (описанной в статье ORB_SLAM3), не забываем про патч, который там также упомянут:

download patch https://groups.google.com/g/bcm2835/c/BwZXVsDRtwI tar zxvf bcm2835-1.59.tar.gz cd bcm2835-1.59 patch -lNp1 --input=/path/to/patch/bcm2835_rpi4.diff 

Сборка ноды:

cd ~/kalibr_workspace/src catkin build -DCMAKE_BUILD_TYPE=Release -j4

Перед тем как начать калибровку.

В идеале kalibr должен запускаться командой

rosrun kalibr <command_you_want_to_run_here>

Однако, если этого не происходит, то можно запускать исполняемые файлы kalibra из директории:

/home/ubuntu/kalibr_workspace/src/kalibr/aslam_offline_calibration/kalibr/python

Все будет работать.

Также в этих файлах, чтобы они работали необходимо внести небольшие косметические изменения, а именно: переставить import cv2 в первые позиции перед другими импортами:

Алгоритм калибровки

будет выглядеть так:

• настроить фокус камеры,
• настроить сенсор камеры,
• подготовить доску с aprilgrid,
• записать rosbag для static dataset,
• откалибровать со static dataset,
• записать rosbag для dynamic dataset,
• откалибровать с dynamic dataset.

Отчасти данный алгоритм приведен по ссылке.
Пройдемся по алгоритму и разберем нюансы.

Настроить фокус камеры.

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

Можно это сделать «на глазок», убедившись, что камера четко видит изображение.

Но можно и воспользоваться средствами самого kalibra.

Последующие команды можно выполнять непосредственно на raspberry, а можно и через сессию x2go. Но для последнего варианта необходимо поправить raspicam ноду, добавив в launch файл viewer (перед закрывающим тегом launch):

<node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">     <remap from="image" to="image"/>     <param name="autosize" value="true" />   </node> </launch> 

Полностью launch файл

<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="raspicam"/>   <arg name="camera_name" default="camerav2_320x240"/>    <node type="raspicam_node" pkg="raspicam_node" name="raspicam_node" output="screen">     <param name="private_topics" value="false"/>      <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_name" value="$(arg camera_name)"/>     <param name="width" value="320"/>     <param name="height" value="240"/>      <param name="framerate" value="20"/>     <param name="exposure_mode" value="antishake"/>     <param name="shutter_speed" value="0"/>   </node> <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">     <remap from="raspicam_node/image" to="image"/>     <param name="autosize" value="true" />   </node> </launch> 

Запустив мастер-ноду:

sudo bash -c "source /home/ubuntu/kalibr_workspace/devel/setup.bash; roscore"

В другом терминале запустим raspicam node (ноду камеры):

export LD_PRELOAD="/home/ubuntu/userland/build/lib/libmmal_vc_client.so /home/ubuntu/userland/build/lib/libvcsm.so /home/ubuntu/userland/build/lib/libmmal_core.so /home/ubuntu/userland/build/lib/libmmal_util.so" sudo bash -c "source /home/ubuntu/kalibr_workspace/devel/setup.bash; roslaunch raspicam_node camerav2_320x240.launch"

Убедимся, что все работает:

и запустим файл, который выведет картинку с фокусом камеры:

cd /home/ubuntu/kalibr_workspace/src/kalibr/aslam_offline_calibration/kalibr/python python3 kalibr_camera_focus --topic /image/compressed

Задача после запуска исполняемого файла:
крутить оптику камеры, чтобы fde был минимален (на выводимой картинке виден параметр). При этом -11 лучше, чем -9. Да и визуально будет понятно, когда камера расфокусирована, а когда нет.

Фокусироваться желательно на той поверхности, на которой будет висеть или стоять калибровочная доска.

Настроить сенсор камеры.

Этот шаг, несмотря на его наличие в официальном wiki kalibra можно пропустить, если взять с себя обещание не делать резких движений при съемке с камеры и наличии достаточного освещения снимаемой калибровочной поверхности.

Тем не менее, если этот шаг в числе рекомендуемых, пройдемся по нему.

Запустив roscore, запустим raspicam node (camerav2_640x480.launch или иной), чтобы видеть картинку:

sudo bash -c "source /home/ubuntu/kalibr_workspace/devel/setup.bash; roslaunch raspicam_node camerav2_320x240.launch"

В третьем терминале запустим пакет перенастройки камеры:

sudo bash -c "source /home/ubuntu/kalibr_workspace/devel/setup.bash; rosrun rqt_reconfigure rqt_reconfigure"

Откроется окно, в котором можно на лету менять настройки камеры и одновременно наблюдать эффект на выводимом raspicam node изображении.

В этом окне необходимо поменять настройки, которые должны привести к уменьшению размытия изображения при движении камеры:

Далее эти параметры можно через окно reconfigure сохранить в yaml файл, но можно и не сохранять, т.к. эти параметры, которые вы изменили — параметры по умолчанию до момента пока вы их снова не поменяете.

Подготовить доску с aprilgrid.

Kalibr предлагает для калибровки три типа «досок»:

• aprilgrid;
• сheckerboard(шахматная доска);
• circlegrid.

Как правило, на практике при калибровке используется шахматная доска как наиболее понятное и доступное.

Особой разницы между досками нет, но сам kalibr рекомендует использовать aprilgrid:

It is recommended to use the Aprilgrid due to the following benefits:

• partially visible calibration boards can be used
• pose of the target is fully resolved (no flips)

Поэтому калибровка будет производиться с aprilgrid.

Что такое aprilgrid ?

Это, как уже было сказано, всего лишь картинка с квадратиками. Однако, их размеры и расстояние между ними имеет значение и об этом будет сказано дополнительно.

Саму картинку с aprilgrid можно скачать с wiki kalibra — здесь.

Понадобятся как картинка в pdf, так и yaml файл к ней:

Данная картинка должна в идеале быть 80 на 80 см, но не у каждого есть такой принтер, поэтому обычная распечатка на А4 тоже сгодится.

Однако есть следующие рекомендации:

— не печатать в градациях серого, т.е. квадраты должны быть черного цвета без искажений,
— клеить aprilgrid на матовую ровную поверхность без глянца и бликов,
— понадобятся два экземляра картинки:

• один приклеить на ровную поверхность, которую нужно будет перемещать перед неподвижной камерой, создавая таким образом static dataset
• второй экземпляр — приклеить на стену или закрепить на штативе, он будет неподвижен во время перемещения камеры вокруг него — dynamic dataset.

— картинку желательно использовать 6х6 квадратов, она приведена по ссылке выше, несмотря на то, что у kalibra есть файл генерации других паттернов с большим разнообразием.

С произвольно сконфигурированным aprilgrid может не запуститься калибровка, выдав ошибку:

[FATAL] [1663165566.276776]: No corners could be extracted for camera /raspicam_node/image/compressed! Check the calibration target configuration and dataset.

— обратить внимание на маленькие оси на картинке, картинка должна их учесть. То есть после распечатки картинки, не переворачивать ее, не зеркалить и т.п.

— не оставлять слишком много белого пространства листа вокруг картинки.

Резюмируя: если вы просто выведите на А4 картинку без искажений (лист А4 вертикально) и наклеите ее на ровную поверхность, все будет хорошо.

Далее нужно будет поправить yaml файл для aprilgrid. Здесь ничего сложного.

Необходимо на распечатанной картинке с aprilgrid измерить расстояния и внести правки:

Записать rosbag для static dataset.

После того как доски калибровки готовы, можно перейти к записи rosbag.

Rosbag — это условно говоря «сумка», в которую ROS складывает данные из топиков. Из каких именно топиков и как долго складывает — решает пользователь. Похоже на запись аудио-/видео- файлов только в особом формате.

Перед тем как запустить непосредственно калибровку необходимо записать rosbag, из которого программа далее будет извлекать данные и по ним калибровать. О том, ЧТО записалось в rosbag и записалось ли вообще будет указано далее.

Перед стартом необходимо:

— подготовить доску с aprilgrid для static dataset, которую предстоит перемещать перед камерой, записывая rosbag.
— запустить и убедиться, что работают топики roscore и raspicam node (или usb node)

sudo bash -c "source /home/ubuntu/kalibr_workspace/devel/setup.bash; roscore" export LD_PRELOAD="/home/ubuntu/userland/build/lib/libmmal_vc_client.so /home/ubuntu/userland/build/lib/libvcsm.so /home/ubuntu/userland/build/lib/libmmal_core.so /home/ubuntu/userland/build/lib/libmmal_util.so" sudo bash -c "source /home/ubuntu/kalibr_workspace/devel/setup.bash; roslaunch raspicam_node camerav2_320x240.launch"

— доска хорошо освещена.

В терминале запускаем запись:

rosbag record /image --duration=60 -O static.bag #60секунд записи только видеокамеры

Далее необходимо плавно, не спеша перемещать доску с aprilgrid перед камерой из края в край в зоне ее видимости на фокусном расстоянии (которое ранее настроили). Приближать-удалять или совсем убирать доску не нужно, так как данные кадры отбракуются скорее всего как размытые или в связи с отсутствием доски. Также лучше не дрожать, не дергать руками и двигаться достаточно медленно, чтобы минимизировать размытие, которое все равно будет возникать на rolling shutter камерах.

После записи в директории появится файл static.bag и этого файла будет достаточно, если цель — откалибровать только камеру без учета imu.

Откалибровать со static dataset.

После того, как получен rosbag, можно приступить к собственно калибровке.

Команда калибровки для static dataset (без imu):

cd ~/kalibr_workspace/src/kalibr/aslam_offline_calibration/kalibr/python python3 kalibr_calibrate_cameras --model pinhole-equi --topics /image --bag static.bag --target aprilgrid_6x6.yaml

Соответственно, для fish-eye камеры с rolling shutter:

python3 kalibr_calibrate_rs_cameras --model pinhole-radtan-rs --topic /image --bag 320x240_static.bag --target aprilgrid_6x6.yaml --inverse-feature-variance 1 --frame-rate 20

По результатам калибровки со static dataset kalibr выдаст файл

cam0:   camera_model: pinhole   distortion_coeffs: [-0.29585544183959606, 0.06950559854738034, 0.000981636728463331, 0.0010529550643745315]   distortion_model: radtan   intrinsics: [155.59508048438533, 155.001082471171, 148.3289996162289, 128.18248085164035]   line_delay: 8.839452618620747e-05   resolution: [320, 240]   rostopic: /image 

, который будет нужен для дальнейшей калибровки уже с imu.

*Во время запуска калибровки может выскочить ошибка:

Это произошло потому, что при записи rosbag использовался launch файл raspicam nodы, который не ссылается на yaml файл настроек камеры.

Чтобы заново не записывать rosbag необходимо вручную указать FOCAL_LENGTH.
Перед запуском калибровки ввести:

export KALIBR_MANUAL_FOCAL_LENGTH_INIT=1

И, далее, когда программа попросит ввести значение:

Значение 158 получено расчетным путем:
Технические характеристики камеры Waveshare Camera H:
Диагональ ¼ дюйма
Длина фокуса 3.15 мм
Разрешение кадра: 320х240
3,15мм/6,35ммx320=158

**Вышеуказанная ошибка не возникнет, если, например записать rosbag, используя стандартный launch файл raspicam node, у которого есть yaml файл настроек, например camerav2_640x480.launch и далее запустить калибровку:

python3 kalibr_calibrate_rs_cameras --model pinhole-radtan-rs --topic /image --bag 640x480_static.bag --target aprilgrid_6x6.yaml --inverse-feature-variance 1 --frame-rate 30

При старте программа покажет FOCAL_LENGTH:

Если посчитать его вручную:
317,48 (3,15мм/6,35ммx640). Похоже на правду.

Записать rosbag для dynamic dataset.

Подготовка к записи немного отличается от предыдущего пункта:
— в launch файле камеры (raspicam node) выставить частоты кадров (fps) 20Hz, в launch imu частоту- 200Hz.
— aprilgrid закреплен на стене неподвижно, так как камера с imu будет совершать движения вокруг aprilgrid.
— запустить предварительно необходимые ноды с roscore, raspicam и imu:

1st: sudo bash -c "source /home/ubuntu/kalibr_workspace/devel/setup.bash; roscore" 2d: export LD_PRELOAD="/home/ubuntu/userland/build/lib/libmmal_vc_client.so /home/ubuntu/userland/build/lib/libvcsm.so /home/ubuntu/userland/build/lib/libmmal_core.so /home/ubuntu/userland/build/lib/libmmal_util.so" sudo bash -c "source /home/ubuntu/kalibr_workspace/devel/setup.bash; roslaunch raspicam_node camerav2_320x240.launch" 3d: sudo bash -c "source /home/ubuntu/kalibr_workspace/devel/setup.bash; roslaunch ros_mpu6050_node mpu6050.launch"

Далее можно запускать запись rosbag (dynamic dataset):

rosbag record /raspicam_node/image/compressed --duration=180 /imu/data -O dynamiс_320x240_30fps_with_imu.bag #180секунд записи видеокамеры и imu

Сам процесс таинственных движений с камерой и imu перед aprilgrid (pitch,yaw,roll) можно посмотреть на видео, размещенном на странице калибра. При всем при этом, движения на видео чрезмерно быстрые, что может сказаться на снимках с использованием камеры с rolling shutter.

Какого размера может получиться rosbag? Добавление imu не сильно сказывается на размере файла на выходе, так как львиную долю там занимают картинки с камеры. Чем выше разрешение камеры и дольше запись rosbag, тем больше размер rosbag.

Откалибровать c dynamic dataset.

Калибровка с dynamic dataset:

 cd ~/kalibr_workspace/src/kalibr/aslam_offline_calibration/kalibr/python python3 kalibr_calibrate_imu_camera --cam 320x240_static-camchain.yaml --target aprilgrid_6x6.yaml --imu imu0.yaml --bag dynamiс_320x240.bag

Здесь все используемые файлы ранее встречались кроме imu0.yaml. Как можно понять из названия, это файл с настройками для imu. В нем должны содержаться параметры imu либо из datasheet либо полученные при калибровке самого imu.

Вот пример содержимого

rostopic: /imu/data accelerometer_noise_density: 0.05 #continous accelerometer_random_walk: 0.001 gyroscope_noise_density: 0.02 #continous gyroscope_random_walk: 4.0e-05 update_rate: 200 

По завершении калибровки для dynamic dataset, которая занимает от 15 мин и более в зависимости от размера rosbag, kalibr выдаст итоговые файлы, один из которых будет содержать сведения калибровки camera-imu.

Примеры файлов:
— 320x240_dynamic-results-imucam.txt
— 320x240_dynamic-report-imucam.pdf

На этом калибровка с kalibr закончена!

Посмотреть в rosbag.

Иногда, по непонятным причинам калибровка не запускается либо проводится неудачно.
В этих случаях может помочь изучение записавшегося rosbag, его распаковка. При этом не имеет значение static или dynamic rosbag.

В kalibr есть пакет bagextractor:

python3 kalibr_bagextractor --image-topics /image --output-folder . --bag static.bag

С помощью команды выше bagextractor извлечет bag, сложив все снимки, которые были сделаны при калибровке в cam0 папку. Далее, просмотрев эти снимки, визуально можно будет понять, что пошло не так.

Перенос параметров в файл настроек ORB_SLAM3.

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

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

%YAML:1.0  System.SaveAtlasToFile: "my_01" #System.LoadAtlasFromFile: "my_01" #-------------------------------------------------------------------------------------------- # Camera Parameters. Adjust them! #-------------------------------------------------------------------------------------------- File.version: "1.0" Camera.type: "PinHole"  # Camera calibration and distortion parameters (OpenCV)  #Camera1.fx: 517.306408 #Camera1.fy: 516.469215 #Camera1.cx: 318.643040 #Camera1.cy: 255.313989 Camera1.fx: 155.595080 Camera1.fy: 155.00108 Camera1.cx: 148.389000 Camera1.cy: 128.182481  #Camera1.k1: 0.262383 #Camera1.k2: -0.953104 #Camera1.p1: -0.005358 #Camera1.p2: 0.002628 #Camera1.k3: 1.163314 Camera1.k1: -0.295855 Camera1.k2: 0.069506 Camera1.p1: 0.000982 Camera1.p2: 0.001053  # Camera frames per second  Camera.fps: 5  # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) Camera.RGB: 0  # Camera resolution Camera.width: 320 Camera.height: 240  #-------------------------------------------------------------------------------------------- # ORB Parameters #--------------------------------------------------------------------------------------------  # ORB Extractor: Number of features per image ORBextractor.nFeatures: 1000  # ORB Extractor: Scale factor between levels in the scale pyramid  ORBextractor.scaleFactor: 1.2  # ORB Extractor: Number of levels in the scale pyramid ORBextractor.nLevels: 1  #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: -1.8 Viewer.ViewpointF: 500.0 Viewer.imageViewScale: 0.3 

и

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

%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 Camera1.fx: 155.595080 Camera1.fy: 155.001082 Camera1.cx: 148.329000 Camera1.cy: 128.182481   # Equidistant distortion 0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182 #Camera.bFishEye: 1 #Camera1.k1: 0.003482389402 #Camera1.k2: 0.000715034845 #Camera1.k3: -0.002053236141 #Camera1.k4: 0.000202936736 Camera1.k1: -0.29585544183959606 Camera1.k2: 0.06950559854738034 Camera1.k3: 0.000981636728463331 Camera1.k4: 0.0010529550643745315      # Camera resolution Camera.width: 320 Camera.height: 240  # Camera frames per second  Camera.fps: 3  # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) Camera.RGB: 1  # Transformation from body-frame (imu) to camera IMU.T_b_c1: !!opencv-matrix    rows: 4    cols: 4    dt: f #   data: [-0.9995250378696743, 0.0075019185074052044, -0.02989013031643309, 0.045574835649698026,  #          0.029615343885863205, -0.03439736061393144, -0.998969345370175, -0.071161801837997044, #         -0.008522328211654736, -0.9993800792498829, 0.03415885127385616, -0.044681254117144367, #          0.0, 0.0, 0.0, 1.0]    data: [0.98598023, 0.05458661, 0.15768097, 0.09686663,          -0.04210964, -0.83299201,  0.55168024,  0.13264273,          0.16146134, -0.5505857, -0.81901503, -0.32186711,          0.0, 0.0, 0.0, 1.0]   # IMU noise (Use those from VINS-mono) IMU.NoiseGyro: 0.02 #0.004 # 0.00016 (TUM) # 0.00016    # rad/s^0.5  IMU.NoiseAcc: 0.05  #0.04  # 0.0028 (TUM) # 0.0028     # m/s^1.5 IMU.GyroWalk: 4e-05 #0.000022 #(VINS and TUM) rad/s^1.5 IMU.AccWalk: 0.001  #0.0004 # 0.00086 # 0.00086    # m/s^2.5 IMU.Frequency: 200.0 System.thFarPoints: 7.0  #-------------------------------------------------------------------------------------------- # 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.scaleFactor: 1.2  # ORB Extractor: Number of 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 

Несколько пояснений, что куда попадает из файла калибровки 320x240_dynamic-results-imucam.txt:

Спасибо за внимание.