Vins初始化
Vins初始化过程比较复杂，单看论文难以理解，需要结合代码一起理解。在这里记录一下对初始化的理解，方便以后查看。
初始化的入口函数在Estimator::processImage函数中：
1、初始化的前提条件：
1.1、solver_flag == INITIAL(默认)
1.2、外参已经标定(即已经求解出q(c→b))
1.3、滑动窗口内有足够的图像帧(10帧)
在这里需要说明一下，estimator_node.cpp中的process()线程每运行一次，都会运行一次processImage函数和processImu函数。在窗口内有足够的图像帧之前，每次运行processImage函数，都会将当前需要处理的图像帧插入到窗口中；并且processImu函数也会计算出各图像帧的PVQ与两个相邻像帧间的预积分。
// 使用imu测量值计算预积分
// 将两帧图像间所有imu数据处理完之后，可以得到两帧图像之间的增量
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
// 采用中值法计算第j帧图像的PVQ,Ps[j]、Vs[j]、Rs[j]
// 需要注意的是：在初始化之前，此处的g,Bas,Bgs都为0，所有此处算出的PVQ存在误差
int j = frame_count;
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
Vs[j] += dt * un_acc;1.4、当前帧时间戳 - 上一帧时间戳 > 0.1
2、在满足初始化的条件后，进入initialStructure()函数中
2.1、通过加速度标准差判断IMU是否有充分运动以初始化
2.2、将所有特征点都放入sfm_f中
for (auto &it_per_id : f_manager.feature)
{ // 遍历所有特征点
int imu_j = it_per_id.start_frame - 1;
SFMFeature tmp_feature; // 创建个SFMFeature类对象
tmp_feature.state = false; // 特征点三角化标志，false为没有三角化
tmp_feature.id = it_per_id.feature_id; // 特征点id
for (auto &it_per_frame : it_per_id.feature_per_frame)
{ // 遍历可以观察到特征点的所有帧
imu_j++;
Vector3d pts_j = it_per_frame.point; // 特征点归一化坐标
tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{pts_j.x(),
pts_j.y()})); // 包含可以观察特征点的所有帧
}
sfm_f.push_back(tmp_feature); // 将特征点添加进sfm_f
}2.3、在窗口内找出某帧作为参考帧，并计算当前帧的位姿relative_R，relative_T。需要注意的是图像帧的位姿表示为：relative_R = 参考帧到当前帧的逆；relative_T = -参考帧到当前帧的逆*参考帧到当前帧的位移
bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)
{
for (int i = 0; i < WINDOW_SIZE; i++)
{ // 遍历窗口中当前帧之前的所有图像帧(前9帧)
vector> corres;
// 寻找第i帧与第WINDOW_SIZE帧(也就是当前帧)之间共同特征点
corres = f_manager.getCorresponding(i, WINDOW_SIZE);
if (corres.size() > 20)
{ // 条件1、两帧共同特征点数量 > 20
double sum_parallax = 0;
double average_parallax;
for (int j = 0; j < int(corres.size()); j++)
{ // 计算视差
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());
if(average_parallax * 460 > 30 && m_estimator.solveRelativeRT(corres,
relative_R, relative_T))
{ // 条件2：视差和 > 30 并且 通过对应特征点，可以计算出当前帧的位姿
// 这里的460 ？？？
l = i;
return true;
}
}
}
return false;
} 2.4、计算出窗口内所有帧到参考帧的旋转与平移，并三角化得到地图点(以参考帧为世界坐标系的三角化)；使用ceres，优化得到的旋转、平移，然后转换成各帧的位姿q、T(返回值)
ps:这段代码比较长,但比较重要
bool GlobalSFM::construct(int frame_num, Quaterniond* q, Vector3d* T, int l, const Matrix3d relative_R, const Vector3d relative_T, vector &sfm_f, map &sfm_tracked_points)
{
feature_num = sfm_f.size(); // 特征点数量
// 参考帧旋转q(四元数)，没有旋转
q[l].w() = 1;
q[l].x() = 0;
q[l].y() = 0;
q[l].z() = 0;
T[l].setZero(); // 参考帧平移向量，为0
// 当前帧的位姿
q[frame_num - 1] = q[l] * Quaterniond(relative_R);
T[frame_num - 1] = relative_T;
// rotate to cam frame
Matrix3d c_Rotation[frame_num];
Vector3d c_Translation[frame_num];
Quaterniond c_Quat[frame_num];
double c_rotation[frame_num][4];
double c_translation[frame_num][3];
Eigen::Matrix Pose[frame_num];
// 数组pose[]存放的是参考帧到第i帧的旋转与位移
c_Quat[l] = q[l].inverse();
c_Rotation[l] = c_Quat[l].toRotationMatrix();
c_Translation[l] = -1 * (c_Rotation[l] * T[l]);
Pose[l].block<3, 3>(0, 0) = c_Rotation[l];
Pose[l].block<3, 1>(0, 3) = c_Translation[l];
c_Quat[frame_num - 1] = q[frame_num - 1].inverse();
c_Rotation[frame_num - 1] = c_Quat[frame_num - 1].toRotationMatrix();
c_Translation[frame_num - 1] = -1 * (c_Rotation[frame_num - 1] * T[frame_num - 1]);
Pose[frame_num - 1].block<3, 3>(0, 0) = c_Rotation[frame_num - 1];
Pose[frame_num - 1].block<3, 1>(0, 3) = c_Translation[frame_num - 1];
// 1: trangulate between l ----- frame_num - 1
// 2: solve pnp l + 1; trangulate l + 1 ------- frame_num - 1;
for (int i = l; i < frame_num - 1 ; i++)
{
// solve pnp
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; // 参考帧到第i帧的旋转
c_Translation[i] = P_initial; // 参考帧到第i帧的平移
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];
}
// i=1时三角化当前帧的特征点(以参考帧作为世界坐标系)
// i>1时三角化第i帧的特征点
triangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);
}
// 3: triangulate l-----l+1 l+2 ... frame_num -2
// 此步骤我理解为查漏补缺
for (int i = l + 1; i < frame_num - 1; i++)
triangulateTwoFrames(l, Pose[l], i, Pose[i], sfm_f);
// 4: solve pnp l-1; triangulate l-1 ----- l
// l-2 l-2 ----- l
for (int i = l - 1; i >= 0; i--)
{
//solve pnp
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];
//triangulate
triangulateTwoFrames(i, Pose[i], l, Pose[l], sfm_f);
}
// 5: triangulate all other points
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);
}
}
// full BA
ceres::Problem problem;
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
for (int i = 0; i < frame_num; i++)
{ // 遍历所有帧，向problem中添加旋转与平移
//double array for ceres
c_translation[i][0] = c_Translation[i].x();
c_translation[i][1] = c_Translation[i].y();
c_translation[i][2] = c_Translation[i].z();
c_rotation[i][0] = c_Quat[i].w();
c_rotation[i][1] = c_Quat[i].x();
c_rotation[i][2] = c_Quat[i].y();
c_rotation[i][3] = c_Quat[i].z();
problem.AddParameterBlock(c_rotation[i], 4, local_parameterization);
problem.AddParameterBlock(c_translation[i], 3);
if (i == l)
{
problem.SetParameterBlockConstant(c_rotation[i]);
}
if (i == l || i == frame_num - 1)
{
problem.SetParameterBlockConstant(c_translation[i]);
}
}
for (int i = 0; i < feature_num; i++)
{ // 遍历所有特征点
if (sfm_f[i].state != true)
continue;
for (int j = 0; j < int(sfm_f[i].observation.size()); j++)
{
int l = sfm_f[i].observation[j].first;
ceres::CostFunction* cost_function =
ReprojectionError3D::Create(sfm_f[i].observation[j].second.x(),
sfm_f[i].observation[j].second.y());
problem.AddResidualBlock(cost_function, NULL, c_rotation[l],
c_translation[l], sfm_f[i].position);
}
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.max_solver_time_in_seconds = 0.2;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
if (summary.termination_type == ceres::CONVERGENCE || summary.final_cost < 5e-03)
{
//cout << "vision only BA converge" << endl;
}
else
{
//cout << "vision only BA not converge " << endl;
return false;
}
// 将优化后的旋转与位移转换成位姿
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];
// 转换成第i帧的位姿
q[i] = q[i].inverse();
T[i] = -1 * (q[i] * Vector3d(c_translation[i][0], c_translation[i][1],
c_translation[i][2]));
}
// 下面的代码完全可以移到上面，故将其注释
/*
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++)
{ // 遍历所有特征点，将三角化成功的放入sfm_tracked_points中
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]);
}
return true;
} 2.5、对于所有的图像帧，包括不在滑动窗口中的，提供初始的RT估计，然后solvePnP进行求解,得到每一帧的姿态。
在这里需要提出个问题：由processImage函数可以知道，当滑动窗口内帧数不足时,会直接将新传入的processImage的图像帧添加进窗口，即frame_count++，如下面代码：
if (frame_count == WINDOW_SIZE)
{
//
}
else
frame_count++;那么当 processImage函数运行10次后，即frame_count=10，系统就会开始进行初始化了，那么问题来了，这里的不在滑动窗口内的图像帧是哪里来的？？？
解答：放在第4部分
for (int i = 0; frame_it != all_image_frame.end( ); frame_it++)
{ // 遍历所有帧
// provide initial guess
cv::Mat r, rvec, t, D, tmp_r;
if((frame_it->first) == Headers[i].stamp.toSec())
{ // 如果是全局sfm过的图像帧，则不需在进行pnp求解
frame_it->second.is_key_frame = true; // 关键帧
// q(ci→c0)*q(b→c) = q(bi→c0)
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())
{ // 如果不在全局sfm中的图像帧，则进行pnp求解
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;
vector pts_3_vector;
vector 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);
}
}
}
cv::Mat K = (cv::Mat_(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;
}
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);
// 将变换矩阵变成位姿
R_pnp = tmp_R_pnp.transpose();
MatrixXd T_pnp;
cv::cv2eigen(t, T_pnp);
T_pnp = R_pnp * (-T_pnp);
// q(ci→c0)*q(b→c) = q(bi→c0)
frame_it->second.R = R_pnp * RIC[0].transpose();
frame_it->second.T = T_pnp;
} 2.6、进行视觉惯性联合初始化，即运行visualInitialAlign()函数，由于函数代码较长，所以放到下一节中。
if (visualInitialAlign())
return true;
else
{
ROS_INFO("misalign visual structure with IMU");
return false;
}3、视觉惯性联合初始化
3.1、计算出陀螺仪的bais、尺度s、 重力加速度g和速度各图像帧的速度。需要说明的是，在此之前的，陀螺仪bais与g都为0，所以processImu函数中计算的PVQ存在较大的误差。
// 计算陀螺仪偏置，尺度，重力加速度和速度
// 参数：所有图像帧；陀螺仪偏置数组；重力加速度；速度数组
bool VisualIMUAlignment(map &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)
{
// 计算陀螺仪偏置
solveGyroscopeBias(all_image_frame, Bgs);
if(LinearAlignment(all_image_frame, g, x))
return true;
else
return false;
} 3.1.1、陀螺仪增量的计算公式，直接将崔大佬的推导粘贴过来了。
/// 计算陀螺仪偏置
void solveGyroscopeBias(map &all_image_frame, Vector3d* Bgs)
{
Matrix3d A;
Vector3d b;
Vector3d delta_bg;
A.setZero();
b.setZero();
map::iterator frame_i;
map::iterator frame_j;
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end();
frame_i++)
{
frame_j = next(frame_i);
MatrixXd tmp_A(3, 3);
tmp_A.setZero();
VectorXd tmp_b(3);
tmp_b.setZero();
// 前后两帧图像帧的旋转
Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R,
O_BG);
// 获取对陀螺仪bais的雅克比
tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
// 获取imu预积分中的旋转与q_ij的旋转差
A += tmp_A.transpose() * tmp_A;
b += tmp_A.transpose() * tmp_b;
}
delta_bg = A.ldlt().solve(b); // 计算出陀螺仪的增量
for (int i = 0; i <= WINDOW_SIZE; i++)
Bgs[i] += delta_bg;
/// 使用计算出来的bgs重新计算预积分
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( );
frame_i++)
{
frame_j = next(frame_i);
frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
}
} 问题：为啥用Bgs[0]重新计算预积分，而不是Bgs[i]
解释：因为这里将所有帧都进行了预积分，不仅仅是窗口内的帧；在3.3中，使用Bgs[i]将窗口内的所有帧都重新预积分了
3.1.2、初始化尺度，重力加速度和速度，老规矩，看崔大佬的推导。
简单一点解释初始化过程就是：imu预积分的位移、旋转增量 - 图像通过几何方法算出来的位移、整理 = 0
尺度：图像通过几何方法算出来的位移和特征点深度是没有尺度信息的，所以需要计算尺度。
重力加速度：没看到初始化为g赋值，所有在此步骤之前，g应该一直都为0；注意此处算出的g时在c0相机坐标系下的g。
速度：之前的每帧图像的速度V[i]是对加速度*时间算出，存在误差
bool LinearAlignment(map &all_image_frame, Vector3d &g, VectorXd &x)
{
int all_frame_count = all_image_frame.size();
int n_state = all_frame_count * 3 + 3 + 1; // 状态量个数(其实主要是速度的的)
MatrixXd A{n_state, n_state};
A.setZero();
VectorXd b{n_state};
b.setZero();
map::iterator frame_i;
map::iterator frame_j;
int i = 0;
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end();
frame_i++, i++)
{
frame_j = next(frame_i);
MatrixXd tmp_A(6, 10); // 6 * 10
tmp_A.setZero();
VectorXd tmp_b(6); // 6 * 1
tmp_b.setZero();
double dt = frame_j->second.pre_integration->sum_dt; // 两帧间隔时间
tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 *
Matrix3d::Identity();
tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T -
frame_i->second.T) / 100.0;
tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i
->second.R.transpose() * frame_j->second.R * TIC[0]
- TIC[0];
tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt *
Matrix3d::Identity();
tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v;
Matrix cov_inv = Matrix::Zero();
cov_inv.setIdentity();
MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A; // 10 * 10
VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b; // 10 * 1
A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
b.segment<6>(i * 3) += r_b.head<6>();
A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>();
b.tail<4>() += r_b.tail<4>();
A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>();
A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>();
}
/// 为什么*1000？ 为了算的更精确吗？？？
A = A * 1000.0;
b = b * 1000.0;
x = A.ldlt().solve(b);
double s = x(n_state - 1) / 100.0; // 尺度，这里除100的目的是把尺度的单位变成cm嘛？？
g = x.segment<3>(n_state - 4); // 重力加速度
if(fabs(g.norm() - G.norm()) > 1.0 || s < 0)
{ // 如果g的偏差过大，或者尺度<0，则返false
return false;
}
// 修正g，此函数放下面进行讲解
RefineGravity(all_image_frame, g, x);
s = (x.tail<1>())(0) / 100.0;
(x.tail<1>())(0) = s; // 尺度的返回值
if(s < 0.0 )
return false;
else
return true;
} 3.1.3、优化g，为什么要优化g？因为上面求出的g可能不满足其模长为9.81这个条件，下面图继续引用崔大佬的推导。在这里解释下为什么g的模固定，就只有两个2个自由度了：因为g是个3维向量，有3个自由度，当有一个方向的大小都确定的时候，就固定了一个自由度；如果想要确定g，那么只需要确定另外两个方向的大小就ok。
代码变化不多，上面的代码理解的化，这里应该没问题
void RefineGravity(map &all_image_frame, Vector3d &g, VectorXd &x)
{
Vector3d g0 = g.normalized() * G.norm(); //
Vector3d lx, ly;
int all_frame_count = all_image_frame.size();
int n_state = all_frame_count * 3 + 2 + 1;
MatrixXd A{n_state, n_state};
A.setZero();
VectorXd b{n_state};
b.setZero();
map::iterator frame_i;
map::iterator frame_j;
for(int k = 0; k < 4; k++)
{
MatrixXd lxly(3, 2);
lxly = TangentBasis(g0); // 计算b1、b2的方向
int i = 0;
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end();
frame_i++, i++)
{
frame_j = next(frame_i);
MatrixXd tmp_A(6, 9);
tmp_A.setZero();
VectorXd tmp_b(6);
tmp_b.setZero();
double dt = frame_j->second.pre_integration->sum_dt;
tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
tmp_A.block<3, 2>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 *
Matrix3d::Identity() * lxly;
tmp_A.block<3, 1>(0, 8) = frame_i->second.R.transpose() * (frame_j->second.T
- frame_i->second.T) / 100.0;
tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i
->second.R.transpose() * frame_j->second.R * TIC[0]
- TIC[0] - frame_i->second.R.transpose() * dt * dt
/ 2 * g0;
tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
tmp_A.block<3, 2>(3, 6) = frame_i->second.R.transpose() * dt *
Matrix3d::Identity() * lxly;
tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - frame_i
->second.R.transpose() * dt * Matrix3d::Identity()
* g0;
Matrix cov_inv = Matrix::Zero();
cov_inv.setIdentity();
MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;
A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
b.segment<6>(i * 3) += r_b.head<6>();
A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>();
b.tail<3>() += r_b.tail<3>();
A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>();
A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>();
}
A = A * 1000.0;
b = b * 1000.0;
x = A.ldlt().solve(b);
VectorXd dg = x.segment<2>(n_state - 3);
g0 = (g0 + lxly * dg).normalized() * G.norm();
//double s = x(n_state - 1);
}
g = g0;
} 3.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.3、设置特征点深度(特征点在其起始帧的深度)；并根据陀螺仪的偏置bgs[]矩阵，重新计算预积分。需要注意的是，这里求出的深度还是没有尺度的，在3.4中将会给深度加上尺度。
// 将所有特征点的深度置为-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]));
double s = (x.tail<1>())(0);
// 重新计算预积分
for (int i = 0; i <= WINDOW_SIZE; i++)
{
pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
}3.4、更新Ps、Vs、depth
需要注意Ps这个状态量，Ps前后有过三个值：
3.4.1、Ps[i]由imu积分获得，Ps[i] += dt * Vs[i] + 0.5 * dt * dt * un_acc;
3.4.2、Ps[i]变成以c0帧照片为参考坐标系，第i帧照片的位移(简单来说，就是第i帧到第c0帧的向量)
3.4.3、在2的基础上补充尺度信息，也就是下面的代码；
// Ps按尺度缩放，TIC[0]其实为0向量，所以下面代码可以简化成：
// Ps[i] = s * Ps[i]
for (int i = frame_count; i >= 0; i--)
Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);
// 用前面计算出比较精确的速度代替原有的速度Vs[i]
// 原来的Vs[i]是加速度*时间算出来的
int kv = -1;
map::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); // Vs为优化得到的速度
}
}
// 特征点深度按尺度缩放
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; // 特征点深度*尺度
} 3.5、通过将重力旋转到z轴上，得到世界坐标系与摄像机坐标系c0之间的旋转矩阵rot_diff，然后将第i帧图像帧相对于c0图像帧的PVQ转换成相对于世界坐标系的PVQ。
计算旋转矩阵rot_diff的步骤：
3.5.1、先计算（0，0，1）与g之间的旋转矩阵R1
3.5.2、算出这旋转矩阵之后，两个坐标系还没有重合，还存在的偏航角(即两个坐标系z轴重合了，但是其它两轴还没有重合)，所有还需要计算出偏航角，并将其转换成旋转矩阵R2
3.5.3、R2*R1才是旋转矩阵rot_diff。
Matrix3d R0 = Utility::g2R(g);
// 下面两行代码不是太理解，R0已经算出来了，按道理，yaw不是为0嘛，还是说算两遍结果可以更加精确
double yaw = Utility::R2ypr(R0 * Rs[0]).x();
R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;
g = R0 * g;
// Matrix3d rot_diff = R0 * Rs[0].transpose();
Matrix3d rot_diff = R0;
// 将PVQ从参考坐标系c0旋转到世界坐标系w
for (int i = 0; i <= frame_count; i++)
{
Ps[i] = rot_diff * Ps[i]; // 第i帧坐标系原点到世界坐标系原点的向量
Rs[i] = rot_diff * Rs[i]; // 第i帧坐标系到世界坐标系的旋转
Vs[i] = rot_diff * Vs[i]; // 第i帧图像相对于世界坐标系的速度
}4、如果前面3步骤中的每一个函数都能正常运行(即都return ture)，那么初始化到这里就已经完成。
4.1、对2.5的问题进行解答：
在初始化时候，如果result = initialStructure()返回的是ture，那么这时候的所有帧(即all_image_fram)就是窗口内的10帧；但是如果result = initialStructure()返回的是false，即初始化的时候有个环节出现错误，那么就会进入slideWindow()函数。在slideWindow()函数中会出现all_image_fram>10的情况。slideWindow()函数相关信息请看4.2。
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(); 4.2、滑动窗口函数。滑动窗口函数根据边缘化的不同，分成去除窗口中的第一帧或者去除倒数第二帧两种情况。
4.2.1、marginalization_flag == MARGIN_OLD 清除第一帧，修改特征点的起始帧。因为在此情况下，会清除所有图像帧中的第一帧到窗口中的第一帧之间的帧(读起来有点拗口，其实处理完后就是 all_image_frame的第一帧就是窗口内的第一帧)，所以all_image_frame=10，不会增加all_image_frame的数量。
4.2.2、marginalization_flag == MARGIN_SECOND_NEW 清除倒数第二帧，此时all_image_frame的数量会增加。(2.5的问题到这里才是真正的解决)
void Estimator::slideWindow()
{
TicToc t_margin;
// 边缘化第一帧
if (marginalization_flag == MARGIN_OLD)
{
double t_0 = Headers[0].stamp.toSec();
back_R0 = Rs[0];
back_P0 = Ps[0];
if (frame_count == WINDOW_SIZE)
{
for (int i = 0; i < WINDOW_SIZE; i++)
{ // 将后一帧的信息给前一帧
Rs[i].swap(Rs[i + 1]);
std::swap(pre_integrations[i], pre_integrations[i + 1]);
dt_buf[i].swap(dt_buf[i + 1]);
linear_acceleration_buf[i].swap(linear_acceleration_buf[i + 1]);
angular_velocity_buf[i].swap(angular_velocity_buf[i + 1]);
Headers[i] = Headers[i + 1];
Ps[i].swap(Ps[i + 1]);
Vs[i].swap(Vs[i + 1]);
Bas[i].swap(Bas[i + 1]);
Bgs[i].swap(Bgs[i + 1]);
}
// 将原最后一帧的参数信息作为新帧的估计值
Headers[WINDOW_SIZE] = Headers[WINDOW_SIZE - 1];
Ps[WINDOW_SIZE] = Ps[WINDOW_SIZE - 1];
Vs[WINDOW_SIZE] = Vs[WINDOW_SIZE - 1];
Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1];
Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1];
Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1];
delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0,
Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};
dt_buf[WINDOW_SIZE].clear();
linear_acceleration_buf[WINDOW_SIZE].clear();
angular_velocity_buf[WINDOW_SIZE].clear();
// 这个代码写的有水平
if (true || solver_flag == INITIAL)
{
map::iterator it_0;
it_0 = all_image_frame.find(t_0); // 寻找窗口中的第一帧
delete it_0->second.pre_integration;
it_0->second.pre_integration = nullptr;
for (map::iterator it = all_image_frame.begin();
it != it_0; ++it)
{ // 遍历所有图像帧中的第一帧到窗口中的第一帧
if (it->second.pre_integration)
delete it->second.pre_integration;
it->second.pre_integration = NULL;
}
// 清除所有图像帧中的第一帧到窗口中的第一帧
all_image_frame.erase(all_image_frame.begin(), it_0);
all_image_frame.erase(t_0);
}
slideWindowOld();
}
}
else
{ // 边缘化新帧
if (frame_count == WINDOW_SIZE)
{
for (unsigned int i = 0; i < dt_buf[frame_count].size(); i++)
{
double tmp_dt = dt_buf[frame_count][i];
Vector3d tmp_linear_acceleration = linear_acceleration_buf[frame_count][i];
Vector3d tmp_angular_velocity = angular_velocity_buf[frame_count][i];
pre_integrations[frame_count - 1]->push_back(tmp_dt,
tmp_linear_acceleration, tmp_angular_velocity);
dt_buf[frame_count - 1].push_back(tmp_dt);
linear_acceleration_buf[frame_count -1].push_back(tmp_linear_acceleration);
angular_velocity_buf[frame_count - 1].push_back(tmp_angular_velocity);
}
Headers[frame_count - 1] = Headers[frame_count];
Ps[frame_count - 1] = Ps[frame_count];
Vs[frame_count - 1] = Vs[frame_count];
Rs[frame_count - 1] = Rs[frame_count];
Bas[frame_count - 1] = Bas[frame_count];
Bgs[frame_count - 1] = Bgs[frame_count];
delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0,
Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};
dt_buf[WINDOW_SIZE].clear();
linear_acceleration_buf[WINDOW_SIZE].clear();
angular_velocity_buf[WINDOW_SIZE].clear();
slideWindowNew();
}
}
}