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

这篇具有很好参考价值的文章主要介绍了使用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”)

        如下图:

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

        安装imu_utils:

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

        这样就编译成功了:

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

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,正常编译即可。不会出什么问题。

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

2 标定数据录制

        IMU数据:

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

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

        这里是一个可以使用的转化脚本:将文本的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和相机数据:

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

        下面这个脚本是合并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话题

<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录制话题:

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

        修改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 

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

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

 rosbag play imu.bag -r 200

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

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

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

        标定文件存储在我们指定的路径中。使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

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

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

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        我们需要制作参数文档,参数文档的数学信息如下:

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

原始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 

        然后就开始了标定工作:

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab        解释一下具体的参数:

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

        --bag:包的路径

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

        --topics:图像信息的话题

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

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

        标定完成后,会输出几个文件:

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

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

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

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

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

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

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:显示标定过程

        执行如下:

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

        标定结束:

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

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

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

        标定完毕。

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文件夹:

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

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

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

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

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

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

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

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

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

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

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

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab

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

        此外,还需要改一个地方:

mono_inertial_euroc.cc文件的86行改为:

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

        这样就可以跑啦!

使用kaliber与imu_utils进行IMU、相机+IMU联合标定,数码相机,自动驾驶,matlab文章来源地址https://www.toymoban.com/news/detail-726553.html

到了这里,关于使用kaliber与imu_utils进行IMU、相机+IMU联合标定的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 使用Kalibr标定相机和IMU(ZED+px4)外参

    提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档 本文仅使用kalibr中cam-imu的联合标定,关于camera内参默认已经知道了。主要会总结一些标定上的小经验,有不对的地方,希望网友可以指出。文章最后将使用标定的cam-imu外参在VINS-Fusion中进行验证 Kalibr是

    2023年04月19日
    浏览(65)
  • Camera-IMU联合标定原理

    ​ 在VIO系统中,camera-imu间内外参精确与否对整个定位精度起着重要的作用。所以良好的标定结果是定位系统的前提工作。 目前标定算法主要分为离线和在线标定,离线标定以kalibr为代表,能够标定camera内参、camera-imu之间位移旋转、时间延时以及imu自身的刻度系数、非正交性

    2024年02月08日
    浏览(32)
  • 学习Kalibr工具---Camera与IMU联合标定过程

    以双目+IMU为例进行介绍相机和IMU 的联合标定,也叫多传感器融合; 录制含有标定板图像的bag包,注意需要不断移动标定板。 运行Kalibr工具箱中 多相机标定 节点程序。 我们使用的最终命令 参考链接:https://github.com/ethz-asl/kalibr/wiki/camera-imu-calibration (一) 标定的输入文件包括

    2024年02月08日
    浏览(35)
  • 联合标定Android手机的IMU和Camera数据

    通过局域网实现安卓手机和ROS的通讯,进一步通过Kalibr工具实现手机IMU和相机的联合标定。 基于ROS下的信息发布和订阅,手机和PC在一个局域网下进行信息(image和IMU)传输。操作步骤: 在安卓手机中安装github上的2个开源Android_Camera-IMU和android_ros_sensors中的任意一个,基于r

    2024年02月09日
    浏览(36)
  • 相机与IMU标定教程

    标定教程 way 1、 imu_utils标定IMU的内参,可以校准IMU的噪声密度和随机游走噪声 2、kalibr包标定相机的内外参数,相机与IMU之间的外参 1.1安装环境 这里使用的包是 imu_utils ,使用这个包可以校准IMU的噪声密度和随机游走噪声 step1: 安装ceres库 下载编译 ceres-solver step2: 安装 cod

    2023年04月18日
    浏览(26)
  • d435i 相机和imu标定

    使用 imu_utils 功能包标定 IMU,由于imu_utils功能包的编译依赖于code_utils,需要先编译code_utils,主要参考 相机与IMU联合标定_熊猫飞天的博客-CSDN博客 Ubuntu20.04编译并运行imu_utils,并且标定IMU_学无止境的小龟的博客-CSDN博客 1.1 编译 code_utils 创建工作空间 1.1.1 修改 CMakeLists.txt 文件

    2024年02月09日
    浏览(44)
  • 使用lidar_align进行激光雷达与IMU的外参标定(超详细教程)

    1、下载lidar_align源码 ethz-asl/lidar_align: A simple method for finding the extrinsic calibration between a 3D lidar and a 6-dof pose sensor (github.com) https://github.com/ethz-asl/lidar_align 2、解压到ros工作空间目录下的src文件中 不知道如何创建ros工作空间的可以参考我另一篇博客: ubuntu下如何创建ros工作空间

    2024年02月08日
    浏览(37)
  • 视觉SLAM十四讲|【五】相机与IMU时间戳同步

    Z [ u v 1 ] = [ f x 0 c x 0 f y c y 0 0 1 ] [ X Y Z ] = K P Z begin{bmatrix} u \\\\ v \\\\ 1 end{bmatrix}= begin{bmatrix} f_x 0 c_x \\\\ 0 f_y c_y \\\\ 0 0 1 end{bmatrix} begin{bmatrix} X \\\\ Y \\\\ Z end{bmatrix}= KP Z ​ u v 1 ​ ​ = ​ f x ​ 0 0 ​ 0 f y ​ 0 ​ c x ​ c y ​ 1 ​ ​ ​ X Y Z ​ ​ = K P 其中, K = [ f x 0 c x 0 f y c y 0 0

    2024年01月20日
    浏览(36)
  • 【深蓝学院】手写VIO第8章--相机与IMU时间戳同步--笔记

    时间戳同步的原因 :如果不同步,由于IMU频率高,可能由于时间戳不同步而导致在两帧camera之间的时间内用多了或者用少了IMU的数据,且时间不同步会导致我们首尾camera和IMU数据时间不同,会使估计存在误差,使我们的系统精度下降甚至出现错误的预测。如果以IMU时间为准确

    2024年02月05日
    浏览(26)
  • 最新综述!基于相机、LiDAR和IMU的多传感器融合SLAM

    作者:萧炎 | 来源:3DCV 在公众号「 3DCV 」后台,回复「原论文」可获取论文pdf和代码链接 近年来,同步定位与建图(SLAM)技术在自动驾驶、智能机器人、增强现实(AR)、虚拟现实(VR)等领域得到广泛应用。使用最流行的三种类型传感器(例如视觉传感器、LiDAR传感器和

    2024年03月26日
    浏览(47)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包