前往小程序,Get更优阅读体验!
立即前往
首页
学习
活动
专区
工具
TVP
发布
社区首页 >专栏 >slam标定(三) vio系统

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优化方程当中,进行非线性优化迭代,如下所示:

代码语言:javascript
复制
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的标定结果拿来使用,然后执行下述命令进行标定:

代码语言:javascript
复制
kalibr_calibrate_imu_camera --bag voi.bag --target 2april_6x6.yaml --imu imu_bmx160.yaml --cams camchain-cam.yaml

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

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

代码语言:javascript
复制
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标定系列文章

1. slam标定(二) 双目立体视觉

2. slam标定(一) 单目视觉

3. IMU标定(三)确定误差的标定

4. IMU标定(二)随机误差的标定

5. IMU标定

本文参与 腾讯云自媒体同步曝光计划,分享自微信公众号。
原始发表:2021-03-14,如有侵权请联系 cloudcommunity@tencent.com 删除

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

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

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

评论
登录后参与评论
0 条评论
热度
最新
推荐阅读
目录
  • 视觉标定(三) vio标定
    • 一、vio标定原理
      • 二、标定步骤
      领券
      问题归档专栏文章快讯文章归档关键词归档开发者手册归档开发者手册 Section 归档