VINS( 三 ) _坐标_甘肃龙网

< gpsTimeList.size(); i++){ stringstream ss;ss << setfill('0') << setw(10) << i;//读取GPS 信息FILE* gpsFile;string gpsFilePath = dataPath2 + "oxts/data/" + ss.str() + ".txt";gpsFile = std::fopen(gpsFilePath.c_str() , "r");if(gpsFile == NULL){printf("cannot find file: %s\n", gpsFilePath.c_str());return 0;}double lat, lon, alt, roll, pitch, yaw;double vn, ve, vf, vl, vu;double ax, ay, az, af, al, au;double wx, wy, wz, wf, wl, wu;double pos_accuracy, vel_accuracy;double navstat, numsats;double velmode, orimode;fscanf(gpsFile, "%lf %lf %lf %lf %lf %lf ", &lat, &lon, &alt, &roll, &pitch, &yaw);fscanf(gpsFile, "%lf %lf %lf %lf %lf ", &vn, &ve, &vf, &vl, &vu);fscanf(gpsFile, "%lf %lf %lf %lf %lf %lf ", &ax, &ay, &az, &af, &al, &au);fscanf(gpsFile, "%lf %lf %lf %lf %lf %lf ", &wx, &wy, &wz, &wf, &wl, &wu);fscanf(gpsFile, "%lf %lf %lf %lf %lf %lf ", &pos_accuracy, &vel_accuracy, &navstat, &numsats, &velmode, &orimode);std::fclose(gpsFile);fprintf (outFile_gps, "%f %f %f %f %f \n",gpsTimeList[i],lat,lon,alt,pos_accuracy);}if(outFile_imu != NULL)fclose (outFile_imu);if(outFile_gps != NULL)fclose (outFile_gps);return 0;}
提取数据
cd /
devel/setup.bash
vins
获得数据后，需要计算IMU和相机之间的外参！具体的参数介绍看下面的连接！
代码如下：
import numpy as np;from numpy import *IMU_2_V =np.mat([[9.999976e-01, 7.553071e-04, -2.035826e-03,-8.086759e-01],[-7.854027e-04, 9.998898e-01, -1.482298e-02, 3.195559e-01],[2.024406e-03, 1.482454e-02, 9.998881e-01,-7.997231e-01],[0,0,0,1]])V_2_C = np.mat([[7.967514e-03, -9.999679e-01, -8.462264e-04, -1.377769e-02],[-2.771053e-03, 8.241710e-04, -9.999958e-01, -5.542117e-02],[9.999644e-01, 7.969825e-03, -2.764397e-03, -2.918589e-01],[0,0,0,1]])C_2_C1 = np.mat([[9.993440e-01, 1.814887e-02, -3.134011e-02, -5.370000e-01],[-1.842595e-02, 9.997935e-01, -8.575221e-03, 5.964270e-03],[3.117801e-02, 9.147067e-03, 9.994720e-01, -1.274584e-02],[0,0,0,1]])C_2_C1_ = np.mat([[1, 0, 0, 0.537150653267924],[0, 1, 0, 0,],[0, 0, 1, 0,],[0, 0, 0, 1]])IMU_2_C0 = V_2_C*IMU_2_VIMU_2_C0 = IMU_2_C0.I#####################################IMU_2_C1 = C_2_C1_.I*V_2_C*IMU_2_VIMU_2_C1 = IMU_2_C1.Iprint("Tb_2_c0:")print(mat(IMU_2_C0))print("Tb_2_c1:")print(mat(IMU_2_C1))
可能有人对公式有点迷茫！这里说一下转换矩阵的 ;
可以是 a坐标系到b坐标系之间的转换；也是 b坐标系上的点，经过矩阵投影到 a 坐标系上的点；
还可以是 a 坐标系下，b坐标系的姿态！
外参定义：b坐标系下的点经过 T 后，转换到 C坐标系下的矩阵！故外参 =(这个写法可能每个人不一样！)

文章插图
运行程序：
cd /
.py
结果如下：

body_T_cam0: !!opencv-matrixrows: 4cols: 4dt: ddata: [ 0.00875116, -0.00479609,0.99995027,1.10224312,-0.99986428, -0.01400249,0.00868325, -0.31907194,0.01396015, -0.99989044, -0.00491798,0.74606588,0, 0, 0, 1]body_T_cam1: !!opencv-matrixrows: 4cols: 4dt: ddata: [ 0.00875116, -0.00479609,0.99995027,1.10694381,-0.99986428, -0.01400249,0.00868325, -0.8561497 ,0.01396015, -0.99989044, -0.00491798,0.75356458,0, 0, 0, 1]

得到外参后，我们把参数放在自己写的配置文件中即可运行程序
注意啦、注意啦！注意啦！
程序中有两个C0到_C1 的转换矩阵，其中，是下面这个文件给的，但是这个是不准确的，不要相信呀，我被坑残了！
而是 VINS- 跑纯双目时给的矩阵（因为没有IMU的时候，body = C0）
这个才是对的，不然的话你们跑 IMU+双目+GPS 用 KITTI数据效果特别差的！自己可以体验一下！
其实我们也可以分析出是有问题的，你看看平移量t就明白了，再看看最前面我们放的两张图片中相机之间的位置，只有x方向上有区别！