VINS-DUAL 测试（小觅相机）

桓琰

于 2021-09-27 21:01:38 发布

阅读量343

点赞数

分类专栏： SLAM 文章标签： python 线性代数

本文链接：https://blog.csdn.net/tsuibeyond/article/details/120503958

版权

SLAM 专栏收录该内容

8 篇文章 3 订阅

订阅专栏

设置小觅相机参数

直接从小觅相机内部获取各种参数，把小觅相机给的SDK编译好后，找到以下路径中生成的可执行文件

MYNT-EYE-D-SDK/samples/_output/bin

直接执行下面两个文件：

./get_imu_params
./get_image_params

此时在当前路径下生成两个后缀为 .params 的文本文件

根据跑 Euroc 的经验，我们需要获取的参数是（截图自 euroc_config.yaml 文件）：
请添加图片描述
然而，我们从小觅相机中获取的数据为
左相机到右相机之间的旋转和平移关系 $R_l^r$ ， $t_l^r$
左相机到IMU之间的旋转和平移关系 $R_l^i$ ， $t_l^i$
意思是我们需要求解 $R_r^i$ ， $t_r^i$
(其中 $t_l^r$ ， $t_l^i$ 和 $t_r^i$ 是 $3\times 1$ 的列向量)
可以通过以下公式描述上述关系

$\left[ \begin{array}{cc} R_l^r & t_l^r \\ 0 & 1 \end{array} \right] = \left[ \begin{array}{cc} R_r^i & t_r^i \\ 0 & 1 \end{array} \right]^{-1} \left[ \begin{array}{cc} R_l^i & t_l^i \\ 0 & 1 \end{array} \right]$
使用一个python脚本算一下：

import numpy as np

R_rl = np.array([
    [0.99989593029022217, -0.00441348552703857, -0.01373481750488281], 
    [0.00438022613525391, 0.99998736381530762, -0.00244581699371338], 
    [0.01374542713165283, 0.00238549709320068, 0.99990260601043701]
    ]).astype(float)
t_rl = np.array([-120.27870941162109375*0.001, 0.00000000000000000, 0.00000000000000000]).astype(float)


T_rl = np.zeros([4,4]).astype(float)
T_rl[0:3,0:3] = R_rl
T_rl[0:3,3] = t_rl
T_rl[3,3] = float(1)

R_il = np.array([
    [0.99996651999999997, 0.00430873000000000, 0.00695718000000000], 
    [0.00434878000000000, -0.99997400999999997, -0.00575128000000000], 
    [0.00693222000000000, 0.00578135000000000, -0.99995926000000002]
    ]).astype(float)
t_il = np.array([-47.77362000000000108*0.001, -2.23730999999999991*0.001, -1.60071000000000008*0.001]).astype(float)

T_il = np.zeros([4,4]).astype(float)
T_il[0:3,0:3] = R_il
T_il[0:3,3] = t_il
T_il[3,3] = float(1)

T_ir_inv = np.dot(T_rl, np.linalg.inv(T_il))
T_ir = np.linalg.inv(T_ir_inv)

print(T_ir)

# Motion Intrinsics: {accel: {scale: [1.00714999814528117, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 1.00764000000000009, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 1.00417000000000001], assembly: [1.00000000000000000, -0.00067702800000000, 0.00029412800000000, 0.00000000000000000, 1.00000000000000000, 0.00646412000000000, -0.00000000000000000, 0.00000000000000000, 1.00000000000000000], drift: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000], noise: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000], bias: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000], x: [0.00412313300000000, -0.00011329890000000], y: [0.00927127780000000, -0.00013050414000000], z: [0.01740089710000000, -0.00004611135500000]}, gyro: {scale: [0.99681200000000003, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.99685199999999996, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.99445200000000000], assembly: [1.00000000000000000, -0.00521334000000000, -0.00060721600000000, 0.00612016000000000, 1.00000000000000000, 0.00764685000000000, 0.00281205000000000, -0.00986672000000000, 1.00000000000000000], drift: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000], noise: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000], bias: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000], x: [0.50698715429999996, -0.01063148400000000], y: [0.39468408640000002, -0.00773029620000000], z: [-0.18057246162999999, -0.00114695160000000]}}
# Motion Extrinsics left to imu: {rotation: [0.99996651999999997, 0.00430873000000000, 0.00695718000000000, 0.00434878000000000, -0.99997400999999997, -0.00575128000000000, 0.00693222000000000, 0.00578135000000000, -0.99995926000000002], translation: [-47.77362000000000108, -2.23730999999999991, -1.60071000000000008]}

# VGA Intrinsics left: {width: [640], height: [480], fx: [519.80944824218750000], fy: [519.35339355468750000], cx: [317.36367797851562500], cy: [240.04396057128906250], coeffs: [0.02751922607421875, -0.03797149658203125, -0.00048065185546875, -0.00146865844726562, 0.00000000000000000], p: [540.00000000000000000, 0.00000000000000000, 302.95776367187500000, 0.00000000000000000, 0.00000000000000000, 540.00000000000000000, 247.39123535156250000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 1.00000000000000000, 0.00000000000000000], r: [0.99994635581970215, 0.00022053718566895, 0.01035428047180176, -0.00020790100097656, 0.99999916553497314, -0.00121951103210449, -0.01035451889038086, 0.00121736526489258, 0.99994564056396484]}
# VGA Intrinsics right: {width: [640], height: [480], fx: [517.09716796875000000], fy: [516.94146728515625000], cx: [315.47244262695312500], cy: [254.48048400878906250], coeffs: [0.02334594726562500, -0.03065109252929688, 0.00072479248046875, -0.00186157226562500, 0.00000000000000000], p: [540.00000000000000000, 0.00000000000000000, 302.95776367187500000, -64980.50390625000000000, 0.00000000000000000, 540.00000000000000000, 247.39123535156250000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 1.00000000000000000, 0.00000000000000000], r: [0.99969911575317383, 0.00457525253295898, 0.02409851551055908, -0.00460469722747803, 0.99998867511749268, 0.00116312503814697, -0.02409303188323975, -0.00127363204956055, 0.99970889091491699]}
# VGA Extrinsics left to right: {rotation: [0.99989593029022217, -0.00441348552703857, -0.01373481750488281, 0.00438022613525391, 0.99998736381530762, -0.00244581699371338, 0.01374542713165283, 0.00238549709320068, 0.99990260601043701], translation: [-120.27870941162109375, 0.00000000000000000, 0.00000000000000000]}
# HD Intrinsics left: {width: [1280], height: [720], fx: [1042.07275390625000000], fy: [1041.41284179687500000], cx: [641.40972900390625000], cy: [363.59826660156250000], coeffs: [0.01922988891601562, -0.01678848266601562, 0.00020599365234375, 0.00019454956054688, 0.00000000000000000], p: [1052.09997558593750000, 0.00000000000000000, 591.21838378906250000, 0.00000000000000000, 0.00000000000000000, 1052.09997558593750000, 363.12438964843750000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 1.00000000000000000, 0.00000000000000000], r: [0.99961113929748535, -0.00814092159271240, 0.02666985988616943, 0.00814664363861084, 0.99996674060821533, -0.00010359287261963, -0.02666819095611572, 0.00032079219818115, 0.99964427947998047]}
# HD Intrinsics right: {width: [1280], height: [720], fx: [1033.11401367187500000], fy: [1032.40722656250000000], cx: [633.47186279296875000], cy: [362.29101562500000000], coeffs: [0.02237701416015625, -0.02913665771484375, 0.00073623657226562, -0.00110626220703125, 0.00000000000000000], p: [1052.09997558593750000, 0.00000000000000000, 591.21838378906250000, -129621.76562500000000000, 0.00000000000000000, 1052.09997558593750000, 363.12438964843750000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 1.00000000000000000, 0.00000000000000000], r: [0.99929559230804443, -0.00355386734008789, 0.03735816478729248, 0.00354599952697754, 0.99999356269836426, 0.00027847290039062, -0.03735888004302979, -0.00014579296112061, 0.99930179119110107]}
# HD Extrinsics left to right: {rotation: [0.99993216991424561, -0.00460124015808105, -0.01069486141204834, 0.00459778308868408, 0.99998927116394043, -0.00034415721893311, 0.01069629192352295, 0.00029492378234863, 0.99994266033172607], translation: [-119.82902526855468750, 0.00000000000000000, 0.00000000000000000]}

# Final Output

# [[ 9.99747887e-01  8.67180456e-03  2.07117616e-02  7.24747656e-02]
#  [ 8.84062436e-03 -9.99928361e-01 -8.07628400e-03 -1.17397111e-03]
#  [ 2.06402297e-02  8.25745744e-03 -9.99752944e-01  8.81870194e-04]
#  [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]

然后将其整理成

#Rotation from camera0 frame to imu frame, imu^R_cam
extrinsicRotation0: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.99996651999999997, 0.00430873000000000, 0.00695718000000000, 
             0.00434878000000000, -0.99997400999999997, -0.00575128000000000, 
             0.00693222000000000, 0.00578135000000000, -0.99995926000000002]
#Translation from camera0 frame to imu frame, imu^T_cam
extrinsicTranslation0: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [-0.04777362000000000108, -0.00223730999999999991, -0.00160071000000000008]

#Rotation from camera1 frame to imu frame, imu^R_cam
extrinsicRotation1: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [9.99747887e-01,  8.67180456e-03,  2.07117616e-02,
           8.84062436e-03, -9.99928361e-01, -8.07628400e-03,
          2.06402297e-02,  8.25745744e-03, -9.99752944e-01]
#Translation from camera1 frame to imu frame, imu^T_cam
extrinsicTranslation1: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [7.24747656e-02,-1.17397111e-03,8.81870194e-04]

（在计算的时候，误以为 numpy 中 * 和 dot 都是矩阵乘法的运算，但实际上 * 是逐个元素对应相乘，dot 才是矩阵乘法）

在粘贴上面的配置信息到 yaml 文件中时，发现 OpenCV 在解析配置文件时总是报错，反复对比官方给的配置文件后，终于发现是缩进没有到位的问题。

查找 yaml 的语法后，总结以下规则：
规则一：缩进
yaml使用一个固定的缩进风格表示数据层结构关系，Saltstack需要每个缩进级别由两个空格组成。一定不能使用tab键
规则二：冒号
yaml:
mykey: my_value
每个冒号后面一定要有一个空格（以冒号结尾不需要空格，表示文件路径的模版可以不需要空格）
规则三：短横线
想要表示列表项，使用一个短横杠加一个空格。多个项使用同样的缩进级别作为同一个列表的一部分

（小觅相机官方文档链接：http://doc.myntai.com/resource/sdk/mynt-eye-sdk-guide-2.0.1-rc0-html-zh-Hans/mynt-eye-sdk-guide-2.0.1-rc0-html-zh-Hans/src/mynteye/contents.html）
（备忘：Euroc 的图像尺寸：752x480）

做完了上面的步骤，在小觅相机上试了一下，效果不容乐观

又测试了下 VINS-Fusion，还是官方的效果好

经过上面的尝试，准备使用小觅发布 VINS-Fusion 来继续开发了

桓琰

关注

0
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
VINS-DUAL 测试（小觅相机）

设置小觅相机参数直接从小觅相机内部获取各种参数，把小觅相机给的SDK编译好后，找到以下路径中生成的可执行文件MYNT-EYE-D-SDK/samples/_output/bin直接执行下面两个文件：./get_imu_params./get_image_params此时在当前路径下生成两个后缀为 .params 的文本文件根据跑 Euroc 的经验，我们需要获取的参数是（截图自 euroc_config.yaml 文件）：然而，我们从小觅相机中获取的数据为左相机到右相机之间的旋转和平
复制链接

扫一扫

专栏目录