本文详细介绍了视觉惯性系统初始化的过程，包括如何利用运动信息判断初始化可行性，通过纯视觉SFM恢复相机位姿和特征点深度，接着进行视觉/惯性联合初始化，标定IMU角速度bias和世界坐标系的重力、尺度因子。通过旋转和平移约束优化参数，最终实现坐标系对齐，确保系统准确启动。
【4】视觉/惯导联合初始化
为什么要初始化？
答：
- 视觉系统只能获得二维信息，损失了一维信息(深度), 所以需要动一下，也就是三角化才能重新获得损失的深度信息；
- 但是，这个三角化恢复的深度信息，是个“伪深度”，它的尺度是随机的，不是真实的，所以就需要IMU来标定这个尺度；
- 要想让IMU标定这个尺度，IMU也需要动一下（代码中求运动激励），得到PVQ的P；
- 另外，IMU存在bias，视觉获得的旋转矩阵不存在bias，所以可以用视觉来标定IMU的旋转bias；
- 需要获得世界坐标系这个先验信息，通过初始化能借助g来确定；
初始化的逻辑图如下：
初始化的流程图：
上一步【3】的目的是求相机和视觉之间的外参，为什么要放在 【4】的前面呢，哈哈，因为联合初始化的时候需要用到人家，开始上程序！
//【4】进行初始化，一般初始化只进行一次；
if (solver_flag == INITIAL)
{
//frame_count是滑动窗口中图像帧的数量，一开始初始化为0，滑动窗口总帧数WINDOW_SIZE=10
if (frame_count == WINDOW_SIZE)
{
bool result = false;
//确保有足够的frame参与初始化，有外参，且当前帧时间戳大于初始化时间戳+0.1秒
if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
{
// 4.1 视觉惯性联合初始化-包括纯视觉SfM，视觉和IMU的松耦合
result = initialStructure();
// 4.2 更新初始化时间戳-
initial_timestamp = header.stamp.toSec();
}
if(result)//初始化成功则进行一次非线性优化
{
solver_flag = NON_LINEAR;// 先进行一次滑动窗口非线性优化，得到当前帧与第一帧的位姿
solveOdometry(); // 执行非线性优化具体函数solveOdometry()
slideWindow(); // 滑动窗
// 4.5 剔除feature中估计失败的点（solve_flag == 2）0 haven't solve yet; 1 solve succ;
f_manager.removeFailures();
ROS_INFO("Initialization finish!");
// 4.6 初始化窗口中PVQ
last_R = Rs[WINDOW_SIZE];//得到当前帧与第一帧的位姿
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
else
slideWindow();// 初始化失败则直接滑动窗口
//因为只有在solveOdometry后才确定特征点的solve_flag(是否成功解算得到depth值)
}
else
frame_count++;// 图像帧数量+1，直到满足窗口数量
}解析：
首先判断是否可以进行初始化，如果可以，看一下滑动窗口内帧的数量是否足够！如果这两个条件满足，并且有相机和IMU间的外参以及系统运行了一段时间，那么就可以初始化了，然后初始化完成后可以进行一次非线性优化，得到位姿态；如果帧不够，就进行滑窗！
子程序 initialStructure();
主要分为：1、纯视觉SFM估计滑动窗内相机位姿和路标点逆深度 （k时刻的相机相对于0时刻的位姿态 Ck0，C0看作参考坐标系，可以与世界坐标系转换） 2、视觉惯性联合校准，SFM与IMU积分对齐（利用平移和旋转特性，后面会推导的）。
【1】
- 选择一个滑动窗，在最后一帧与滑动窗之前帧 寻找帧：跟踪到的点数目大于30个，并且视差超过20的，找到后用五点法本质矩阵初始化恢复R和t，否则滑动窗内保留最新图像帧，继续下一帧。
- 随意设置一个尺度因子，三角化两帧观测到的所有路标点。 再用Pnp算法估计滑动窗内所有其余帧的位姿。 滑动窗内全局BA重投影误差优化所有帧位姿。
- 假设IMU-camera外参知道，乘以视觉得到的位姿，转换到IMU坐标系下。
【2】
（1）陀螺仪零偏bg标定。
（2）速度v，重力g,尺度s初始化。
（3）重力矢量修正（优化方向）。
Quaterniond Q[frame_count + 1]; //旋转四元数Q
Vector3d T[frame_count + 1]; //平移矩阵T
map<int, Vector3d> sfm_tracked_points; //存储SFM重建出特征点的坐标
vector<SFMFeature> sfm_f; //SFMFeature三角化状态、特征点索引、平面观测、位置坐标、深度
all_image_frame： // 这个数据结构，它包含了滑窗内所有帧的视觉和IMU信息，它是一个hash，以时间戳为索引
为什么容量是frame_count + 1？因为滑窗的容量是10，再加上当前最新帧，所以需要储存11帧的值！
-------------------------------------------------------------------------------------------------
数据结构: vector<SFMFeature> sfm_f
它定义在initial/initial_sfm.h中，
struct SFMFeature
{
bool state;//状态（是否被三角化）
int id;
vector<pair<int,Vector2d>> observation;//所有观测到该特征点的 图像帧ID 和 特征点在这个图像帧的归一化坐标
double position[3];//在帧l下的空间坐标
double depth;//深度
};
（1）这一部分的思想就是通过计算滑窗内所有帧的线加速度的标准差，判断IMU是否有充分运动激励，以判断是否进行初始化
bool Estimator::initialStructure()
{
TicToc t_sfm;
{ map<double, ImageFrame>::iterator frame_it;
// 1.1 先求均值 aver_g
Vector3d sum_g;//sum_g需要置0,bug fixed。
//(第一次循环，求出滑窗内的平均线加速度
for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
{
//all_image_frame这个数据结构，见5.2-2，它包含了滑窗内所有帧的视觉和IMU信息
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;
//总帧数减1，因为all_image_frame第一帧不算
aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);
//1.2 再求标准差var,表示线加速度的标准差。
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);//计算加速度的方差
//cout << "frame g " << tmp_g.transpose() << endl;
}
var = sqrt(var / ((int)all_image_frame.size() - 1));//计算标准差
//以标准差判断可观性
if(var < 0.25)
{
ROS_INFO("IMU excitation not enouth!");
//return false;
//这里不能简单的返回false,这样会导致sfm中pnp无解,如果要返回false就需要改滑窗的特征点管理
}
}疑问：为什么要用这种方法判断一下视觉 IMU 是否可以初始化？不能直接初始化吗？
答：联合初始化的核心思想就是利用运动去求参数，比如相机和IMU都是安装在载体上，那么他们实际的距离一定是一样的，只是测量出的距离不一样而以;故可以利用距离相等求出IMU 的bias, 而旋转也是一个道理！
（2）滑窗内全局的SFM
Quaterniond Q[frame_count + 1];//Twc由相机系到世界系的变换,也就是在世界系(参考关键帧)下的位姿
Vector3d T[frame_count + 1];
map<int, Vector3d> sfm_tracked_points;//feature_id + 三角化且优化后的点坐标
vector<SFMFeature> sfm_f;
//2.1: SFMFeature结构体，它存放着一个特征点的所有信息。容器定义完了，接下来就是往容器里放数据。
for (auto &it_per_id : f_manager.feature)//对于滑窗中出现的所有特征点
{
int imu_j = it_per_id.start_frame - 1;//从start_frame开始帧编号
SFMFeature tmp_feature;
tmp_feature.state = false; //状态（是否被三角化）
tmp_feature.id = it_per_id.feature_id;//特征点id
//对于当前特征点 在每一帧的坐标
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;//帧编号+1
Vector3d pts_j = it_per_frame.point;//当前特征在编号为imu_j++的帧的归一化坐标
//该feature在哪几帧中被观测到,同时记录其在滑动窗口中的位置
tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{pts_j.x(), pts_j.y()}));//把当前特征在当前帧的坐标和当前帧的编号配上对
//tmp_feature.observation里面存放着的一直是同一个特征，每一个pair是这个特征在 不同帧号 中的 归一化坐标。
}
sfm_f.push_back(tmp_feature);//sfm_f里面存放着是不同特征
}
//在滑窗(0-9)中找到第一个满足要求的帧(第l帧)，它与最新一帧(frame_count=10)有足够的共视点和平行度，并求出这两帧之间的相对位置变化关系；
Matrix3d relative_R;
Vector3d relative_T;//当前帧到共视帧
int l;//滑窗中满足与最新帧视差关系的那一帧的帧号l
//2.2 a.计算滑窗内的每一帧(0-9)与最新一帧(10)之间的视差，直到找出第一个满足要求的帧，作为我们的第l帧；
if (!relativePose(relative_R, relative_T, l))
{
//这里的第L帧是从第一帧开始到滑动窗口中第一个满足与当前帧的平均视差足够大的帧l，
//会作为 参考帧 到下面的全局sfm 使用，得到的Rt为当前帧到第l帧的坐标系变换Rt。
ROS_INFO("Not enough features or parallax; Move device around");
return false;
}
//construct()：对窗口中每个图像帧求解sfm问题，
// 得到所有图像帧相对于参考帧l的姿态四元数Q、平移向量T和特征点坐标sfm_tracked_points （核心）
// 看一下*construct()*里面干了什么。见initial/initial_sfm.cpp文件
GlobalSFM sfm;
if(!sfm.construct(frame_count + 1, Q, T, l,
relative_R, relative_T,
sfm_f, sfm_tracked_points))
{
//这说明了在初始化后期的第一次slidingwindow() marg掉的是old帧。
ROS_DEBUG("global SFM failed!");
marginalization_flag = MARGIN_OLD;
return false;
}sfm.construct() 这个函数就是对窗口中每个图像帧求解sfm问题，得到所有图像帧相对于参考帧C0的旋转四元数Q、平移向量T和特征点坐标(核心！)，
这一部分就不细说了，可以参考 高翔 14讲 或者下面的连接：
单目视觉（6）：SFM之矩阵估计 (五)_吕爽-CSDN博客 然后我们进行下一部分：
理论部分：
目的：利用SfM确定各个pose 和 特征点的相对于c0帧的位置关系；
这一部分和基于图像的三维重建比较像，可以用三角化和PnP把这一串的ck帧的位姿和特征点位置确定下来(特征点是伪深度)，在加上外参数qbc和pbc，一系列bk帧的位姿也确定下来。注意，这里把c0帧作为基础帧，实际上，c0帧旋转一下使gc0和gw方向一致时获得的坐标系就是vins的世界坐标系，也就是先验，这一部分也是很重要的部分。
先推导公式，所有帧的位姿(Rc0ck,qc0ck)表示相对于 第一帧相机坐标系(·)c0。相机到IMU的外参为(Rbc,qbc)，得到姿态从相机坐标系转换到IMU坐标系的关系。
经过 纯视觉SFM 后，Tc0_ck 求出来了，而 Tb_c 是知道的, 那我们根据上面的公式便可以求出 Tc0_bk, 后面我们就知道他的用途了！
将T展开有成R与p有：
左侧矩阵的两项写开：
（3）视觉/IMU 的联合初始化
第一部分：利用旋转求 IMU 的bias
理论部分
1、利用相机旋转约束标定IMU角速度bias，求解的目标函数如下公式所示：
这个目标函数是个 求和 的公式，所有程序里面需要遍历 all_image_frame，然后叠加A和b， 看程序就明白啦！
在SfM完成且外参数标定完之后，头两个值是已知的了，而且我们假设头两个值是准的。理想状态下，这三个数乘积应该是单位四元数，很可惜，第三个值是IMU预积分得到的，而预积分里面是有bias的。
所以，通过最小化这个目标函数的，可以把旋转bias标定出来！在IMU预积分部分（两帧间IMU的变化量，不是两个IMU采样时刻），有：
带入到损失函数里，可以得到：
或者是：
带入bias的残差后，得到，
实部没有需要标定的量，所以只用考虑虚部，也就是：
两侧再乘以 ，可以构造出 Ax=B 的形式，在采用LDLT分解，就可以求出状态量：
A x = B
疑问：为什么不像那种用高斯牛顿法迭求解，而更像是用直接法,也就是矩阵运算的方式来求待优化的状态量，也是一样的思路。
我的理解是：初始化只有一次，时间短，我们开始只是得到一个大致的值，后面还需要进行优化！
代具体代码见:initial_aligment.cpp 函数 solveGyroscopeBias()
void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
{
Matrix3d A;
Vector3d b;
Vector3d delta_bg;
A.setZero();
b.setZero();
map<double, ImageFrame>::iterator frame_i;//第i帧
map<double, ImageFrame>::iterator frame_j;//第j帧
for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
{
frame_j = next(frame_i);//j帧等于i帧的下一帧。
MatrixXd tmp_A(3, 3);
tmp_A.setZero();
VectorXd tmp_b(3);
tmp_b.setZero();
//视觉给出的i帧->j帧的旋转(四元数),对应公式(2)的右侧,公式(2)指的是上面给出注释的公式编号.
//qc0_bk' * qc0_bk+1.
Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
// IMU预积分中旋转对gyro bias的jacobian
tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
//对应公式(3)的右侧
tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
//对应公式(5)
A += tmp_A.transpose() * tmp_A;
b += tmp_A.transpose() * tmp_b;
}
//Cholesky分解求解线性方程，svd分解求解线性方程；Cholesky分解要求矩阵为对称且正定的。
delta_bg = A.ldlt().solve(b);
ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());
for (int i = 0; i <= WINDOW_SIZE; i++)
Bgs[i] += delta_bg;
//利用新的Bias重新repropagate。
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]);
//此处初始的加表零偏设置为0了，因为被重力吸收了，开始很小！
//这里为什么是第一帧的bias,滑窗内的bias都相同?
}
}/-----------------------------------------------------------------------------------------------程序----------------------------------------------------------------------------------------------/
思考：
第二部分：利用IMU的平移 初始化速度、重力和尺度因子
说明： 因为上一部分我们已经求出来IMU的bias,然后积分修正后这个平移是相对 准确的、可信
需要估计的变量
世界坐标系 w 下预积分量约束有：
将世界坐标系 w 换成相机初始时刻坐标系 C0 有
转变的方法：
w 下标 变成 C0; 需要注意的是 多了一个尺度因子 s 在位置上, 意思是世界坐标系中的位置单位是米，而C0坐标系下我们缺少尺度，所以用 s 表示。
疑问：为什么要把世界坐标系下的位姿转换到 C0坐标系下？
答：同学们，目前咱们处于初始化阶段呀，w坐标系我们不知道，只知道c0坐标系，所以需要把上面的公式转到c0坐标系上，咱们的目的就是去解决这个问题的，别把问题的因果关系混淆了！
上式中，等号左边减去等号右边就是残差，理想状态下，这个残差是0（因该是不完全相等，噪声的存在），这个值是不相等的,那么带入上式得。你可以参考上面求IMU bias的方法，我们要构造出一个AX = b的方程就可以了！
注意：这一步你可能看的不是很明白，你把前面的 式(3) 带到 上面这个公式就能看懂了！
把这个等式也转为 A x = b这种线性方程组的形式，如下式：
或者矩阵的形式：
对于beta而言，也是类似的，
那么，把这两个矩阵合体，就能得到论文中的等式： Hx = b
H x = b
同样采用LDLT分解，就能求出状态量：
代码部分：
(1)参数的传入和容器的定义
bool LinearAlignment(map<double, ImageFrame> &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<double, ImageFrame>::iterator frame_i;
map<double, ImageFrame>::iterator frame_j;
上式中，A和b对应的是Ax=b。注意，传入的参数是all_image_frame，不仅仅是滑窗内的帧。
frame_i和frame_j分别读取all_image_frame中的相邻两帧。
(2)套本小节的最后一个公式，构造Ax=b等式
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);
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, 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<double, 6, 6> cov_inv = Matrix<double, 6, 6>::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<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>();
}
这里加上了信息矩阵cov_inv，但是却是单位矩阵，有什么用呢？ 乘1000，除100的操作 ？另外需要注意的地方是，虽然公式中只计算了相邻2帧，但是代码中，它把所有帧都放进去了，因此，A矩阵和b向量是远大于公式中的尺寸，而且在代码中有一个叠加的操作！非线性优化里构造H矩阵和J矩阵也是一样的。
(3)ldlt分解，得到尺度和g的初始值，并用先验判断
x = A.ldlt().solve(b);
double s = x(n_state - 1) / 100.0;
g = x.segment<3>(n_state - 4);
if(fabs(g.norm() - G.norm()) > 1.0 || s < 0)
{//如果重力加速度与参考值差太大或者尺度为负则说明计算错误
return false;
}
(4) 利用gw的模长已知这个先验条件进一步优化gc0
RefineGravity(all_image_frame, g, x);
s = (x.tail<1>())(0) / 100.0;
(x.tail<1>())(0) = s;
ROS_DEBUG_STREAM(" refine " << g.norm() << " " << g.transpose());
if(s < 0.0 )
return false;
else
return true;
}
疑问: 为什么需要优化重力向量？
答：求解重力向量 g c 0 过程中,并没有加入模长限制 ||g c 0 ||= 9.81，三维变量 g c 0 实际只有两个自由度，多一个限制条件，得到的值会更加准确，这个不难理解吧！
疑问：g是3自由度的向量优化，但是实际上g只有2自由度，因为它的模长是已知的，就是怎么样用一个2自由度的表达式来表示3维的向量呢？
答：三维向量自由度为 2,可以采用球面坐标进行参数化，表达和示例图如下：
这个也不难理解吧！采用球面坐标进行参数化，也就是用g的模长（固定）作为半径画一个半球，上图蓝色线对应的是gc0的测量值的方向(也就是优化前的方向)，在这个交点上找到一个切平面，用gc0，b1，b2构造一个坐标系，那么在轴b1和b2上坐标值w1和w2就是我们需要求的量。求出来之后，把等号右边加在一起，就是优化后的gc0值。注意，b1的方向是由gc0的测量值的方向与[1,0,0]作叉乘得到的，b2的方向是由gc0的测量值的方向与b1作叉乘得到的。
这样 Hx = b 进行更新，此时带优化量减少了1维，如下：
同样使用LDLT分解：
则 优化重力向量 代码部分如下：
void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
Vector3d g0 = g.normalized() * G.norm();
Vector3d lx, ly;
//VectorXd x;
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<double, ImageFrame>::iterator frame_i;
map<double, ImageFrame>::iterator frame_j;
//进行4次迭代更新重力向量，为什么是4次？经验，4次后达到一定的精度。
for(int k = 0; k < 4; k++)
{
MatrixXd lxly(3, 2);
lxly = TangentBasis(g0);
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<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();
//cov.block<6, 6>(0, 0) = IMU_cov[i + 1];
//MatrixXd cov_inv = cov.inverse();
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>();
}
//这里求解的是重力向量的变化量，求解的是w1,w2。参考公式
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);
} //这里也没有给出收敛的条件，怎么保证迭代是正确的? //tl:循环4次
g = g0;
}子程序 TangentBasis( g0)
/*
求解重力向量在切平面上的两个分量。
g0：重力向量，根据公式来的！
*/
MatrixXd TangentBasis(Vector3d &g0)
{
Vector3d b, c;
Vector3d a = g0.normalized();
Vector3d tmp(0, 0, 1);
if(a == tmp)
tmp << 1, 0, 0;
//这个叉乘为什么搞得这么费劲，这个地方应该是a×tmp
b = (tmp - a * (a.transpose() * tmp)).normalized();//三个分量都有值
c = a.cross(b);
MatrixXd bc(3, 2);
bc.block<3, 1>(0, 0) = b;
bc.block<3, 1>(0, 1) = c;
return bc;
}第三部分：将相机坐标系对齐世界坐标系
分析：我们的坐标一定是要转换到世界坐标系下面的，这个没有什么异议！之前我们的坐标系都是参考相机的第一帧C0而以，并不是真正意义上的坐标系，也是无奈之举！
可是咱现在上面的g0已经得到优化了，你可能会问，这能干什么呀？ 你想一下，gc0已经求出来了，而gw一直就是一个已知的量（大小=9.81,方向 [0 0 1]），因此它们之间的夹角θ是能很快求出来的；
所以我们然后可以用gc0和gw作叉乘得到一个旋转轴u ( a、b向量叉✖的结果是一个与 a、b 同时垂直的向量)；最后把c0坐标系，绕着转轴旋转一个θ，就能找到c0 到 w 系的对齐关系，也就是Rwc0 = exp([θu])；
把所有 c0 坐标系下的变量旋转到 w系下，所有量都乘上Rwc0就可以了。我们定义的c0 与 w 系的原点坐标是重合的，把相机平移和特征点尺度恢复到米制单位，对齐不就行了嘛！具体理论如下：
对齐流程：
遗留问题：
问： 加速度 bias 为何没有估计?
答：初始化时间短，加速度 bias小 且与 重力融合，难以估计
其他初始化方法：
1 静止初始化:直接用加速度测量重力方向,初始速度为 0.
2 除 vins-mono 外的其他运动初始化方案 a , b 。
a
Janne Mustaniemi et al. “Inertial-based scale estimation for structure from motion on mobile devices”. In:
2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE. 2017, pp. 4394–4401.
b
Javier Domínguez-Conti et al. “Visual-Inertial SLAM Initialization: A General Linear Formulation and a
Gravity-Observing Non-Linear Optimization”. In: 2018 IEEE International Symposium on Mixed and Augmented
Reality (ISMAR). IEEE. 2018, pp. 37–45.
到此初始化内容结束，后面会接触优化部分！
1200
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
