[从零写VIO|第七节]——VINS-Mono代码精简版代码详解——初始化4visualInitialAlign()(内容|代码)

接上一篇博客——视觉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]);    
}

关于IMU预积分的知识点以及相关代码解析见我的另一篇博客——指路

5. 将$P_s、V_s$、depth尺度s缩放后更新坐标系

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

公式：

$s\ast p_{bk,b0}=s\ast p_{bk,cl}-s\ast p_{b0,cl}=(s\ast p_{ck,cl}-R_{bk,cl}\ast p_{cb})-(s\ast p_{c0,cl}-R_{b0,cl}\ast p_{cb})$

// 5. 将Ps、Vs、depth尺度s缩放后从l帧转变为相对于c0帧图像坐标系下
/**           
* 前面初始化中，计算出来的是相对滑动窗口中第l帧的位姿，在这里转换到IMU bo坐标系下           
* 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帧imu坐标系到第0帧imu坐标系的变换
// 之前相机第l帧为参考系，转换到IMU bo为基准坐标系        
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

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

经过$R_0$修正，参考坐标系(z)轴向下，(x)轴是$c_0$的前方。不过需要注意的是前面视觉在计算位姿的流程中我们找到与窗口中第一个与滑窗中最后一帧有足够视差的帧$c_l$建立参考系，这里的(c_0)实际上不一定是第0帧，它在某些情况下可能是第$l$帧$c_l$。因此，我们要使参考坐标系的(x)轴朝向真正的第(0)帧的水平正前方。
相关代码：

 /**
     * refine之后就获得了C_0坐标系下的重力g^{c_0}，此时通过将g^{c_0}旋转至z轴方向，
     * 这样就可以计算相机系到世界坐标系的旋转矩阵q_{c_0}^w，这里求得的是rot_diff,这样就可以将所有变量调整至世界系中。
    */    
// Rs是IMU第k帧到滑动窗口中图像第l帧的旋转
// R0将参考坐标系旋转到z轴垂直向上    
Matrix3d R0 = Utility::g2R(g);
// R0将参考系的y轴旋转到第0帧的IMU正前方，这个时候x轴也确定了向右。     
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将ng1旋转到[0,0,1]，也就相当于旋转坐标系把z轴旋转到了ng1   
    R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix();
    // 旋转的过程中可能改变了yaw角，再把yaw旋转回原来的位置。
    // 这个是沿z轴的旋转，因此不改变g沿z轴的方向。只是坐标系的x,y轴改变了。     
    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_l旋转到世界坐标系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)，另一半才是松耦合初始化。

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