slam标定(三) vio系统

猫叔Rex

发布于 2021-03-16 15:40:06

1.4K0

发布于 2021-03-16 15:40:06

文章被收录于专栏：科学计算

视觉标定(三) vio标定

一、vio标定原理

vio由双目相机及imu构成，两者分别对应camera坐标系与body坐标系，我们需要对两个坐标系之间的位姿矩阵和图像与imu数据时间偏移进行标定。

我们利用B样条曲线对vio系统进行标定。B样条曲线是贝塞尔曲线的延申，贝塞尔曲线是一种高阶曲线，可用于复杂模型的设计，但有两个缺点，一是阶数太高，二是改变局部影响整体。针对这两个缺点，B样条曲线应运而生，B样条曲线实现了指定阶数，实现了贝塞尔曲线的拼接。

标定中我们只是借用B样条曲线实现残差方程的建立，这里我们不对B样条曲线的具体原理展开详述，只介绍其基本使用。B样条曲线主要由控制点，节点，基本函数表组成，我们只要指定了控制点与节点数量，即可按照基本函数表实现B样条曲线的估计，如下图所示，方形控制点，三角形节点。

相机由原始的图像只能得到位姿数据，而imu可以得到三轴加速度及角速度，显然二者没有直接的联系，因此我们通过B样条曲线对相机进行估计，与imu建立残差方程，将加速度误差，角速度误差，零偏误差，位姿运动误差加入到BA优化方程当中，进行非线性优化迭代，如下所示：

for cam in self.CameraChain.camList:
    cam.findTimeshiftCameraImuPrior(self.ImuList[0], verbose)

############################################
## add error terms
############################################
#Add calibration target reprojection error terms for all camera in chain
self.CameraChain.addCameraChainErrorTerms(problem, self.poseDv, blakeZissermanDf=blakeZisserCam, timeOffsetPadding=timeOffsetPadding)

# Initialize IMU error terms.
for imu in self.ImuList:
    imu.addAccelerometerErrorTerms(problem, self.poseDv, self.gravityExpression, mSigma=huberAccel, accelNoiseScale=accelNoiseScale)
    imu.addGyroscopeErrorTerms(problem, self.poseDv, mSigma=huberGyro, gyroNoiseScale=gyroNoiseScale, g_w=self.gravityExpression)

    # Add the bias motion terms.
    if doBiasMotionError:
        imu.addBiasMotionTerms(problem)

# Add the pose motion terms.
if doPoseMotionError:
    self.addPoseMotionTerms(problem, mrTranslationVariance, mrRotationVariance)

# Add a gravity prior
self.problem = problem

时间的偏移依然采用B样条曲线对相机的旋转角速度进行估计，将陀螺仪角速度作为测量，进行互相关计算，得到偏移索引。

二、标定步骤

我们依然采用kalibr工具进行标定，采集vio数据时相比双目数据尽量运动较大充分激励imu，对于生成的bag文件，我们将之前imu与cams的标定结果拿来使用，然后执行下述命令进行标定：

kalibr_calibrate_imu_camera --bag voi.bag --target 2april_6x6.yaml --imu imu_bmx160.yaml --cams camchain-cam.yaml

等待漫长的时间，cam0、cam1、陀螺仪、加速度误差如下图所示，由图可见，cams的重投影误差大于2个像素，标定结果不是很好，最后重新进行双目标定，陀螺仪与加速度误差正常：

最终的标定结果如下所示：

cam0:
  T_cam_imu:
  - [-0.021640613967887967, 0.9996846699952313, 0.012737519523803142, 0.0943689450739029]
  - [-0.05318702193582009, -0.01387363738123537, 0.9984881886549353, -0.016075204084512736]
  - [0.9983500510966545, 0.020930426711856576, 0.0534704845028372, -0.002524643123212848]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [-0.029772066960790305, 0.06754143559647674, -0.05439220501790035,
    0.013562369372802113]
  distortion_model: equidistant
  intrinsics: [288.42279508017543, 288.2501118514416, 314.48227248976, 209.13222129484865]
  resolution: [640, 400]
  rostopic: /cam0/image_raw
  timeshift_cam_imu: 0.038078317290195576
cam1:
  T_cam_imu:
  - [0.01016316997918798, 0.9998444491129306, -0.014414837981561512, -0.020421941435881386]
  - [-0.03860727630216526, 0.014797185810593971, 0.9991448951521571, -0.011343342336408197]
  - [0.999202776313448, -0.009597961770464236, 0.038751657220695845, -0.003410126461149912]
  - [0.0, 0.0, 0.0, 1.0]
  T_cn_cnm1:
  - [0.9991256216395883, -0.028805073535218002, 0.03030280386253717, -0.11509491601667944]
  - [0.028354632585837286, 0.9994824918256658, 0.015190896776503355, 0.0020860975265854184]
  - [-0.030724696812546234, -0.01431838931524384, 0.9994253232399051, 0.0017823520038439426]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [-0.028122535573276014, 0.06241390797743572, -0.050088085109920036,
    0.012376401933419342]
  distortion_model: equidistant
  intrinsics: [289.36201843178367, 289.2052912742452, 312.7097758328454, 210.45276733552467]
  resolution: [640, 400]
  rostopic: /cam1/image_raw
  timeshift_cam_imu: 0.03760055980093673

SLAM标定系列文章

5. IMU标定

本文参与腾讯云自媒体同步曝光计划，分享自微信公众号。

原始发表：2021-03-14，如有侵权请联系 cloudcommunity@tencent.com 删除

本文分享自傅里叶的猫微信公众号，前往查看

如有侵权，请联系 cloudcommunity@tencent.com 删除。

本文参与腾讯云自媒体同步曝光计划，欢迎热爱写作的你一起参与！

登录后参与评论