目录
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 相机标定
我们先需要下载标定版,这里我推荐带编码信息的棋盘格标定板:
标定版下载链接
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 #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_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.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";
这样就可以跑啦!