kalibr进行相机内参以及相机imu的融合标定

1、采集标定数据

参考该视频教程：

https://www.youtube.com/watch?app=desktop\&v=puNXsnrYWTY

2、转rosbag包

首先安装好Docker

在linux下运行如下命令，获取 VideoIMUCapture-Android 的github库

git clone https://github.com/DavidGillsjo/VideoIMUCapture-Android.git

进入对应文件夹

cd VideoIMUCapture-Android/calibration

需要以root权限进入容器，那么需要输入类似命令，换为自己的路径

SUDO=1 DUSER=root DATA=/home/ljy18/VideoIMUCapture-Android/dataset ./run_dockerhub.sh

容器是退出后删除，文件夹挂在到容器内后对应的目录为/data

需要转为euroc格式的数据集bag包，原始文件夹格式需要为

dataset_name/

├── mav0/

│ ├── cam0/

│ │ ├── data/

│ │ │ ├── 1403....png

│ │ │ └── ...

│ │ ├── data.csv

│ ├── imu0/

│ │ ├── data.csv

其中：

mav0/cam0/data.csv的格式为

#timestamp [ns],filename

1403636579763555584,1403636579763555584.png

1403636579863555584,1403636579863555584.png

...

mav0/imu0/data.csv的格式为

#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1],

a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2]

1403636579763555584,-0.0015,0.0023,0.0009,9.8085,-0.0321,0.0230

...

Imu频率要远大于图片帧频率

用当前目录下的kalibr_bagcreater.py（见附录）去替换掉原本针对旧数据集格式的kalibr_bagcreater：

cp /data/kalibr_bagcreater.py /kalibr_workspace/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_bagcreater

转为bag包

kalibr_bagcreater \

  --folder /data/VideoTV00001_EuRoC \

  --output-bag /data/VideoTV00001_calibration/VideoTV00001_euroc.bag

查看话题名称是否正确

rosbag info /data/VideoTV00001_calibration/VideoTV00001_euroc.bag

出现类似如下内容即格式正确

types:       sensor_msgs/Image [060021388200f6f0f447d0fcd9c64743]

             sensor_msgs/Imu   [6a62c6daae103f4ff57a132d6f95cec2]

topics:      /cam0/image_raw    2154 msgs    : sensor_msgs/Image

             /imu0             13542 msgs    : sensor_msgs/Imu

3、相机内参标定

进入标定文件夹

cd /data/VideoTV00001_calibration

在该目录下新建一个target.yaml文件，这个是所用标定板的配置文件，参考标定板如图：

target.yaml文件内容为

tagCols: 6

tagRows: 6

tagSize: 0.021

tagSpacing: 0.3

target_type: aprilgrid

其中tagSize是打印后每个格子的实际尺寸，单位是米。需要根据实际调整。

进行相机内参标定

kalibr_calibrate_cameras --bag VideoTV00001_euroc.bag --target target.yaml --models pinhole-radtan --topics /cam0/image_raw

标定完成后会弹出可视化结果并输出标定结果文件至该目录下

4、相机imu外参标定

需要再新建imu.yaml和camchain.yaml文件

imu.yaml是imu内参的配置文件，可在产品手册中获取或者获取一小时以上imu的静置数据进行imu内参标定。Imu.yaml的格式如下：

accelerometer_noise_density: 0.00061667

accelerometer_random_walk: 0.0000043333

gyroscope_noise_density: 0.000098902

gyroscope_random_walk: 0.0000012928

rostopic: /imu0

update_rate: 188.58097757972

单位为

|-------------------------|--------|---------------|
| gyroscope_noise_density | 角速度白噪声 | rad/s/√Hz |

|-----------------------------|--------|--------------|
| accelerometer_noise_density | 加速度白噪声 | m/s²/√Hz |

|-----------------------|-----------|----------------|
| gyroscope_random_walk | 陀螺仪偏置随机游走 | rad/s²/√Hz |

|---------------------------|------------|--------------|
| accelerometer_random_walk | 加速度计偏置随机游走 | m/s³/√Hz |

update_rate根据实际频率填写

camchain.yaml是相机内参的配置文件，根据刚才相机内参的标定结果填写，camchain.yaml的内容格式如下：

cam0:

  cam_overlaps: []

  camera_model: pinhole

  distortion_coeffs: [0.07936144147573729, -0.09439727502429124, -0.001944215543450636,

    -0.0013778851088523114]

  distortion_model: radtan

  intrinsics: [643.3140528909328, 642.1840798849714, 986.8635102770212, 495.85841930165645]

  resolution: [1920, 1080]

  rostopic: /cam0/image_raw

进行外参标定

kalibr_calibrate_imu_camera --target target.yaml --imu imu.yaml --cams camchain.yaml --bag VideoTV00001_euroc.bag

标定完成后会弹出可视化结果并输出标定结果文件至该目录下

**附录：**新的kalibr_bagcreater.py内容如下，可以放在/data/目录下：

#!/usr/bin/env python

# -*- coding: utf-8 -*-

# Compatible with EuRoC dataset format

# Structure:

# dataset_name/

# ├── mav0/

# │   ├── cam0/

# │   │   ├── data/

# │   │   │   ├── 1403....png

# │   │   │   └── ...

# │   │   ├── data.csv

# │   ├── imu0/

# │   │   ├── data.csv

#

# Output topics:

#   /cam0/image_raw

#   /imu0



print("importing libraries...")



import rosbag

import rospy

from sensor_msgs.msg import Image

from sensor_msgs.msg import Imu

import ImageFile

import time, sys, os

import argparse

import cv2

import numpy as np

import csv



parser = argparse.ArgumentParser(description='Create a ROS bag using EuRoC dataset (images + IMU).')

parser.add_argument('--folder', metavar='folder', required=True, help='Root folder of dataset (contains mav0)')

parser.add_argument('--output-bag', metavar='output_bag', default="output.bag", help='ROS bag output path')

parsed = parser.parse_args()



# === Helper functions ===



def read_image_list(cam_dir):

    """Read image timestamps and filenames from camX/data.csv"""

    csv_path = os.path.join(cam_dir, "data.csv")

    data_dir = os.path.join(cam_dir, "data")

    images = []

    timestamps = []

    if not os.path.exists(csv_path):

        print(" Image CSV not found:", csv_path)

        return []

    with open(csv_path, 'r') as f:

        reader = csv.reader(f)

        header = next(reader, None)

        for row in reader:

            if len(row) < 2:

                continue

            ts, filename = row[0], row[1]

            full_path = os.path.join(data_dir, filename)

            if os.path.exists(full_path):

                images.append((ts, full_path))

            else:

                print(" Missing image file:", full_path)

    # sort by timestamp

    images.sort(key=lambda x: int(x[0]))

    return images





def read_imu_data(imu_dir):

    """Read IMU measurements from imu0/data.csv"""

    imu_csv = os.path.join(imu_dir, "data.csv")

    if not os.path.exists(imu_csv):

        print(" IMU CSV not found:", imu_csv)

        return []

    imu_data = []

    with open(imu_csv, 'r') as f:

        reader = csv.reader(f)

        header = next(reader, None)

        for row in reader:

            if len(row) < 7:

                continue

            timestamp = row[0]

            gyro = [float(x) for x in row[1:4]]

            accel = [float(x) for x in row[4:7]]

            imu_data.append((timestamp, gyro, accel))

    return imu_data





def load_image_to_rosmsg(filename, timestamp_str):

    """Convert image to ROS Image message with timestamp"""

    image_np = cv2.imread(filename, cv2.IMREAD_GRAYSCALE)

    if image_np is None:

        print(" Failed to load:", filename)

        return None, None



    secs = int(timestamp_str[0:-9])

    nsecs = int(timestamp_str[-9:])

    timestamp = rospy.Time(secs, nsecs)



    rosimg = Image()

    rosimg.header.stamp = timestamp

    rosimg.height = image_np.shape[0]

    rosimg.width = image_np.shape[1]

    rosimg.encoding = "mono8"

    rosimg.step = rosimg.width

    rosimg.data = image_np.tostring()



    return rosimg, timestamp





def create_imu_msg(timestamp_str, gyro, accel):

    """Convert IMU sample to ROS Imu message"""

    secs = int(timestamp_str[0:-9])

    nsecs = int(timestamp_str[-9:])

    timestamp = rospy.Time(secs, nsecs)



    msg = Imu()

    msg.header.stamp = timestamp

    msg.angular_velocity.x = gyro[0]

    msg.angular_velocity.y = gyro[1]

    msg.angular_velocity.z = gyro[2]

    msg.linear_acceleration.x = accel[0]

    msg.linear_acceleration.y = accel[1]

    msg.linear_acceleration.z = accel[2]

    return msg, timestamp





# === Main logic ===



try:

    bag = rosbag.Bag(parsed.output_bag, 'w')

    print(" Creating bag:", parsed.output_bag)



    # Camera

    cam0_dir = os.path.join(parsed.folder, "mav0", "cam0")

    image_list = read_image_list(cam0_dir)

    print(" Found {} images".format(len(image_list)))

    for ts, filename in image_list:

        rosimg, t = load_image_to_rosmsg(filename, ts)

        if rosimg is not None:

            bag.write("/cam0/image_raw", rosimg, t)



    # IMU

    imu0_dir = os.path.join(parsed.folder, "mav0", "imu0")

    imu_list = read_imu_data(imu0_dir)

    print(" Found {} imu samples".format(len(imu_list)))

    for ts, gyro, accel in imu_list:

        imumsg, t = create_imu_msg(ts, gyro, accel)

        bag.write("/imu0", imumsg, t)



    print(" Finished writing bag file.")



finally:

    bag.close()

    print(" Bag closed successfully.")