接上一篇博客——视觉IMU对齐的内容和代码的解析，我们已经通过VisualIMUAlignment函数得到了初始值：陀螺仪的bias、尺度因子s、有模长限制的重力向量、各个帧的速度，同时借助重力向量实现了相机坐标系与世界坐标系的对齐。

总结一下就是：相机-IMU对齐指的是将视觉SFM结构和IMU的预积分结果进行对齐，主要分为1）陀螺仪偏差的标定；2）速度、重力向量和尺度初始化；3）对重力进行修正三部分。

在将从视觉SFM中估计出来的位姿信息和IMU预积分的结果对齐之后，我们需要获得世界坐标系中的位姿，也就是计算出PVQ，这样就完成了位姿的初始化估计，后边将用于进行单目紧耦合的VIO操作。

也就是Estimator::visualInitialAlign()视觉惯性联合优化部分。

对应的流程图为：

初始化的理论部分都在visualInitialAlign()函数里。

1. 计算陀螺仪偏置，尺度，重力加速度和速度

bool Estimator::visualInitialAlign()
{TicToc t_g;VectorXd x;if(!VisualIMUAlignment(all_image_frame, Bgs, g, x))return false;...
}

关于VisualIMUAlignment(all_image_frame, Bgs, g, x)，它在initial/initial_alignment.h里面。

bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)
{// 1.1 利用相机旋转约束标定IMU角速度bias    solveGyroscopeBias(all_image_frame, Bgs);// 1.2 利用IMU的平移约束估计重力方向/各b_k帧速度/尺度scaler    if(LinearAlignment(all_image_frame, g, x))        return true;    else         return false;
}

这一部分已经在上一篇博客讨论过了，指路——视觉IMU对齐。继续…

2. 获取滑动窗口内所有图像帧相对于第$l$帧的位姿信息$P_s、R_s$，并将其置为关键帧

bool Estimator::visualInitialAlign()
{// 1. 计算陀螺仪偏置，尺度，重力加速度和速度TicToc t_g;VectorXd x;if(!VisualIMUAlignment(all_image_frame, Bgs, g, x))return false;...// 2. 传递所有图像帧的位姿Ps、Rs，并将其置为关键帧    for (int i = 0; i <= frame_count; i++)    {        Matrix3d Ri = all_image_frame[Headers[i]].R;        Vector3d Pi = all_image_frame[Headers[i]].T;        Ps[i] = Pi;        Rs[i] = Ri;        all_image_frame[Headers[i]].is_key_frame = true;    }...
}

3. 重新计算所有f_manager的特征点深度

// 3. 重新计算所有f_manager的特征点深度    
VectorXd dep = f_manager.getDepthVector();    
for (int i = 0; i < dep.size(); i++)        dep[i] = -1;//将所有特征点的深度置为-1    
f_manager.clearDepth(dep);//triangulat on cam pose , no tic //重新计算特征点的深度    
Vector3d TIC_TMP[NUM_OF_CAM];    
for (int i = 0; i < NUM_OF_CAM; i++)        TIC_TMP[i].setZero();    //RIC中存放的是相机到IMU的旋转，在相机-IMU外参标定部分求得    ric[0] = RIC[0];    f_manager.setRic(ric);    // 三角化计算地图点的深度// Ps中存放的是各个帧相对于参考帧之间的平移，RIC[0]为相机-IMU之间的旋转    f_manager.triangulate(Ps, &(TIC_TMP[0]), &(RIC[0]));

getDepthVector()相关代码如下：

// 求逆深度VectorXd 
FeatureManager::getDepthVector()
{    VectorXd dep_vec(getFeatureCount());    int feature_index = -1;    for (auto &it_per_id : feature) // 特征点    {        it_per_id.used_num = it_per_id.feature_per_frame.size();        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))            continue;
#if 1        dep_vec(++feature_index) = 1. / it_per_id.estimated_depth;
#else        dep_vec(++feature_index) = it_per_id->estimated_depth;
#endif    }    return dep_vec;
}

【注意】

这里的triangulate()和之前视觉初始化中的并不是同一个函数。

这里的函数是定义在feature_manager.cpp里的，之前的是在initial_sfm.cpp里面。数据结构不同，但是原理是一样的，原理详情可见——视觉初始化中的1.4 调用sfm.construct()函数进行三维重建(4)部分。

4. IMU的bias改变，重新计算滑窗内的预积分

这里陀螺仪的偏差Bgs改变了，需遍历滑动窗口中的帧，重新预积分。

for (int i = 0; i <= WINDOW_SIZE; i++)    
{        pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);    
}

$\red{关于IMU预积分函数的解析，之后抽空做，这里先占个坑...}$

5. 将$P_s、V_s$、depth尺度s缩放后从l帧转变为相对于$c_0$帧图像坐标系下

前面初始化中，计算出来的是相对滑动窗口中第$l$帧的位姿，在这里转换到第b0帧坐标系下$R_s$是IMU第$k$帧到滑动窗口中图像第$l$帧的旋转 ,$P_s$是滑动窗口中第$k$帧到第l帧的平移量。

公式：

$s*p_{bk,​b0}​​=s*p_{bk,cl}​​−s*p_{b0,​cl}​​=(s*p_{ck,​cl}​​−R_{bk​,cl}​​*p_{cb}​)−(s*p_{c0,​cl}​​−R_{b0​,cl}​​*p_{cb}​)$

// 5. 将Ps、Vs、depth尺度s缩放后从l帧转变为相对于c0帧图像坐标系下
/**           
* 前面初始化中，计算出来的是相对滑动窗口中第l帧的位姿，在这里转换到第b0帧坐标系下           
* s*p_bk^​b0​​=s*p_bk^​cl​​−s*p_b0^​cl​​=(s*p_ck^​cl​​−R_bk​^cl​​*p_c^b​)−(s*p_c0^​cl​​−R_b0​^cl​​*p_c^b​)           
* TIC[0]是相机到IMU的平移量           
* Rs是IMU第k帧到滑动窗口中图像第l帧的旋转           
* Ps是滑动窗口中第k帧到第l帧的平移量           
* 注意：如果运行的脚本是配置文件中无外参的脚本，那么这里的TIC都是0          
*/    
// (1) 位移Ps    
double s = (x.tail<1>())(0);
for (int i = frame_count; i >= 0; i--)        
Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);    
// （2）速度Vs
int kv = -1;   
map<double, ImageFrame>::iterator frame_i;    
for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++)    
{        if (frame_i->second.is_key_frame)        {            kv++;            Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);        }    
}    
// (3) 深度
// 更新每个地图点被观测到的帧数(used_num)和预测的深度(estimated_depth)
for (auto &it_per_id : f_manager.feature)    
{        it_per_id.used_num = it_per_id.feature_per_frame.size();        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))            continue;        it_per_id.estimated_depth *= s;    
}

6. 通过将重力旋转到z轴上，得到世界坐标系与相机坐标系$c_0$之间的旋转矩阵rot_diff

理论知识在上一篇博客的最后——指路

相关代码：

 /*** refine之后就获得了C_0坐标系下的重力g^{c_0}，此时通过将g^{c_0}旋转至z轴方向，* 这样就可以计算相机系到世界坐标系的旋转矩阵q_{c_0}^w，这里求得的是rot_diff,这样就可以将所有变量调整至世界系中。*/    
// Rs是IMU第k帧到滑动窗口中图像第l帧的旋转    
Matrix3d R0 = Utility::g2R(g);     
double yaw = Utility::R2ypr(R0 * Rs[0]).x();    
//  相机系到世界坐标系的旋转矩阵 R0    
R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;    
g = R0 * g;    
//Matrix3d rot_diff = R0 * Rs[0].transpose();    
Matrix3d rot_diff = R0;

其中g2R(g)的实现代码为：

// R2ypr：旋转矩阵或四元数 到 欧拉角
// ypr2R：欧拉角 到 旋转矩阵或四元数
// 重力旋转到z轴上
Eigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g)
{    Eigen::Matrix3d R0;    Eigen::Vector3d ng1 = g.normalized();    Eigen::Vector3d ng2{0, 0, 1.0};    // 得到两个向量ng1, ng2之间的旋转    R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix();     double yaw = Utility::R2ypr(R0).x(); // 偏航角    R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;     // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0;    return R0; 
}

【补充】

1. Eigen::Quaterniond::FromTwoVectors()：Returns a quaternion representing a rotation between the two arbitrary vectors a and b.
2. vins中旋转矩阵 四元数 转 欧拉角的方法

旋转矩阵或四元数 到 欧拉角的方法：

Utility::R2ypr(q_array[i - j].toRotationMatrix())

输出的是：yaw pitch roll 的vector3d向量，单位是度数，（正负180）

欧拉角 到 旋转矩阵或四元数 的方法:

Utility::ypr2R(Vector3d(-90, 0, 90)); //R_baselink_imu

 输入的是：yaw pitch roll 的vector3d向量，单位是度数，（正负180）

7. 所有变量从参考坐标系c_0旋转到世界坐标系w

//7.所有变量从参考坐标系c_0旋转到世界坐标系w    
for (int i = 0; i <= frame_count; i++)    
{        Ps[i] = rot_diff * Ps[i];        Rs[i] = rot_diff * Rs[i];        Vs[i] = rot_diff * Vs[i];    
}    
//ROS_DEBUG_STREAM("g0     " << g.transpose());    
//ROS_DEBUG_STREAM("my R0  " << Utility::R2ypr(Rs[0]).transpose());return true;
}

至此，初始化的工作全部完成！！
总结一下，初始化一半的工作在于视觉SfM(这部分作用仅仅负责求相机pose)，另一半才是松耦合初始化。

$\red{遗留问题：}$
$\red{1. 三角化里SVD的应用}$
$\red{2. IMU预积分}$
$\red{3. 相机与世界系的对齐???}$