VINS 细节系列 - 双目+IMU初始化
目录
一、相关概念
假设有IMU坐标系和相机坐标系如下:
IMU 和 相机有规定其本身的x,y,z轴的方向
1、imu 一般规定右前下分别为x,y,z轴
2、相机一般认为,z轴平行于镜头朝外,x轴与像平面的水平方向一致,从左到右,y轴与像平面垂直方向一致,从上到下
假如: 相机坐标系作为参考坐标系,解算出了 imu坐标系 在 cam坐标系 下的位姿: R ,t
t 代表了平移向量,为imu坐标系原点在相机坐标系的位置;
R 代表的含义可以有以下解释:
- 相机坐标系如何旋转才能转到IMU坐标系 --------- 坐标系的变换
- 表示imu坐标系中的点 通过 这个旋转矩阵 变换到相机坐标系中去 --------- 点的变换
- imu坐标系 在 相机坐标系 的 姿态 (这个更关键,后面会说明白)
注意:坐标系的变换和点的变换是 “相反”的过程;个人感觉这个坐标系转换的概念还是要清晰的!
二、初始化
重力对齐
我们习惯把 第 一帧相机坐标系当作参考坐标系,因为此时的IMU测量的重力加速度还没有进行优化,没有办法与重力加速度 g 对齐,实属无奈之举!
理论部分
我们用第一次MU测量的加速度 g0 =[-9.4 −2.3 0.4],这时候第一帧imu大概是这个样子,坐标系为 C0
因为程序中,这个加速度 g0 是两帧之间IMU加速度的平均值得到的,故这个IMU坐标系也可以看作第一帧相机坐标系;
假设我们要和理想IMU坐标系重力 g = [0 0 1] 对齐, 参考图如下:
注意:为什么是[0 0 9.8]而不是[0 0 -9.8] ?因为vins的公式推算中,g都是用-号,因此这个相当于是一个朝上的重力
第一部:根据两个向量 g、g0 求出 矩阵R0 (这个是有对应公式的;这里没有t0是因为 他们的原点是一样的,只是为了便于区分,图才分为两个);
R0 便是 C0坐标系在 理想坐标系下的姿态;这里我们想一下,这里的R0仅仅是姿态,我们上一步根据重力对齐求出来了,但是这个姿态在
理想坐标系下还是那种 偏航角 yaw 不是0的状态 ,比如:20度;
有人就会有疑问:C0坐标系下的点 g = R0 ✖g0 , 这个不就对齐了嘛?这个....这个有点偷换概念了, 我也迷糊了一会,还是想通了(自己认为)
我们再看一下第一节中R的概念:
- 相机坐标系如何旋转才能转到IMU坐标系 --------- 坐标系的变换
- 表示imu坐标系中的点 通过 这个旋转矩阵 变换到相机坐标系中去 --------- 点的变换
- imu坐标系 在 相机坐标系 的 姿态 (这个更关键,后面会说明白)
他们是独立的表达,可不能混淆!我们用的是 姿态 这个概念,
(这个可以理解吧,可能会优点迷惑,这个是两个坐标系的相对变化,后面的C1,C2...Cn 他们之间的姿态都是我们要求的,只是会最终转换到C0参考系下)
现在我们想把R0的偏航角yaw 在 理想坐标系中为0,怎么办?很好办呀,求出来,旋转补偿一下就行了,也是后面程序对应的内容!
假设C0绕着理想坐标系的Z轴旋转 -yaw, 对应的矩阵为R1,则 R0 = R1*R0 便是 C0坐标系在理想坐标系下 偏航角为 0的 姿态!
有人又问:为什么偏航角yaw要转换成0 ?自己想吧!
程序部分
/-----------------------------------------------------------------程序---------------------------------------------------------------------/
//初始第一个imu位姿
void Estimator::initFirstIMUPose(vector<pair<double, Eigen::Vector3d>> &accVector)
{
printf("init first imu pose\n");
initFirstPoseFlag = true;
//计算加速度的均值
Eigen::Vector3d averAcc(0, 0, 0);
int n = (int)accVector.size();
for(size_t i = 0; i < accVector.size(); i++)
{
averAcc = averAcc + accVector[i].second;
}
averAcc = averAcc / n;
printf("averge acc %f %f %f\n", averAcc.x(), averAcc.y(), averAcc.z());
Matrix3d R0 = Utility::g2R(averAcc);
double yaw = Utility::R2ypr(R0).x();
R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;//另初始的航向为0
Rs[0] = R0;
cout << "init R0 " << endl << Rs[0] << endl;
}
子程序 g2R ()
Eigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g)
{
Eigen::Matrix3d R0;
Eigen::Vector3d ng1 = g.normalized();
Eigen::Vector3d ng2{0, 0, 1.0};
R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix();
double yaw = Utility::R2ypr(R0).x();
R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;
return R0;
}
/-----------------------------------------------------------------程序---------------------------------------------------------------------/
得到的 R0 存入 滑动窗口 的世界坐标系下的旋转 Rs[0];上面我们说的理想坐标系,也是世界坐标系!
自己疑问的地方:
double yaw = Utility::R2ypr(R0).x();
R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;
为什么出现两次?
双目加IMU的初始化
理论部分
对陀螺仪bias的校正
程序部分
/-----------------------------------------------------------------程序---------------------------------------------------------------------/
if(STEREO && USE_IMU)
{
//有了深度之后就可以进行PnP求解
f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
// 双目三角化
f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
if (frame_count == WINDOW_SIZE)
{
map<double, ImageFrame>::iterator frame_it;
int i = 0;
for (frame_it = all_image_frame.begin(); frame_it != all_image_frame.end(); frame_it++)
{
frame_it->second.R = Rs[i];
frame_it->second.T = Ps[i];
i++;
}
solveGyroscopeBias(all_image_frame, Bgs);
for (int i = 0; i <= WINDOW_SIZE; i++)
{
pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
}
optimization();
updateLatestStates(); //让此时刻的值都等于上一时刻的值,用来更新状态
solver_flag = NON_LINEAR;
slideWindow();
ROS_INFO("Initialization finish!");
}
}
/-----------------------------------------------------------------程序---------------------------------------------------------------------/