最近在使用IMU和双目相机进行相关VIO算法的测试，首先要对IMU和相机的外参进行标定，本文主要是对标定过程做一个全面的记录，方便总结和讨论。测试中采用的是小觅双目模组标准版S1030-IR-120/Mono，整个标定过程都是依赖标定工具Kalibr（github网址）。本文将从以下几个方面进行总结：
- 小觅双目SDK在ubuntu环境的安装
- Kalibr 源码安装
- 调用小觅SDK发布数据
- 双目内参与外参标定
- IMU内参标定
- 相机IMU外参标定
1. 双目DSK在ubuntu下的安装
小觅双目SDK的安装比较简单，可以直接参照官网，需要提前安装好opencv。
// 获取代码
git clone https://github.com/slightech/MYNT-EYE-S-SDK.git
// 准备依赖
cd <sdk> # <sdk> 是指sdk路径
make init
// 编译代码, 默认安装在/usr/local目录下
make install
// 编译样例
make samples
// 如果安装了ros,可以继续编译SDK提供的ros接口
make ros
// 安装完成后，source bash文件
source wrappers/ros/devel/setup.bash
// 这个节点没有图像显示，可以通过rostopic list查看发布的主题
roslaunch mynt_eye_ros_wrapper mynteye.launch
// 这个节点可以通过RViz查看各种发布的数据
roslaunch mynt_eye_ros_wrapper display.launch
2. Kalibr源码安装
Kalibr可以通过源码安装，或者直接使用提供的CDE-Package。对于CDE-Package，下载后直接解压就可以使用但是无法使用Kalibr的Camera Focus 和 Calibration Validator工具。通过源码安装需要提前安装Ros，官网的教程是基于ros indigo版本的，后续的版本可以按照同样的步骤安装。需要值得注意的是，Kalibr依赖大量的python包，因此如果环境中安装了anaconda，需要注意在安装过程中不要系统下的python2.7和anaconda下的python版本混用。现在以ros kinectic为例，总结一下在kinetic环境下的安装过程。
安装依赖项
sudo apt-get install python-setuptools
sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen
sudo apt-get install ros-kinetic-vision-opencv ros-kinetic-image-transport-plugins ros-kinetic-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
创建工作空间
mkdir -p ~/kalibr_workspace/src
cd ~/kalibr_workspace
source /opt/ros/kinetic/setup.bash
catkin init
catkin config --extend /opt/ros/kinetic
catkin config --merge-devel # Necessary for catkin_tools >= 0.4. catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
下载源码
cd ~/kalibr_workspace/src
git clone https://github.com/ethz-asl/Kalibr.git
编译
cd ~/kalibr_workspace
catkin build -DCMAKE_BUILD_TYPE=Release -j4
设置环境变量
source ~/kalibr_workspace/devel/setup.bash
3. 调用小觅SDK发布数据
由于Kalibr需要ros打包成的bag数据作为数据输入流，因此我们需要将从小觅SDK获取的数据通过ros节点发布出去。SDK提供了ros数据发布程序，由于发布的数据类型太多，我们做标定其实只需要左右相机的数据和imu数据，因此单独写了一个程序通过ros发布数据，如下所示
/************************************************************************
> File Name: read_img_imu_to_publish_online.cpp
> Author:
> Mail:
> Created Time: 2020年04月02日 星期四 16时01分12秒
************************************************************************/
#include "mynteye/api/api.h"
MYNTEYE_USE_NAMESPACE
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <thread>
#include <vector>
#include <sstream>
#include <fstream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Header.h>
#define SHOW_IMAGE
std::shared_ptr<API> auto_api;
ros::Publisher pub_imu;
ros::Publisher pub_img_left, pub_img_right;
void publish_img_and_imu()
{
cv::Mat frame_left;
cv::Mat frame_right;
cv::Mat frame_combine;
sensor_msgs::ImagePtr msg_cam;
//double time_cam;
std_msgs::Header header_cam;
double time_imu;
sensor_msgs::Imu msg_imu;
unsigned long int pre_imu_time = 0;
int times_over_time = 0;
while (1)
{
auto_api->WaitForStreams();
auto &&left_data = auto_api->GetStreamData(Stream::LEFT);
auto &&right_data = auto_api->GetStreamData(Stream::RIGHT);
auto &&motion_datas = auto_api->GetMotionDatas();
frame_left = left_data.frame;
frame_right = right_data.frame;
if (!frame_left.empty() && !frame_right.empty())
{
// publish left camera
ros::Time t_left(left_data.img->timestamp / (1e6));
header_cam.stamp = t_left;
msg_cam = cv_bridge::CvImage(header_cam, "mono8", frame_left).toImageMsg();
pub_img_left.publish(msg_cam);
// publish right camera
ros::Time t_right(right_data.img->timestamp / (1e6));
header_cam.stamp = t_right;
msg_cam = cv_bridge::CvImage(header_cam, "mono8", frame_right).toImageMsg();
pub_img_right.publish(msg_cam);
// publish imu data
for (auto &data : motion_datas)
{
time_imu = data.imu->timestamp;
ros::Time t_imu(time_imu / (1e6));
msg_imu.header.stamp = t_imu;
msg_imu.linear_acceleration.x = data.imu->accel[0] * 9.81;
msg_imu.linear_acceleration.y = data.imu->accel[1] * 9.81;
msg_imu.linear_acceleration.z = data.imu->accel[2] * 9.81;
msg_imu.angular_velocity.x = data.imu->gyro[0] / 57.29578;
msg_imu.angular_velocity.y = data.imu->gyro[1] / 57.29578;
msg_imu.angular_velocity.z = data.imu->gyro[2] / 57.29578;
pub_imu.publish(msg_imu);
pre_imu_time = (unsigned long int)data.imu->timestamp;
}
#ifdef SHOW_IMAGE
cv::hconcat(frame_left, frame_right, frame_combine);
cv::imshow("frame_left_right", frame_combine);
cv::waitKey(10);
#endif
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "xiaomi");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
auto_api = API::Create(argc, argv);
bool ok = false;
auto &&request = auto_api->SelectStreamRequest(&ok);
if (!ok)
{
VFF_PRINT_INFO_RED("SelectStreamRequest(&ok) failed !!!\n");
return -1;
}
auto_api->ConfigStreamRequest(request);
// enable left and right image
auto_api->SetOptionValue(Option::FRAME_RATE, 20);
auto_api->EnableStreamData(Stream::LEFT);
auto_api->EnableStreamData(Stream::RIGHT);
// Enable this will cache the motion datas until you get them.
auto_api->EnableMotionDatas();
// Enable imu timestamp correspondence int device;
auto_api->EnableImuTimestampCorrespondence(true);
// Enable all device
auto_api->Start(Source::ALL);
std::thread process(publish_img_and_imu);
pub_img_left = n.advertise<sensor_msgs::Image>("left", 1000);
pub_img_right = n.advertise<sensor_msgs::Image>("right", 1000);
pub_imu = n.advertise<sensor_msgs::Imu>("imu", 1000);
ros::spin();
return 0;
}
4. 双目内参与外参标定
现在开始标定双目内参和外参，我们可以使用Kalibr提供的kalibr_create_target_pdf 生成合适的checkboard 或者 Aprilgrid。比如调用如下就可以获得边长为5cm，x方向7个角点，y方向6个角点的棋盘格，最后原比例打印即可。
kalibr_create_target_pdf --type checkerboard --nx 7 --ny 6 -csx 0.05 --csy 0.05接着就是通过SDK发布主题，例如运行本文示例的发布数据节点，可以发布左右图像和imu的主题，如下图所示
、
由于获取图像数据的帧率为20HZ，这会使得标定的图像过多, 而导致计算量太大. 最好将ros topic的频率降低到4hz左右进行采集，指令如下 :
rosrun topic_tools throttle messages /xiaomi/left/ 4.0 /left
rosrun topic_tools throttle messages /xiaomi/right/ 4.0 /right
下面就是制作bag文件
rosbag record -O stereo_calibra.bag /left /right
在采集图像的过程中，应该尽量保证左右图像始终能够覆盖棋盘格，并且在不同的距离和姿态下采集棋盘格图像。
采集完成后，就可以进行标定了，示例如下
source ros_ws/kalibr/devel/setup.bash
kalibr_calibrate_cameras --bag stereo_calibra.bag --topics /left /right --models pinhole-equi pinhole-equi --target checkboard_0.028.yaml
其中--bag 指定我们制作的bag文件， --topics 指定图像的主题名称 ， --models 指定相机的成像和畸变模型，Kalibr支持['pinhole-radtan', 'pinhole-equi', 'omni-radtan', 'pinhole-fov', 'ds-none', 'omni-none', 'eucm-none'] 7中模式，对应小觅双目相机，文档中说明是pinhole的成像模型和KB的畸变模型，因此选择pinhole-equi模式，对成像模型和畸变模型分类不太清楚的小伙伴可以参考这篇博客，最后是指定棋盘格参数文件，定义如下
target_type: 'checkerboard' #gridtype
targetCols: 7 #number of internal chessboard corners
targetRows: 6 #number of internal chessboard corners
rowSpacingMeters: 0.0280 #size of one chessboard square [m]
colSpacingMeters: 0.0280 #size of one chessboard square [m]
5. IMU内参标定
现在进行IMU内参标定，港科大提供了一个c++版本的基于艾伦方差分析IMU性能的ros程序包（github网址）。安装之前需要安装ceres库。具体安装过程参考博客。安装时不要同时把imu_utils和code_utils一起放到src下进行编译。由于imu_utils 依赖 code_utils，所以先把code_utils放在工作空间的src下面，进行编译。然后再将imu_utils放到src下面，再编译。在code_utils下面找到sumpixel_test.cpp，修改#include "backward.hpp"为 #include “code_utils/backward.hpp”，再编译。否则报错:code_utils-master/src/sumpixel_test.cpp:2:24: fatal error: backward.hpp:No such file or directory。
接着静止IMU两个小时，录制bag文件
rosbag record xiaomi/imu/ -O imu.bag最后标定IMU
rosbag play -r 200 imu.bag
roslaunch imu_utils myImu.launch其中myImu.launch文件如下所示，根据自己的命名和时长进行相应的配置更改
<launch>
<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
<param name="imu_topic" type="string" value= "xiaomi/imu/"/> #imu topic的名字
<param name="imu_name" type="string" value= "myImu"/>
<param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
<param name="max_time_min" type="int" value= "120"/> #标定的时长
<param name="max_cluster" type="int" value= "100"/>
</node>
</launch>
标定结束后会给出imu加速度计和陀螺仪在三个方向上的白噪声和bias，不知道为什么x-axis没有结果？
%YAML:1.0
---
type: IMU
name: myImu
Gyr:
unit: " rad/s"
avg-axis:
gyr_n: 4.0247260386465795e-04
gyr_w: 5.9733058341974576e-06
x-axis:
gyr_n: 0.
gyr_w: 0.
y-axis:
gyr_n: 1.0945630658410598e-03
gyr_w: 1.7594523317638316e-05
z-axis:
gyr_n: 1.1285474575291389e-04
gyr_w: 3.2539418495405600e-07
Acc:
unit: " m/s^2"
avg-axis:
acc_n: 1.1629728464510575e-02
acc_w: 6.0224438509619770e-04
x-axis:
acc_n: 1.1728515107341944e-02
acc_w: 1.8647839735588166e-04
y-axis:
acc_n: 1.2602944080536213e-02
acc_w: 8.0636267894838463e-04
z-axis:
acc_n: 1.0557726205653565e-02
acc_w: 8.1389207898432698e-046. 相机IMU外参标定
从上面的步骤已经获得了双目相机内参和外参yaml文件，获得了IMU的内参yaml文件，现在使用apritag进行IMU和相机的外参标定，同样可以通过kalibr_create_target_pdf生成我们想要的标定图像。比如可以选择设置水平方向和竖直方向的tag数量，设置每个tag的边长大小和tag之间的间隔大小。在下面的事例中，我们选择生成水平方向6个tags和竖直方向7个tags的标定板，每个tag的边长为2cm, tag之间的间距为6mm，其中tspace的含义是tag之间的距离与tag边长的比值。
kalibr_create_target_pdf --type apriltag --nx 6 --ny 7 --tsize 0.02 --tspace 0.3制作完成后，原比例打印，可以通过测试做最后的确定。接着我们定义标定的参数yaml文件，如下所示
target_type: 'aprilgrid' #gridtype
tagCols: 6 #number of apriltags
tagRows: 7 #number of apriltags
tagSize: 0.02 #size of apriltag, edge to edge [m]
tagSpacing: 0.3 #ratio of space between tags to tagSize
接下来就是采集数据制作bag文件，采集指定如下
rosbag record -O stereo_imu_calibra.bag /xiaomi/left /xiaomi/right/ /xiaomi/imu/
采集过程中需要注意，具体流程可以查看视频（需翻墙），或者直接下载我上传的资源。
1. 采集数据的起始和结束阶段注意别晃动太大，如从桌子上拿起或者放下。如果有这样的动作，在标定阶段应该跳过bag数据集的首尾的数据.
2. 首先充分旋转IMU，让陀螺仪的每个轴充分激励，依对roll, yall, pitch进行充分旋转，旋转过程中始终保持图像能够覆盖标定板，且每个轴旋转3次。
3. 接着在X,Y,Z方向依次充分平移3次。
4. 最后在空间中做8字形的运动。
采集数据后就可以做最后的标定了
kalibr_calibrate_imu_camera --target april_6x7_2cm_tagsize.yaml --cam camchain-stereo_calib.yaml --imu xiaomi_imu.yaml --bag stereo_imu_calibra.bag --bag-from-to 2 45至于怎么很好的验证的标定的好坏，还在探索中，欢迎讨论交流。
835
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
