目录
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_filenameimport rospy import rosbag from sensor_msgs.msg import Imu, Image from cv_bridge import CvBridge import openpyxl import timeif __name__ == "__main__":############################# rosbag config###########################save_path = os.path.join(ROOT_PATH, "imu.bag")bag = rosbag.Bag(save_path, 'w')############################# main function############################# read dataworkbook = openpyxl.load_workbook(r'D:\projectslam\off_data_zhuan_ros\raw_data\20231010_180949.xlsx')sheet = workbook.active## begin frame by frame processi = 0for row in sheet.iter_rows(values_only=True):#create new messageimu_msg = Imu()imu_msg.header.frame_id = "base_link"imu_msg.header.seq = itimestamp = time.time()formatted_timestamp = "{:.9f}".format(timestamp)secs = int(formatted_timestamp.split('.')[0])nsecs = int(formatted_timestamp.split('.')[1])imu_msg.header.stamp.secs = secsimu_msg.header.stamp.nsecs = nsecsimu_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 += 1time.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_filenameimport rospy import rosbag from sensor_msgs.msg import Imu, Image from cv_bridge import CvBridge import openpyxl import timeif __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 imuworkbook = openpyxl.load_workbook(r'D:\projectslam\off_data_zhuan_ros\qap_out_data\imu.xlsx')sheet = workbook.active## begin frame by frame processi = 0for row in sheet.iter_rows(values_only=True):# create new messageimu_msg = Imu()imu_msg.header.frame_id = "base_link"imu_msg.header.seq = itimestamp = time.time()formatted_timestamp = "{:.9f}".format(timestamp)secs = int(formatted_timestamp.split('.')[0])nsecs = int(formatted_timestamp.split('.')[1])imu_msg.header.stamp.secs = secsimu_msg.header.stamp.nsecs = nsecsimu_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)# 图像 msgimage = 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 = iimg_msg.header.stamp.secs = secsimg_msg.header.stamp.nsecs = nsecsbag.write(topic="/image/data_raw", msg=img_msg)bag.write(topic="/imu/data_raw", msg=imu_msg)i += 1time.sleep(0.033)bag.close()下面开始标定。
3 开始标定
3.1 IMU标定
对于6轴的IMU，我们修改这个文件：
/bag/catkin_imu/src/imu_utils/launch/tum.launch
修改内容如下：
修改我们IMU的录制时间与IMU话题：
<launch><node pkg="imu_utils" type="imu_an" name="imu_an" output="screen"><param name="imu_topic" type="string" value= "/imu/data_raw"/><param name="imu_name" type="string" value= "custom_imu_nrxdwcs"/><param name="data_save_path" type="string" value= "$(find imu_utils)/imu666/"/><param name="max_time_min" type="int" value= "90"/><param name="max_cluster" type="int" value= "50"/></node></launch>修改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 相机标定
我们先需要下载标定版，这里我推荐带编码信息的棋盘格标定板：
标定版下载链接
原始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 #Hzaccelerometer_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: pinholedistortion_coeffs: [-0.34038923175502456, 0.06977055299360228, 0.015293838790916657, -0.010372561499554008]distortion_model: radtanintrinsics: [1685.169877633105, 1656.9322836449144, 997.1304121813936, 474.3184148435317]resolution: [1920, 1080]rostopic: /image/data_raw3.标定版文件，就是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.31841484Camera1.k1: -0.34038923175502456 Camera1.k2: 0.06977055299360228 Camera1.p1: 0.015293838790916657 Camera1.p2: -0.010372561499554008# Camera resolution Camera.width: 1920 Camera.height: 1080Camera.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-matrixrows: 4cols: 4dt: fdata: [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 shutildef 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.xay = msg.linear_acceleration.yaz = msg.linear_acceleration.zwx = msg.angular_velocity.xwy = msg.angular_velocity.ywz = msg.angular_velocity.zcsvwriter.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 timestampcv2.imwrite(path + image_name + '.png', cv_image) # Save as PNG formatimage_names.append(image_name) # Add image name to the listwith 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";这样就可以跑啦！
![[GSEP202306 一级] C++ 时间规划](https://img-blog.csdnimg.cn/6131aae391834d87bf72fc4eb52d7e58.png)
