В данной публикации хотелось бы познакомить с пакетом калибровки камеры Kalibr, в том числе для целей его использования в пакете визуальной навигации ORB_SLAM3. Будет продемонстрирован процесс калибровки камеры fish-eye для raspberry pi. Камера будет калиброваться совместно с гироскопом/акселерометром imu-650 (GY-521). Предполагается, что данная пошаговая инструкция облегчит понимание процесса калибровки в случае возникновения необходимости в таковой.

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

Данная статья будет перекликаться с предыдущей публикаций, в которой был рассмотрен процесс установки и использования 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, также придется выполнить определённые шаги, а именно:

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

test_imu.py


'''
        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 файл

camerav2_320x240.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 выдаст файл

320x240_static-camchain.yaml

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.

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

imu0.yaml

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

Как откалибровать imu и получить свой собственный imu0.yaml

Способы получения параметров для imu два:
— datasheet imu
— калибровка.
Оба эти способа описаны на странице wiki kalibra.
Для калибровка imu потребуется репозиторий и 3 часа свободного времени.
Данный репозиторий, согласно readme устанавливается в catkin_ws/src.
Далее необходимо записать rosbag показаний только imu топика на протяжении 3 часов, в течение которых imu просто должен находиться неподвижно:

rosbag record /imu/data --duration=10800 -O imu.bag

Итоговый файл rosbag займет примерно 450Мб.
После очистки rosbag командой:

rosrun allan_variance_ros cookbag.py --input original_rosbag.bag --output cooked_rosbag.bag

файл уменьшится в несколько раз.
Далее потребуется запустить файл произведения расчетов:

rosrun allan_variance_ros allan_variance [path_to_folder_containing_bag] [path_to_config_file]

В качестве config_file необходимо указать yaml файл, взяв за основу один из yaml файлов из директории config репозитория:

imu_topic: "/imu/data"
imu_rate: 200
measure_rate: 200 # Rate to which imu data is subsampled
sequence_time: 10800 # 3 hours in seconds

Команда может выглядеть так:

rosrun allan_variance_ros allan_variance . ~/kalibr_workspace/src/allan_variance_ros/config/my.yaml

На выходе будет csv файл, который потребуется для формирования итоговых графиков и yaml файла для самого kalibra:

rosrun allan_variance_ros analysis.py --data allan_variance.csv

Файлы, которые получились при калибровке imu-650 (gy-521) можно скачать — здесь.

По завершении калибровки для 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

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

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:

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

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