VINS-Mono代码解读——视觉惯性联合初始化 initialStructure sfm
前言
本文主要介绍VINS状态估计器模块（estimator）中的初始化过程（initial），对应论文第五章（V. ESTIMATOR INITIALIZATION），主要在代码中/vins_estimator节点的相关部分实现。
由于单目紧耦合的VIO是一个高度非线性系统，单目视觉没有尺度信息，IMU的测量又存在偏置误差，如果没有良好的初始值很难将这两种测量结果有机融合，因而初始化是VIO最脆弱的步骤。
VINS采用了视觉和IMU的松耦合初始化方案，首先用从运动恢复结构（SFM）得到纯视觉系统的初始化，即滑动窗口中所有帧的位姿和所有路标点的3d位置，然后将其与IMU预积分结果进行对齐，恢复尺度因子、重力、陀螺仪偏置和每一帧的速度。
考虑初始化的原理和代码实现结合的比较紧密，决定将论文内容放到具体函数出现时再进行解释。
另外我认为要看懂这部分代码，很重要的一点是你要知道每个变量的具体含义，尤其是矩阵R（或四元数Q）和向量T，它代表的是哪一帧相对于什么坐标系的位姿，或是什么坐标系转换到什么坐标系下的变换矩阵。代码中有大量求对称的变换矩阵以及openCV函数求解RT，需要明白其到底在做什么。
更正：
Mat cv::findFundamentalMat(
nputArray points1, //第一幅图像点的数组
InputArray points2, //第二幅图像点的数组
int method = FM_RANSAC, //RANSAC 算法
double param1 = 3., //点到对极线的最大距离，超过这个值的点将被舍弃
double param2 = 0.99, //矩阵正确的可信度
OutputArray mask = noArray() //输出在计算过程中没有被舍弃的点
)
函数findFundamentalMat()既可以用来求基本矩阵F，也可以求本质矩阵E。求什么取决于你的参数points1和points2是像素坐标还是归一化平面坐标。因为对极约束的式子是：
x 2 T E x 1 = p 2 T F p 1 = 0 x^T_2Ex_1=p^T_2Fp_1=0 x2TEx1=p2TFp1=0其中本质矩阵 E = [ t ] × R E=[t]_\times R E=[t]×R，基本矩阵 F = K − T E K − 1 F=K^{-T}EK^{-1} F=K−TEK−1
而在VINS初始化阶段都是用于求本质矩阵然后恢复Rt，之前看错了。
代码实现
代码主要包含在estimator.cpp以及initial文件夹中，有关在初始化前的一些流程可以参考我的上一篇博客VINS-Mono代码解读——状态估计器流程。
这里直接从estimator.cpp中的 if (solver_flag == INITIAL) 开始解释。
初始化入口
frame_count表示目前滑动窗口中图像帧的数量，一开始初始化为0，WINDOW_SIZE=10表示滑动窗口中存储第0帧~第10帧的信息，这里目的是为了确保有足够的frame参与初始化。
if (frame_count == WINDOW_SIZE)
{
bool result = false;
//有外参且当前帧时间戳大于初始化时间戳0.1秒，就进行初始化操作
if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
{
//视觉惯性联合初始化
result = initialStructure();
//更新初始化时间戳
initial_timestamp = header.stamp.toSec();
}
if(result)//初始化成功
{
//先进行一次滑动窗口非线性优化，得到当前帧与第一帧的位姿
solver_flag = NON_LINEAR;
solveOdometry();
slideWindow();
f_manager.removeFailures();
ROS_INFO("Initialization finish!");
last_R = Rs[WINDOW_SIZE];
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
else
slideWindow();//初始化失败则直接滑动窗口
}
else
frame_count++;//图像帧数量+1
initialStructure() 视觉惯性联合初始化
1、通过计算线加速度的标准差，判断IMU是否有充分运动激励，以进行初始化
注意这里并没有算上all_image_frame的第一帧，所以求均值和标准差的时候要减一
map<double, ImageFrame>::iterator frame_it;
Vector3d sum_g;
for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++){
double dt = frame_it->second.pre_integration->sum_dt;
Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
sum_g += tmp_g;
}
Vector3d aver_g;
aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);//均值
double var = 0;
for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
{
double dt = frame_it->second.pre_integration->sum_dt;
Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);
}
var = sqrt(var / ((int)all_image_frame.size() - 1));//标准差
if(var < 0.25)
ROS_INFO("IMU excitation not enouth!");
2、将f_manager中的所有feature保存到vector
这里解释一下SFMFeature，其存放的是特征点的信息
struct SFMFeature
{
bool state;//状态（是否被三角化）
int id;
vector<pair<int,Vector2d>> observation;//所有观测到该特征点的图像帧ID和图像坐标
double position[3];//3d坐标
double depth;//深度
};
3、保证具有足够的视差,由E矩阵恢复R、t
这里的第L帧是从第一帧开始到滑动窗口中第一个满足与当前帧的平均视差足够大的帧，会作为参考帧到下面的全局sfm使用，得到的Rt为当前帧到第l帧的坐标系变换Rt
if (!relativePose(relative_R, relative_T, l))
{
ROS_INFO("Not enough features or parallax; Move device around");
return false;
}
4、对窗口中每个图像帧求解sfm问题，得到所有图像帧相对于参考帧的旋转四元数Q、平移向量T和特征点坐标sfm_tracked_points。
GlobalSFM sfm;
if(!sfm.construct(frame_count + 1, Q, T, l,
relative_R, relative_T,
sfm_f, sfm_tracked_points))
{
//求解失败则边缘化最早一帧并滑动窗口
ROS_DEBUG("global SFM failed!");
marginalization_flag = MARGIN_OLD;
return false;
}
5、对于所有的图像帧，包括不在滑动窗口中的，提供初始的RT估计，然后solvePnP进行优化
map<double, ImageFrame>::iterator frame_it;
map<int, Vector3d>::iterator it;
frame_it = all_image_frame.begin( );
for (int i = 0; frame_it != all_image_frame.end( ); frame_it++)
{
cv::Mat r, rvec, t, D, tmp_r;
if((frame_it->first) == Headers[i].stamp.toSec())
{
frame_it->second.is_key_frame = true;
frame_it->second.R = Q[i].toRotationMatrix() * RIC[0].transpose();
frame_it->second.T = T[i];
i++;
continue;
}
if((frame_it->first) > Headers[i].stamp.toSec())
i++;
//注意这里的 Q和 T是图像帧的位姿，而不是求解PNP时所用的坐标系变换矩阵，两者具有对称关系
Matrix3d R_inital = (Q[i].inverse()).toRotationMatrix();
Vector3d P_inital = - R_inital * T[i];
cv::eigen2cv(R_inital, tmp_r);
//罗德里格斯公式将旋转矩阵转换成旋转向量
cv::Rodrigues(tmp_r, rvec);
cv::eigen2cv(P_inital, t);
frame_it->second.is_key_frame = false;
//获取 pnp需要用到的存储每个特征点三维点和图像坐标的 vector
vector<cv::Point3f> pts_3_vector;
vector<cv::Point2f> pts_2_vector;
for (auto &id_pts : frame_it->second.points)
{
int feature_id = id_pts.first;
for (auto &i_p : id_pts.second)
{
it = sfm_tracked_points.find(feature_id);
if(it != sfm_tracked_points.end())
{
Vector3d world_pts = it->second;
cv::Point3f pts_3(world_pts(0), world_pts(1), world_pts(2));
pts_3_vector.push_back(pts_3);
Vector2d img_pts = i_p.second.head<2>();
cv::Point2f pts_2(img_pts(0), img_pts(1));
pts_2_vector.push_back(pts_2);
}
}
}
//保证特征点数大于 5
cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
if(pts_3_vector.size() < 6)
{
cout << "pts_3_vector size " << pts_3_vector.size() << endl;
ROS_DEBUG("Not enough points for solve pnp !");
return false;
}
/**
*bool cv::solvePnP( 求解 pnp问题
* InputArray objectPoints, 特征点的3D坐标数组
* InputArray imagePoints, 特征点对应的图像坐标
* InputArray cameraMatrix, 相机内参矩阵
* InputArray distCoeffs, 失真系数的输入向量
* OutputArray rvec, 旋转向量
* OutputArray tvec, 平移向量
* bool useExtrinsicGuess = false, 为真则使用提供的初始估计值
* int flags = SOLVEPNP_ITERATIVE 采用LM优化
*)
*/
if (! cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1))
{
ROS_DEBUG("solve pnp fail!");
return false;
}
cv::Rodrigues(rvec, r);
MatrixXd R_pnp,tmp_R_pnp;
cv::cv2eigen(r, tmp_R_pnp);
//这里也同样需要将坐标变换矩阵转变成图像帧位姿，并转换为IMU坐标系的位姿
R_pnp = tmp_R_pnp.transpose();
MatrixXd T_pnp;
cv::cv2eigen(t, T_pnp);
T_pnp = R_pnp * (-T_pnp);
frame_it->second.R = R_pnp * RIC[0].transpose();
frame_it->second.T = T_pnp;
}
6、进行视觉惯性联合初始化
if (visualInitialAlign())
return true;
else
{
ROS_INFO("misalign visual structure with IMU");
return false;
}
bool relativePose(relative_R, relative_T, l)
该函数判断每帧到窗口最后一帧对应特征点的平均视差大于30，且内点数目大于12则可进行初始化，同时返回当前帧到第l帧的坐标系变换R和T
bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)
{
for (int i = 0; i < WINDOW_SIZE; i++)
{
//寻找第i帧到窗口最后一帧的对应特征点
vector<pair<Vector3d, Vector3d>> corres;
corres = f_manager.getCorresponding(i, WINDOW_SIZE);
if (corres.size() > 20){
//计算平均视差
double sum_parallax = 0;
double average_parallax;
for (int j = 0; j < int(corres.size()); j++){
//第j个对应点在第i帧和最后一帧的(x,y)
Vector2d pts_0(corres[j].first(0), corres[j].first(1));
Vector2d pts_1(corres[j].second(0), corres[j].second(1));
double parallax = (pts_0 - pts_1).norm();
sum_parallax = sum_parallax + parallax;
}
average_parallax = 1.0 * sum_parallax / int(corres.size());
//判断是否满足初始化条件：视差>30和内点数满足要求(大于12)
//solveRelativeRT()通过基础矩阵计算当前帧与第l帧之间的R和T,并判断内点数目是否足够
//同时返回窗口最后一帧（当前帧）到第l帧（参考帧）的relative_R，relative_T
if(average_parallax * 460 > 30 && m_estimator.solveRelativeRT(corres, relative_R, relative_T)){
l = i;
ROS_DEBUG("average_parallax %f choose l %d and newest frame to triangulate the whole structure", average_parallax * 460, l);
return true;
}
}
}
return false;
}
在m_estimator.solveRelativeRT(corres, relative_R, relative_T)中主要用到了两个opencv函数，这里稍微解释一下：
/**
* Mat cv::findFundamentalMat( 返回通过RANSAC算法求解两幅图像之间的本质矩阵E
* nputArray points1, 第一幅图像点的数组
* InputArray points2, 第二幅图像点的数组
* int method = FM_RANSAC, RANSAC 算法
* double param1 = 3., 点到对极线的最大距离，超过这个值的点将被舍弃
* double param2 = 0.99, 矩阵正确的可信度
* OutputArray mask = noArray() 输出在计算过程中没有被舍弃的点
* )
*/
cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);
/**
* int cv::recoverPose ( 通过本质矩阵得到Rt，返回通过手性校验的内点个数
* InputArray E, 本质矩阵
* InputArray points1, 第一幅图像点的数组
* InputArray points2, 第二幅图像点的数组
* InputArray cameraMatrix, 相机内参
* OutputArray R, 第一帧坐标系到第二帧坐标系的旋转矩阵
* OutputArray t, 第一帧坐标系到第二帧坐标系的平移向量
* InputOutputArray mask = noArray() 在findFundamentalMat()中没有被舍弃的点
* )
*/
int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);
bool sfm.construct()
纯视觉sfm，求解窗口中所有图像帧的位姿QT（相对于第l帧）和特征点坐标sfm_tracked_points
bool GlobalSFM::construct(int frame_num, Quaterniond* q, Vector3d* T, int l,
const Matrix3d relative_R, const Vector3d relative_T,
vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points)
frame_num=frame_count + 1=11，frame_num-1表示当前帧
1、这里把第l帧看作参考坐标系，根据当前帧到第l帧的relative_R，relative_T，得到当前帧在参考坐标系下的位姿，之后的pose[i]表示第l帧到第i帧的变换矩阵[R|T]
q[l].w() = 1;
q[l].x() = 0;
q[l].y() = 0;
q[l].z() = 0;
T[l].setZero();
q[frame_num - 1] = q[l] * Quaterniond(relative_R);
T[frame_num - 1] = relative_T;
2、先三角化第l帧（参考帧）与第frame_num-1帧（当前帧）的路标点
3、pnp求解参考坐标系到第l+1开始的每一帧的变换矩阵R_initial, P_initial，保存在Pose中。
并与当前帧进行三角化。
for (int i = l; i < frame_num - 1 ; i++)
{
if (i > l)
{
Matrix3d R_initial = c_Rotation[i - 1];
Vector3d P_initial = c_Translation[i - 1];
if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
return false;
c_Rotation[i] = R_initial;
c_Translation[i] = P_initial;
c_Quat[i] = c_Rotation[i];
Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
Pose[i].block<3, 1>(0, 3) = c_Translation[i];
}
triangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);
}
4、对第l帧与从第l+1到frame_num-2的每一帧再进行三角化
for (int i = l + 1; i < frame_num - 1; i++)
triangulateTwoFrames(l, Pose[l], i, Pose[i], sfm_f);
5、PNP求解参考坐标系到从第l-1到第0帧的每一帧之间的变换矩阵，并进行三角化
for (int i = l - 1; i >= 0; i--)
{
Matrix3d R_initial = c_Rotation[i + 1];
Vector3d P_initial = c_Translation[i + 1];
if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
return false;
c_Rotation[i] = R_initial;
c_Translation[i] = P_initial;
c_Quat[i] = c_Rotation[i];
Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
Pose[i].block<3, 1>(0, 3) = c_Translation[i];
triangulateTwoFrames(i, Pose[i], l, Pose[l], sfm_f);
}
6、三角化其他未恢复的特征点。至此得到了滑动窗口中所有图像帧的位姿以及特征点的3d坐标
for (int j = 0; j < feature_num; j++)
{
if (sfm_f[j].state == true)
continue;
if ((int)sfm_f[j].observation.size() >= 2)
{
Vector2d point0, point1;
int frame_0 = sfm_f[j].observation[0].first;
point0 = sfm_f[j].observation[0].second;
int frame_1 = sfm_f[j].observation.back().first;
point1 = sfm_f[j].observation.back().second;
Vector3d point_3d;
triangulatePoint(Pose[frame_0], Pose[frame_1], point0, point1, point_3d);
sfm_f[j].state = true;
sfm_f[j].position[0] = point_3d(0);
sfm_f[j].position[1] = point_3d(1);
sfm_f[j].position[2] = point_3d(2);
}
}
7、使用cares进行全局BA优化，代码略
8、这里得到的是第l帧坐标系到各帧的变换矩阵，应将其转变为每一帧在第l帧坐标系上的位姿
for (int i = 0; i < frame_num; i++)
{
q[i].w() = c_rotation[i][0];
q[i].x() = c_rotation[i][1];
q[i].y() = c_rotation[i][2];
q[i].z() = c_rotation[i][3];
q[i] = q[i].inverse();
}
for (int i = 0; i < frame_num; i++)
{
T[i] = -1 * (q[i] * Vector3d(c_translation[i][0], c_translation[i][1], c_translation[i][2]));
}
for (int i = 0; i < (int)sfm_f.size(); i++)
{
if(sfm_f[i].state)
sfm_tracked_points[sfm_f[i].id] = Vector3d(sfm_f[i].position[0], sfm_f[i].position[1], sfm_f[i].position[2]);
}
bool Estimator::visualInitialAlign() 视觉惯性联合初始化
该函数主要实现了陀螺仪的偏置校准(加速度偏置没有处理)，计算速度V[0:n]、重力g、尺度s。
同时更新了Bgs后，IMU测量量需要repropagate；得到尺度s和重力g的方向后，需更新所有图像帧在世界坐标系下的Ps、Rs、Vs。
1、计算陀螺仪偏置，尺度，重力加速度和速度
bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);
if(!result)
{
ROS_DEBUG("solve g failed!");
return false;
}
2、获取所有图像帧的位姿Ps、Rs，并将其置为关键帧
for (int i = 0; i <= frame_count; i++)
{
Matrix3d Ri = all_image_frame[Headers[i].stamp.toSec()].R;
Vector3d Pi = all_image_frame[Headers[i].stamp.toSec()].T;
Ps[i] = Pi;
Rs[i] = Ri;
all_image_frame[Headers[i].stamp.toSec()].is_key_frame = true;
}
3、重新计算所有特征点的深度
//将所有特征点的深度置为-1
VectorXd dep = f_manager.getDepthVector();
for (int i = 0; i < dep.size(); i++)
dep[i] = -1;
f_manager.clearDepth(dep);
//重新计算特征点的深度
Vector3d TIC_TMP[NUM_OF_CAM];
for(int i = 0; i < NUM_OF_CAM; i++)
TIC_TMP[i].setZero();
ric[0] = RIC[0];
f_manager.setRic(ric);
f_manager.triangulate(Ps, &(TIC_TMP[0]), &(RIC[0]));
4、陀螺仪的偏置bgs改变，重新计算预积分
for (int i = 0; i <= WINDOW_SIZE; i++)
{
pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
}
5、将Ps、Vs、depth尺度s缩放后转变为相对于第0帧图像坐标系下
论文提到的以第一帧c0为基准坐标系，通过相机坐标系ck位姿得到IMU坐标系bk位姿的公式为：
之前都是以第l帧为基准坐标系，转换到以第一帧b0为基准坐标系的话应该是：
s p b k b 0 = s p b k c l − s p b 0 c l = ( s p c k c l − R b k c l p c b ) − ( s p c 0 c l − R b 0 c l p c b ) sp^{b_0}_{b_k}=sp^{c_l}_{b_k}-sp^{c_l}_{b_0} =(sp^{c_l}_{c_k}-R^{c_l}_{b_k}p^b_c)-(sp^{c_l}_{c_0}-R^{c_l}_{b_0}p^b_c) spbkb0=spbkcl−spb0cl=(spckcl−Rbkclpcb)−(spc0cl−Rb0clpcb)
for (int i = frame_count; i >= 0; i--)
Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);
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);
}
}
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轴上，得到世界坐标系与摄像机坐标系c0之间的旋转矩阵rot_diff
Matrix3d R0 = Utility::g2R(g);
double yaw = Utility::R2ypr(R0 * Rs[0]).x();
R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;
g = R0 * g;
Matrix3d rot_diff = R0;
7、所有变量从参考坐标系c0旋转到世界坐标系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];
}
至此VINS初始化的代码流程已经结束了，但是最关键的：
VisualIMUAlignment(all_image_frame, Bgs, g, x) 如何求解陀螺仪偏置、速度、重力和尺度还没有讲，而这才是VIO初始化的关键。这部分将留到下一个博客仔细讨论。