imu标定
imu标定采用imu_utils.
1、安装依赖项
sudo apt-get install libdw-dev
2 .下载imu_utils和code_utils并放入工作空间进行编译
imu_utils下载地址为:https://github.com/gaowenliang/imu_utils
code_utils下载地址为: GitHub - gaowenliang/code_utils: my code utils
需要注意的是:
1 )在此之前需要安装Ceres库
2) 先创建一个工作空间(这里为imu_ws),将code_utils放到src文件夹下编译,通过后再将imu_utils放到src文件夹下编译,不能一起编译,因为后者依赖于前者
3.编译报错
code_utils-master/src/sumpixel_test.cpp:2:24: fatal error: backward.hpp:No such file or directory
在code_utils下面找到sumpixel_test.cpp,修改#include "backward.hpp"为
#include “code_utils/backward.hpp”
再编译,在报错:
error: ‘CV_LOAD_IMAGE_GRAYSCALE’ was not declared in this scope
将对应3.x
版本的旧标志替换为4.x
版本的新标志即可,参考如下:
CV_LOAD_IMAGE_UNCHANGED = -1 ( = cv::IMREAD_UNCHANGED),
CV_LOAD_IMAGE_GRAYSCALE = 0 ( = cv::IMREAD_GRAYSCALE),
CV_LOAD_IMAGE_COLOR = 1 ( = cv::IMREAD_COLOR),
CV_LOAD_IMAGE_ANYDEPTH = 2 ( = cv::IMREAD_ANYDEPTH),
CV_LOAD_IMAGE_ANYCOLOR = 4
再将imu_utils放进src编译
4. 录制imu.bag
保持IMU静止不动至少两个小时,录制IMU的bag
rosbag record /imu -O imu
5. 根据需求修改launch文件
d435i launch文件
<arg name="unite_imu_method" default=""/>
// ########### 改为#############
<arg name="unite_imu_method" default="linear_interpolation"/>
<arg name="enable_gyro" default="false"/> “false” 改为”true”
<arg name="enable_accel" default="false"/> “false” 改为”true”
根据自己的需求对src/imu_utils-master/launch文件进行修改:主要包括名字、时长之类的; 比如:
在~/imu_catkin_ws/src/imu_utils/launch路径创建d435i_imu_calibration.launch
<launch>
<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
<!--TOPIC名称和上面一致-->
<param name="imu_topic" type="string" value= "/camera/imu"/>
<!--imu_name 无所谓-->
<param name="imu_name" type="string" value= "d435i"/>
<!--标定结果存放路径-->
<param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
<!--数据录制时间-min 120分钟 可以自行修改 一般要大于60-->
<param name="max_time_min" type="int" value= "120"/>
<!--采样频率,即是IMU频率,采样频率可以使用rostopic hz /camera/imu查看,设置为400,为后面的rosbag play播放频率-->
<param name="max_cluster" type="int" value= "200"/>
</node>
</launch>
max_time_min:采集IMU数据的时间,越长越好,单位分钟;
max_cluster:Allan方差的cluster,imu_utils中的launch文件中都是100,我一般也设置100
6. 运行标定程序运行bag文件
roscore
rosbag play -r 200 imu.bag
运行launch文件
cd imu_ws
source ./devel/setup.bash
roslaunch imu_utils xx_imu.launch
其中 -r 200是指200速播放bag数据
7. 标定结果
标定完成后,可在data文件中找到对应的yaml文件src/imu_utils/data/xx_imu_param.yaml
%YAML:1.0
---
type: IMU
name: d435i
Gyr:
unit: " rad/s"
avg-axis:
gyr_n: 2.3713647521442301e-03
gyr_w: 1.6634786328395575e-05
x-axis:
gyr_n: 2.5527723048677621e-03
gyr_w: 1.8248792841502254e-05
y-axis:
gyr_n: 3.5989014238402488e-03
gyr_w: 2.4626070373926136e-05
z-axis:
gyr_n: 9.6242052772467902e-04
gyr_w: 7.0294957697583380e-06
Acc:
unit: " m/s^2"
avg-axis:
acc_n: 1.2272815309641657e-02
acc_w: 2.2269630970713836e-04
x-axis:
acc_n: 1.0855035063883016e-02
acc_w: 1.9977097068680263e-04
y-axis:
acc_n: 1.2175166782188903e-02
acc_w: 1.8151134885911570e-04
z-axis:
acc_n: 1.3788244082853051e-02
acc_w: 2.8680660957549681e-04
Kalibr标定
安装kalibr 依赖
sudo apt-get install python3-setuptools
sudo apt-get install python3-setuptools python3-rosinstall python3-ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev
sudo apt-get install libopencv-dev ros-noetic-vision-opencv ros-noetic-image-transport-plugins ros-noetic-cmake-modules python3-software-properties software-properties-common libpoco-dev python3-matplotlib python3-scipy python3-git python3-pip libtbb-dev libblas-dev liblapack-dev python3-catkin-tools libv4l-dev
sudo pip install python-igraph --upgrade
如果不成功,则可以直接安装:
sudo apt-get install python-igraph
sudo apt-get install python3-pyx
sudo apt-get install python3-wxgtk4.0
sudo apt-get install python3-igraph
sudo apt-get install python3-scipy
mkdir -p ~/kalibr_ws/src
cd ~/kalibr_ws
source /opt/ros/noetic/setup.bash
catkin init
catkin config --extend /opt/ros/noetic
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
cd ~/kalibr_ws/src
git clone https://github.com/ethz-asl/Kalibr.git
cd ~/kalibr_ws
catkin build -DCMAKE_BUILD_TYPE=Release -j32
重新打开一个终端:
echo "source ~/kalibr_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc
双目标定
创建april_6x6_A4.yaml文件
target_type: 'aprilgrid' #gridtype
tagCols: 6 #number of apriltags
tagRows: 6 #number of apriltags
tagSize: 0.088 #size of apriltag, edge to edge [m]
tagSpacing: 0.3 #ratio of space between tags to tagSize
#example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]
关闭结构光
rosrun rqt_reconfigure rqt_reconfigure
也可在launch文件直接修改
<!-- rosparam set /camera/stereo_module/emitter_enabled false -->
<rosparam>
/camera/stereo_module/emitter_enabled: 0
</rosparam>
<rosparam if="$(arg emitter_enable)">
/camera/stereo_module/emitter_enabled: 1
</rosparam>
修改相机的帧数(官方推荐是4Hz)
rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_left
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_right
录制ROS数据包
rosbag record -O multicameras_calibration /infra_left /infra_right /color
使用Kalibr标定
注意:标定前先关掉相机,否者会报错
source devel/setup.bash
//标定单目
rosrun kalibr kalibr_calibrate_cameras --target /home/th/yaml/april_6x6_80x80cm.yaml --bag /home/th/camera_cal.bag --models pinhole-radtan --topics /color --show-extraction
//kalibr_calibrate_cameras --target /位置/文件名.yaml --bag /位置/camd435i.bag --bag-from-to 26 100 --models pinhole-radtan --topics /color --show-extraction
kalibr_calibrate_cameras --target april_6x6_A4.yaml --bag multicameras_calibration.bag --models pinhole-equi pinhole-equi pinhole-equi --topics /infra_left /infra_right /color --bag-from-to 10 100 --show-extraction
imu+双目标定
创建:camchain.yaml:
cam0:
camera_model: pinhole
intrinsics: [394.73897935327875, 397.07609983064, 328.08812327934135, 229.9742739261273]
distortion_model: equidistant
distortion_coeffs: [0.42241273556155506, 0.20864813180833605, 0.3979238261062836, 0.5898003650060837]
rostopic: /infra_left
resolution: [640, 480]
cam1:
T_cn_cnm1:
- [0.9994978959284028, -0.0004960676303391997, 0.031681381781581835, -0.049405645049756246]
- [0.0006353578883581325, 0.9999901766268545, -0.00438668099301463, 1.6793675995192084e-05]
- [-0.03167889447310175, 0.004404607438456279, 0.9994883926681007, 0.0014256336467758425]
- [0.0, 0.0, 0.0, 1.0]
camera_model: pinhole
intrinsics: [395.31081333647796, 396.67650876842976, 315.71216250025896, 232.01383312375893]
distortion_model: equidistant
distortion_coeffs: [0.5127606598499351, -0.5373699037573214, 3.847162303528836, -5.204634833610096]
rostopic: /infra_right
resolution: [640, 480]
创建: imu.yaml
#Accelerometers
accelerometer_noise_density: 1.2272815309641657e-02 #Noise density (continuous-time)
accelerometer_random_walk: 2.2269630970713836e-04 #Bias random walk
#Gyroscopes
gyroscope_noise_density: 2.3713647521442301e-03 #Noise density (continuous-time)
gyroscope_random_walk: 1.6634786328395575e-05 #Bias random walk
rostopic: /imu #the IMU ROS topic
update_rate: 200.0 #Hz (for discretization of the values above)
修改:rs_imu_stereo.launch
复制realsense-ros包中rs_camera.launch,重命名为rs_imu_stereo.launch,更改内容为
<arg name="enable_sync" default="false"/>
//改为:
<arg name="enable_sync" default="true"/>
这样来使imu和双目数据时间对齐
<arg name="unite_imu_method" default=""/>
//改为
<arg name="unite_imu_method" default="linear_interpolation"/>
录制 相机 和 imu 的联合数据
调整 相机 和 imu 的 topic 的发布频率以及以新的topic名发布它们,其中双目图像的发布频率改为20Hz,imu发布频率改为200Hz
rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_left
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_right
rosrun topic_tools throttle messages /camera/imu 200.0 /imu
开始录制
rosbag record /infra_left /infra_right /imu -O imu_stereo.bag
运行
rosrun kalibr kalibr_calibrate_imu_camera --bag imu_stereo.bag --cam camchain.yaml --imu imu.yaml --target april_6x6_A4.yaml --bag-from-to 10 50 --show-extraction
其中:
--target ../Aprilgrid/april_6x6_50x50cm_A4.yaml是标定板的配置文件,如果选择棋格盘,注意targetCols和targetRows表示的是内侧角点的数量,不是格子数量。
--bag ../multicameras_calibration_2020-10-29-20-19-06.bag是录制的数据包;
--models pinhole-equi pinhole-equi pinhole-equi表示三个摄像头的相机模型和畸变模型(解释参考https://github.com/ethz-asl/kalibr/wiki/supported-models,根据需要选取);
--topics /infra_left /infra_right /color表示三个摄像头对应的拍摄的数据话题;
–bag-from-to 10 100表示处理bag中10-100秒的数据。(我在实验过程中没有加–bag-from-to 10 100,所以处理的是bag里所有的数据,标定时间比较长)
–show-extraction表示显示检测特征点的过程。
这些参数可以相应的调整。