小觅相机 相机以及IMU外参标定

最近在使用IMU和双目相机进行相关VIO算法的测试，首先要对IMU和相机的外参进行标定，本文主要是对标定过程做一个全面的记录，方便总结和讨论。测试中采用的是小觅双目模组标准版S1030-IR-120/Mono，整个标定过程都是依赖标定工具Kalibr（github网址）。本文将从以下几个方面进行总结：

1. 小觅双目SDK在ubuntu环境的安装
2. Kalibr 源码安装
3. 调用小觅SDK发布数据
4. 双目内参与外参标定
5. IMU内参标定
6. 相机IMU外参标定

1. 双目DSK在ubuntu下的安装

小觅双目SDK的安装比较简单，可以直接参照官网，需要提前安装好opencv。

// 获取代码
git clone https://github.com/slightech/MYNT-EYE-S-SDK.git

// 准备依赖
cd   #  是指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 
#include 
#include 
#include 

#include 
#include 
#include 
#include 

#include 
#include 

#include 
#include 
#include 
#include 
#include 
#include 
#include 


#define SHOW_IMAGE

std::shared_ptr 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("left", 1000);
	pub_img_right = n.advertise("right", 1000);
	pub_imu = n.advertise("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文件如下所示，根据自己的命名和时长进行相应的配置更改

  
      
              #imu topic的名字
             
          
             #标定的时长
          
      
  

标定结束后会给出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-04

6. 相机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

至于怎么很好的验证的标定的好坏，还在探索中，欢迎讨论交流。