使用kaliber与imu_utils进行IMU、相机+IMU联合标定

目录

1 标定工具编译

1.1 IMU标定工具 imu_utils

1.2 相机标定工具 kaliber

2 标定数据录制

3 开始标定

3.1 IMU标定

3.2 相机标定

3.3 相机+IMU联合标定

4 将参数填入ORBSLAM的文件中

---

1 标定工具编译

1.1 IMU标定工具 imu_utils

        标定IMU我们使用imu_utils软件进行标定：

        首先我们安装标定软件的依赖项：Eigen、Ceres

        通过命令行安装Eigen3.3.4即可

sudo apt-get install libdw-dev
sudo apt-get install libeigen3-dev

        安装Ceres1.14.0的依赖项：

sudo apt-get install liblapack-dev libblas-dev libeigen3-dev libgflags-dev libgoogle-glog-dev
sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3 libgflags-dev libgoogle-glog-dev libgtest-dev

       安装Ceres1.14.0

wget -O ~/Downloads/ceres.zip https://github.com/ceres-solver/ceres-solver/archive/1.14.0.zip
cd ~/Downloads/ && unzip ceres.zip -d ~/Downloads/
cd ~/Downloads/ceres-solver-1.14.0
mkdir ceres-bin && cd ceres-bin
cmake ..
sudo make install -j4

        这些安装之后，我们开始安装imu_utils。

        首先为我们要先在ROS环境下编译code_utils，否则会报错：

cd ..catkin_imu/src
git clone https://github.com/gaowenliang/code_utils 
cd ..
catkin_make 

        运行这个步骤会报错，找不到backward.hpp这个头文件:

        解决方案：

        把src/code_utils/CMakeList.txt中，添加路径：include_directories(“include/code_utils”)

        如下图：

        安装imu_utils：

cd ..catkin_imu/src
git clone https://github.com/gaowenliang/imu_utils
cd ..
catkin_make #编译imu_utils

        这样就编译成功了：

1.2 相机标定工具 kaliber

        标定IMU+相机与相机的标定我们使用kaliber软件进行标定：

        先进行依赖安装：

sudo apt install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev
sudo apt install ros-noetic-vision-opencv ros-noetic-image-transport-plugins ros-noetic-cmake-modules
sudo apt install python-software-properties software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython 
sudo apt install libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev 
sudo apt install build-essential python-dev libxml2 libxml2-dev zlib1g-dev bison flex libigraph0-dev texlive-binaries
sudo pip install -i https://pypi.tuna.tsinghua.edu.cn/simple python-igraph
sudo pip install python-igraph --upgrade
sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev ros-melodic-vision-opencv ros-melodic-image-transport-plugins ros-melodic-cmake-modules python-software-properties software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev

        编译：

kaliber下载网站https://gitcode.net/mirrors/ethz-asl/kalibr        从上述网址下载Kaliber，正常编译即可。不会出什么问题。

2 标定数据录制

        IMU数据：

        IMU静置2小时，周围不要有振动，录制完成后利用下面的脚本转化成rosbag的格式。

        这里是一个可以使用的转化脚本：将文本的IMU信息转化为了sensor_msgs/Imu的信息

"""
Function: convert rawdata into rosbag
Author: Yiheng Zhao
Date: 2023.10.11
"""
import math
import os
import cv2
import numpy as np
from vp_config import ROOT_PATH
from utility import ReadQapData, fix_filename

import rospy
import rosbag
from sensor_msgs.msg import Imu, Image
from cv_bridge import CvBridge
import openpyxl
import time

if __name__ == "__main__":
    ###########################
    ## rosbag config
    ###########################
    save_path = os.path.join(ROOT_PATH, "imu.bag")
    bag = rosbag.Bag(save_path, 'w')
    
    ###########################
    ## main function
    ###########################
    ## read data
    workbook = openpyxl.load_workbook(r'D:\projectslam\off_data_zhuan_ros\raw_data\20231010_180949.xlsx')
    sheet = workbook.active
    ## begin frame by frame process
    i = 0
    for row in sheet.iter_rows(values_only=True):
        #create new message
        imu_msg = Imu()
        imu_msg.header.frame_id = "base_link"
        imu_msg.header.seq = i

        timestamp = time.time()
        formatted_timestamp = "{:.9f}".format(timestamp)
        secs = int(formatted_timestamp.split('.')[0])
        nsecs = int(formatted_timestamp.split('.')[1])
        imu_msg.header.stamp.secs = secs
        imu_msg.header.stamp.nsecs = nsecs


        imu_msg.linear_acceleration.x = float(row[9])
        imu_msg.linear_acceleration.y = float(row[10])
        imu_msg.linear_acceleration.z = float(row[11])

        print("acceleration x is %f" % imu_msg.linear_acceleration.x)
        print("acceleration y is %f" % imu_msg.linear_acceleration.y)
        print("acceleration z is %f" % imu_msg.linear_acceleration.z)

        imu_msg.angular_velocity.x = ( float(row[6])/ 180.0 * 3.1415926)
        imu_msg.angular_velocity.y = ( float(row[7])/ 180.0 * 3.1415926)
        imu_msg.angular_velocity.z = ( float(row[8])/ 180.0 * 3.1415926)

        print("angular x is %f" % imu_msg.angular_velocity.x)
        print("angular y is %f" % imu_msg.angular_velocity.y)
        print("angular z is %f" % imu_msg.angular_velocity.z)

        bag.write(topic="/imu/data_raw", msg=imu_msg)
        i += 1

        time.sleep(0.033)

    bag.close()

        我们得到了一个仅含IMU数据的bag。

        相机数据录制：

        缓慢移动相机，且相机和IMU之间不要发生相对运动，将相机左右移动、上下移动、旋转移动充分激励IMU，录制三分钟左右即可。

        我们得到一个bag，包含IMU和相机数据：

        下面这个脚本是合并IMU、相机图像数据的脚本：

"""
Function: convert rawdata into rosbag
Author: Yiheng Zhao
Date: 2023.10.11
"""
import math
import os
import cv2
import numpy as np
from vp_config import ROOT_PATH
from utility import ReadQapData, fix_filename

import rospy
import rosbag
from sensor_msgs.msg import Imu, Image
from cv_bridge import CvBridge
import openpyxl
import time

if __name__ == "__main__":
    ###########################
    ## rosbag config
    ###########################
    save_path = os.path.join(ROOT_PATH, "imu_cam.bag")
    bag = rosbag.Bag(save_path, 'w')

    ###########################
    ## main function
    ###########################
    ## read data image
    # 指定存储图片的目录路径
    image_directory = r'D:\projectslam\off_data_zhuan_ros\qap_out_data\image'
    # 初始化一个空列表来存储图片路径
    image_paths = []
    # 遍历目录下的所有文件
    for root, dirs, files in os.walk(image_directory):
        for file in files:
            # 检查文件扩展名是否为图片格式（例如，这里假设是以.jpg、.png、.jpeg为扩展名的图片）
            if file.lower().endswith(('.jpg', '.png', '.jpeg')):
                # 使用os.path.join()将目录和文件名组合成完整的文件路径
                image_path = os.path.join(root, file)
                # 将图片路径添加到列表中
                image_paths.append(image_path)
    print(image_paths)
    ## read data  imu
    workbook = openpyxl.load_workbook(r'D:\projectslam\off_data_zhuan_ros\qap_out_data\imu.xlsx')
    sheet = workbook.active
    ## begin frame by frame process
    i = 0
    for row in sheet.iter_rows(values_only=True):

        # create new message
        imu_msg = Imu()
        imu_msg.header.frame_id = "base_link"
        imu_msg.header.seq = i

        timestamp = time.time()
        formatted_timestamp = "{:.9f}".format(timestamp)
        secs = int(formatted_timestamp.split('.')[0])
        nsecs = int(formatted_timestamp.split('.')[1])
        imu_msg.header.stamp.secs = secs
        imu_msg.header.stamp.nsecs = nsecs

        imu_msg.linear_acceleration.x = float(row[9])
        imu_msg.linear_acceleration.y = float(row[10])
        imu_msg.linear_acceleration.z = float(row[11])

        print("acceleration x is %f" % imu_msg.linear_acceleration.x)
        print("acceleration y is %f" % imu_msg.linear_acceleration.y)
        print("acceleration z is %f" % imu_msg.linear_acceleration.z)

        imu_msg.angular_velocity.x = (float(row[6]) / 180.0 * 3.1415926)
        imu_msg.angular_velocity.y = (float(row[7]) / 180.0 * 3.1415926)
        imu_msg.angular_velocity.z = (float(row[8]) / 180.0 * 3.1415926)

        print("angular x is %f" % imu_msg.angular_velocity.x)
        print("angular y is %f" % imu_msg.angular_velocity.y)
        print("angular z is %f" % imu_msg.angular_velocity.z)

        # 图像 msg
        image = cv2.imread(image_paths[i])
        my_bridge = CvBridge()
        img_msg = my_bridge.cv2_to_imgmsg(cvim=image)
        img_msg.header.frame_id = "base_link"
        img_msg.header.seq = i

        img_msg.header.stamp.secs = secs
        img_msg.header.stamp.nsecs = nsecs
        bag.write(topic="/image/data_raw", msg=img_msg)
        bag.write(topic="/imu/data_raw", msg=imu_msg)
        i += 1

        time.sleep(0.033)

    bag.close()

        下面开始标定。

3 开始标定

3.1 IMU标定

        对于6轴的IMU，我们修改这个文件：

        /bag/catkin_imu/src/imu_utils/launch/tum.launch

        修改内容如下：

        修改我们IMU的录制时间与IMU话题：

        修改imu_topic为我们包的IMU录制话题：

        修改imu_name为我们IMU的名字：这里我随便起得名，和客户名字有关系.....

        修改max_time_min为我们IMU录制的时间：我这里是从09：55 - 11：30，我选择取前90分钟的数据。

        修改max_cluster为采样频率，由于我录制不够2小时，因此修改采样频率为50HZ（增大了采样频率）。

        修改data_save_path为我们标定完成的路径，即标定文件存放的位置。

        下面开始标定：

        打开标定IMU的ROS节点：

liuhongwei@liuhongwei-Legion-Y9000P-IRX8H:~/Downloads$ cd /bag/catkin_imu/
liuhongwei@liuhongwei-Legion-Y9000P-IRX8H:/bag/catkin_imu$ source devel/setup.bash 
liuhongwei@liuhongwei-Legion-Y9000P-IRX8H:/bag/catkin_imu$ roslaunch imu_utils tum.launch 

        打开节点后，我们以200倍速度播包。

 rosbag play imu.bag -r 200

        播包完毕后，我们IMU标定就完成了。

        标定文件存储在我们指定的路径中。

        第一个文件就是我们需要的IMU参数。

3.2 相机标定

        我们先需要下载标定版，这里我推荐带编码信息的棋盘格标定板：

标定版下载链接https://doc-08-5c-docs.googleusercontent.com/docs/securesc/2nlhb7mn3rh7ilhvic8i1i0lcg6lvbo5/kcic7lcag2vqbkks6cg7sa20rnhoqc5r/1696916775000/08341388560495021951/08634034057607032407/1DqKWgePodCpAKJCd_Bz-hfiEQOSnn_k0?e=download&ax=AA75yW7BQ9IbcKRqN7F30tCa7QeNZmYUtrGfL0rCKL3H-BPWurSVMZ8SlMyN7l7mcABbUuU4t6LKNh1GUv6oaKYdz8fhFhpvrys81_Tr-LK6b6VaHTYZrKdK1Xl-7jalz-zRTbOGJI0B_pxlK-zYjlJ5qptj6eJa12S-A520-9oO-QwEJa2FTA10ED_NooTkPqK2nYqfulra1G-7X7By1KB5iB1aK6goViNqPnnFNBWaSyNKb2GBEDPdMgTphe8yFZ9OSGtrzNW9zdbAdM-Ohm-JP34_llYMgTzRxwqKX9ltC34xf4bCU83vDIOfrjqZHos9XkPmWahZuxtJxZGuRDWIBKhOb1P8y6qOVpvRP-hNZB4z8uPyiQ-Qu8q5xqGH1oT6kuQONiCAm1kDI0c0wp4lBi0DMV_5HHBnOrS7x26nTrsWYFAsqdjcx0awomsAlDtSVMc4zZ8pQJDeoV7Qa19VAC-9BidANzgAca2TyLven2FHj3ogrAz-2nlHDOK6OHT3Rzjdd9I5UNRg3ZQUP5g8SEXUo3qHDM0u1n1PKoaZKoRlFaYTYyZKMTqnhOBiBuyjqNB8LRCIteoBC335dRHdjRSzwlOD79bLwQGjXw_ItlDo_6YUV1ZM8nep9kzzcLNP34d_MUMNp6rSBHyfug5jobqcdtHmcWFgJuf2b0u6H2UWHP-0WRmjbHWfdbDQKK8vEmgRlndGnk6gxL8HqL_PQYO0yJ6ddagbHBztZZCZbXSl_KUPYDVd212u-vsoc6BsgYoj200XU7vQE3AfekgV0RLJNzeL0RCIT7ghfHQIBNXFmfTq8Y4byyh5-wnlqTvHi5WgCsF6x9_2sC6FVdZtvOxmpBlufS_eT9FaWu-cNk30Kor_OnQUv8RMLO9mcJbtzw&uuid=51452ed9-1b64-4adc-88d9-65bedb46fdfc&authuser=0&nonce=5kor9vi5br1lg&user=08634034057607032407&hash=7qn0q7b6strcok04upeb271oq7qcpf6c        我们需要制作参数文档，参数文档的数学信息如下：

原始pdf的格子参数是：
6*6的格子
大格子边长：5.5cm
小格子边长：1.65cm
小格子与大格子边长比例：0.3

调整后的格子参数是：
大格子边长：2.2cm
小格子边长：0.66cm
小格子与大格子边长比例：0.3

        然后如果你是打印成了A4纸的形式，可以参考我的参数文档：A4.yaml

target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.021           #size of apriltag, edge to edge [m]
tagSpacing: 0.285714285714   #ratio of space between tags to tagSize
codeOffset: 0            #code offset for the first tag in the aprilboard

        现在我们进行针孔相机的标定：

rosrun kalibr kalibr_calibrate_cameras --target '/bag/catkin_kaliber/src/Kalibr/a4.yaml' --bag /home/liuhongwei/Desktop/imu_cam.bag --models pinhole-radtan --topics /image/data_raw --bag-from-to 10 100 --show-extraction 

        然后就开始了标定工作：

        解释一下具体的参数：

        --target：标定版的参数，就是我们刚才写的那个

        --bag：包的路径

        --models：针孔相机模型选这个

        --topics：图像信息的话题

        --bag-from-to：选取10-100s的图像进行标定，这个可以按照自己需求改，一般都是前几秒比较模糊就不要了

        --show-extraction：展示图形化界面

        标定完成后，会输出几个文件：

        这个就是我们相机的内参了。

        标定时可能会遇到这个问题，这是因为相机焦距太大了，我们需要设置个初始值：

Initialization of focal length failed. You can enable manual input by setting ‘KALIBR_MANUAL_FOCAL_LENGTH_INIT’.

        遇到这种情况，我们先终端中设置变量 KALIBR_MANUAL_FOCAL_LENGTH_INIT = 1 然后程序运行时手动给相机设置初始焦距。

3.3 相机+IMU联合标定

        这个我们事先制作几个文件：

        1.imu的配置信息，我们取名为imu.yaml，这个就是我们把我们之前标定的IMU信息写入这个文件就行：

rostopic: /imu/data_raw
update_rate: 30.0 #Hz

accelerometer_noise_density: 1.7640241083260223e-03
accelerometer_random_walk: 4.6133140085614272e-05
gyroscope_noise_density: 1.2287169549703986e-05
gyroscope_random_walk: 8.1951127134973680e-07

        图像的话题还有IMU的频率不要忘记修改。

        2.相机的内参标定信息：

        这个是3.2节中生成的文件imu_cam-camchain.yaml：

cam0:
  cam_overlaps: []
  camera_model: pinhole
  distortion_coeffs: [-0.34038923175502456, 0.06977055299360228, 0.015293838790916657, -0.010372561499554008]
  distortion_model: radtan
  intrinsics: [1685.169877633105, 1656.9322836449144, 997.1304121813936, 474.3184148435317]
  resolution: [1920, 1080]
  rostopic: /image/data_raw

        3.标定版文件，就是3.2中我们自己写的

target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.021           #size of apriltag, edge to edge [m]
tagSpacing: 0.285714285714   #ratio of space between tags to tagSize
codeOffset: 0            #code offset for the first tag in the aprilboard

        执行下面代码进行标定：

rosrun kalibr kalibr_calibrate_imu_camera --bag '/home/liuhongwei/Desktop/imu_cam.bag' --target '/bag/catkin_kaliber/src/Kalibr/a4.yaml'  --cam '/bag/catkin_kaliber/src/Kalibr/imu_cam-camchain.yaml'  --imu '/bag/catkin_kaliber/src/Kalibr/imu.yaml' --show-extraction

        参数列表含义如下：

        --bag：数据包路径

        --target：标定版文件路径（A4.yaml）

        --cam：相机内参文件路径（mu_cam-camchain.yaml）

        --imu：IMU标定文件路径（imu.yaml）

        --show-extraction：显示标定过程

        执行如下：

        标定结束：

        结束后生成标定文件imu_cam-results-imucam.txt：

        标定完毕。

4 将参数填入ORBSLAM的文件中

        根据上述我们的标定结果，我们的yaml文件为：

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
File.version: "1.0"

Camera.type: "PinHole"

# Camera calibration and distortion parameters (OpenCV) 
Camera1.fx: 1685.16987763
Camera1.fy: 1656.93228364
Camera1.cx: 997.13041218
Camera1.cy: 474.31841484

Camera1.k1: -0.34038923175502456
Camera1.k2: 0.06977055299360228
Camera1.p1: 0.015293838790916657
Camera1.p2: -0.010372561499554008

# Camera resolution
Camera.width: 1920
Camera.height: 1080

Camera.newWidth: 600
Camera.newHeight: 350

# Camera frames per second 
Camera.fps: 30

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

# Transformation from camera to body-frame (imu)
IMU.T_b_c1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: f
   data: [0.94880513, 0.12309341, 0.27236458, 0.00027046,
         0.12309341, 0.98136615, 0.14754149, -0.00012572,
        -0.29088973, -0.10646184, 0.95081494, 0.00034056,
         0.0, 0.0, 0.0, 1.0]

# IMU noise
IMU.NoiseGyro: 1.2287169549703986e-05 #1.6968e-04
IMU.NoiseAcc: 1.7640241083260223e-03 #2.0e-3
IMU.GyroWalk: 8.1951127134973680e-07
IMU.AccWalk: 4.6133140085614272e-05 # 3e-03
IMU.Frequency: 30.0

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000 # 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: 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 # -1.8
Viewer.ViewpointF: 500.0

5 Euroc单目+IMU数据集制作及跑通

        用这个脚本进行拆包：

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

import rosbag
import csv
from sensor_msgs.msg import Imu
import os
import roslib
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError
import shutil

def CreateDIR():
    folder_name = 'bag_tum'
    subfolders = ['left', 'right' , 'rgb' , 'depth']

    if not os.path.exists(folder_name):
        os.makedirs(folder_name)

    # 在主文件夹下创建子文件夹
    for subfolder in subfolders:
        subfolder_path = os.path.join(folder_name, subfolder)
        if not os.path.exists(subfolder_path):
            os.makedirs(subfolder_path)


def CreateIMUCSV(umpackbag):
    csvfile = open('imudata.csv', 'w')
    csvwriter = csv.writer(csvfile)
    csvwriter.writerow(['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 [rad m s^-2]', 'a_RS_S_y [rad m s^-2]', 'a_RS_S_z [rad m s^-2]'])
    for topic, msg, t in umpackbag.read_messages(topics=['/imu/data_raw']):
        timestamp = msg.header.stamp.to_nsec()
        ax = msg.linear_acceleration.x
        ay = msg.linear_acceleration.y
        az = msg.linear_acceleration.z
        wx = msg.angular_velocity.x
        wy = msg.angular_velocity.y
        wz = msg.angular_velocity.z
        csvwriter.writerow([timestamp, wx, wy, wz, ax, ay, az])
    #umpackbag.close()
    csvfile.close()

def TransIMUdatatotxt():
    csv_file = './imudata.csv'
    txt_file = './imudata.txt'
    with open(csv_file, 'r') as file:
        reader = csv.reader(file)
        with open(txt_file, 'w') as output_file:
            writer = csv.writer(output_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
            for i, row in enumerate(reader):
                if i == 0:
                    writer.writerow(['#' + cell for cell in row])  # 添加#号
                else:
                    writer.writerow(row)


# Save RGBD image and Save its timestamp
def Savergb(umpackbag):
    path = './bag_tum/rgb/'
    bridge = CvBridge()
    image_names = []
    txt_file = './rgbtimestamp.txt'
    with rosbag.Bag(bagname, 'r') as bag:
        for topic, msg, t in umpackbag.read_messages():
            if topic == "/image/data_raw":
                try:
                    cv_image = bridge.imgmsg_to_cv2(msg)
                except CvBridgeError as e:
                    print(e)
                    continue

                #timestr = "%.9f" % msg.header.stamp.to_sec()
                timestr = "%.6f" % msg.header.stamp.to_sec()
		#timestr = "%.1f" % msg.header.stamp.to_sec()
                image_name = timestr
                #image_name = timestr.replace('.', '')  # Remove periods from the timestamp
                cv2.imwrite(path + image_name + '.png', cv_image)  # Save as PNG format
                image_names.append(image_name)  # Add image name to the list
    with open(txt_file, 'w') as f:
        #f.write('\n'.join(["{} rgb/{}.png".format(t, t) for t in image_names]))


        f.write('\n'.join(image_names))






# Script Menu
# Make a folder name bag_tum include three sunfolder : left right rgb , in folder their image in it
# in python main.py folder , create imudata.scv and imudata.txt ,aim for KITTI or TUM dataset
# in python main.py folder , create timestamp.txt for image timestamp
# in python main.py folder , create timestamp.txt for image timestamp
if __name__ == '__main__':
    bagname = 'imu_cam.bag'
    umpackbag = rosbag.Bag(bagname)
    CreateDIR()
    CreateIMUCSV(umpackbag)
    TransIMUdatatotxt()
    Savergb(umpackbag)

        执行脚本后，得到如下文件 + timestamp.txt文件夹：

        我们开始制作数据集：建立一个01文件夹

        将timestamp.txt文件夹放在这里，再创建一个mav0的文件夹。

        在mav0文件夹里面创建cam0和imu0文件夹：

        cam0里面创建data文件夹，存放图像数据，这里的图像就是bag_tum/rgb目录下的图像：

        imu0里面存放的是data.csv和data.txt存放IMU数据。

        至此，我们数据集制作完毕，向程序输入参数：

        ORB词典位置、标定参数文件位置、01文件夹位置以及时间戳的位置。

        此外，还需要改一个地方：

mono_inertial_euroc.cc文件的86行改为：

        string pathImu = pathSeq + "/mav0/imu0/data.txt";

        这样就可以跑啦！