视觉IMU紧耦合优化
深蓝学院VIO课程代码的学习总结 ，项目地址：
github-VINS-Course
solveOdometry()
void Estimator::solveOdometry()
void Estimator::solveOdometry()
{
if (frame_count < WINDOW_SIZE)
return;
if (solver_flag == NON_LINEAR)
{
TicToc t_tri;
// 首先三角化
f_manager.triangulate(Ps, tic, ric);
//cout << "triangulation costs : " << t_tri.toc() << endl;
backendOptimization();
}
}
可以看到三角化更新一些点后，就进入 backendOptimization() ，在这个函数中实现紧耦合优化的核心流程。
backendOptimization()
vector2double()
void Estimator::vector2double()
这个函数将滑动窗口的状态数据赋值给二维数组。这是因为ceres使用时指定的优化参数的形式是数组，这里将数据转换为数组以便之后使用ceres优化，虽然后面没有使用ceres，但是仍然保留这种做法。
参与优化的数组：
para_Pose[i]： 7维 ，保存滑窗每一帧位姿，前3维赋值 Ps[i] ，后4维赋值Rs[i]。
para_SpeedBias[i]: 9维， 依次保存滑窗每一帧的速度Vs[i]、Bas[i]、Bgs[i]。
para_Ex_Pose[i]：7维 ，保存每一个相机的外参，单目就只有一个相机啦，
para_Feature[i][]: 保存FeatureManager的feature容器中第i个特征点的逆深度。
para_Td[0][0]：保存IMU与图像时间戳的偏移。
problemSolve()
void Estimator::problemSolve()
1、将视觉IMU外参数节点 vertexExt 添加进图。
// 先把 外参数 节点加入图优化，这个节点在以后一直会被用到，所以我们把他放在第一个
shared_ptr<backend::VertexPose> vertexExt(new backend::VertexPose()); // 外参数节点也是pose节点
{
Eigen::VectorXd pose(7);
// 获取外参数值
pose << para_Ex_Pose[0][0], para_Ex_Pose[0][1], para_Ex_Pose[0][2], para_Ex_Pose[0][3], para_Ex_Pose[0][4], para_Ex_Pose[0][5], para_Ex_Pose[0][6];
vertexExt->SetParameters(pose); // 节点设置参数
// 如果不优化外参则fix
if (!ESTIMATE_EXTRINSIC) // 外参估计完后设置为1
{
//ROS_DEBUG("fix extinsic param");
// TODO:: set Hessian prior to zero
vertexExt->SetFixed(); // 固定
}
else{
//ROS_DEBUG("estimate extinsic param");
}
problem.AddVertex(vertexExt);
pose_dim += vertexExt->LocalDimension();
}
2、将滑窗内所有相机的pose节点 vertexCam 添加，以及将每一帧相机的速度偏置节点 vertexVB 添加 。
// 遍历滑动窗口 添加滑动窗口所有的节点 - 位姿、速度、偏置
for (int i = 0; i < WINDOW_SIZE + 1; i++)
{
shared_ptr<backend::VertexPose> vertexCam(new backend::VertexPose());
Eigen::VectorXd pose(7);
// 相机的Pose
pose << para_Pose[i][0], para_Pose[i][1], para_Pose[i][2], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5], para_Pose[i][6];
vertexCam->SetParameters(pose);
vertexCams_vec.push_back(vertexCam);
problem.AddVertex(vertexCam);
pose_dim += vertexCam->LocalDimension();
// 相机的速度、偏置节点
shared_ptr<backend::VertexSpeedBias> vertexVB(new backend::VertexSpeedBias());
Eigen::VectorXd vb(9);
vb << para_SpeedBias[i][0], para_SpeedBias[i][1], para_SpeedBias[i][2],
para_SpeedBias[i][3], para_SpeedBias[i][4], para_SpeedBias[i][5],
para_SpeedBias[i][6], para_SpeedBias[i][7], para_SpeedBias[i][8];
vertexVB->SetParameters(vb);
vertexVB_vec.push_back(vertexVB);
problem.AddVertex(vertexVB);
pose_dim += vertexVB->LocalDimension();
}
3、为滑窗1-11帧添加预积分约束边，该边为四元边。
// IMU 添加IMU约束边
for (int i = 0; i < WINDOW_SIZE; i++)
{
int j = i + 1;
// 这个条件在什么时候不会满足 ？？？？？
if (pre_integrations[j]->sum_dt > 10.0)
continue;
// IMU对节点的约束 传入预积分量
std::shared_ptr<backend::EdgeImu> imuEdge(new backend::EdgeImu(pre_integrations[j]));
// 有多个顶点则要创建一个容器 将顶点放入
std::vector<std::shared_ptr<backend::Vertex>> edge_vertex;
// 为边添加顶点 改变连接第i - j个顶点
edge_vertex.push_back(vertexCams_vec[i]);
edge_vertex.push_back(vertexVB_vec[i]);
edge_vertex.push_back(vertexCams_vec[j]);
edge_vertex.push_back(vertexVB_vec[j]);
imuEdge->SetVertex(edge_vertex); // 顶点存放与边的 verticies_
problem.AddEdge(imuEdge);
}
4、遍历 FeatureManager 的 feature容器中所有的特征点，对于每个合法的特征点，将其起始观测帧的深度构建逆深度节点，然后遍历该特征点所有的观测，对除起始帧之外的帧构建重投影边 EdgeReprojection ，该边是一个4元边，连接 逆深度节点、相机节点i、相机节点j, 外参数节点 , 添加信息矩阵和鲁棒核函数后加入图优化。
// Visual Factor
vector<shared_ptr<backend::VertexInverseDepth>> vertexPt_vec;
{
int feature_index = -1;
// 遍历每一个特征
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;
++feature_index;
// imu_i 从起始帧开始
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
Vector3d pts_i = it_per_id.feature_per_frame[0].point; // 起始帧的归一化坐标
// 逆深度节点
shared_ptr<backend::VertexInverseDepth> verterxPoint(new backend::VertexInverseDepth());
VecX inv_d(1);
inv_d << para_Feature[feature_index][0]; // 获取该特征点的逆深度
verterxPoint->SetParameters(inv_d);
problem.AddVertex(verterxPoint); // 添加顶点
vertexPt_vec.push_back(verterxPoint);
// 遍历该特征点的所有观测,对不是该特征点起始观测帧的帧构造重投影边
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
if (imu_i == imu_j)
continue;
// 获得改观测的归一化坐标
Vector3d pts_j = it_per_frame.point;
// 创建重投影边 输入该特征点在其起始帧观测的结果以及第j帧的观测结果
std::shared_ptr<backend::EdgeReprojection> edge(new backend::EdgeReprojection(pts_i, pts_j));
std::vector<std::shared_ptr<backend::Vertex>> edge_vertex;
// 添加该边的顶点 4元边
edge_vertex.push_back(verterxPoint); // 逆深度节点
edge_vertex.push_back(vertexCams_vec[imu_i]); // 相机节点i
edge_vertex.push_back(vertexCams_vec[imu_j]); // 相机节点j
edge_vertex.push_back(vertexExt); //外参数节点
edge->SetVertex(edge_vertex);
// 信息矩阵 project_sqrt_info_ = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
edge->SetInformation(project_sqrt_info_.transpose() * project_sqrt_info_);
// 由于相机观测可能有外点 采用鲁棒核函数
edge->SetLossFunction(lossfunction);
// 添加边
problem.AddEdge(edge);
}
}
}
5、处理先验， 如果 Hprior_ 非空：
将 Hprior_ 赋值给problem类的H_prior_ ;
将 bprior_赋值给problem类的b_prior_ ;
将 errprior_赋值给problem类的err_prior_ ;
将 Jprior_inv_赋值给problem类的Jt_prior_inv_ ;
扩大 H_prior_ 、b_prior_的维度 + 15;
先验信息是关于Pose的，没有landmark的部分。
6、进行求解 problem.Solve(10)， 迭代次数10，迭代过程中需要不断更新先验残差b_prior_， 细节稍后解析。
7、优化后更新Estimator类的先验残差 bprior_、errprior_， 获取problem类的 b_prior_ 赋值给 bprior_ ，获取 problem类的err_prior_赋值给 errprior_，Hprior_ 不需要更新 因为被marg掉的节点的观测信息都被丢弃了。
8、更新状态，读取图优化节点的状态赋值给 para_Pose、para_SpeedBias、para_Feature。
// update parameter 更新滑动窗口全部状态
for (int i = 0; i < WINDOW_SIZE + 1; i++)
{ // 获取该帧的pos节点的状态
VecX p = vertexCams_vec[i]->Parameters();
for (int j = 0; j < 7; ++j)
{
para_Pose[i][j] = p[j];
}
// 获取该帧的速度偏置节点的状态
VecX vb = vertexVB_vec[i]->Parameters();
for (int j = 0; j < 9; ++j)
{
para_SpeedBias[i][j] = vb[j];
}
}
// 遍历每一个特征
for (int i = 0; i < vertexPt_vec.size(); ++i)
{ // 获取逆深度节点的状态
VecX f = vertexPt_vec[i]->Parameters();
para_Feature[i][0] = f[0];
}
优化求解 Solve()
bool Problem::Solve(int iterations)
附 LM算法流程：
如何添加FEJ？？？？？？？
1、void Problem::SetOrdering()
(1)、求得图优化中优化变量的总维度 ordering_generic_，pose节点(包括速度偏置节点)的总维度 ordering_poses_，landmark节点的总维度ordering_landmarks_ 。
(2)、遍历 verticies_,依次设置每个节点的 ordering_id_参数，表示该节点的状态变量在其相同类别的节点状态变量(Pose节点或landmark节点)的排序，这个变量也将决定节点的jacobian在H和b矩阵中的位置。 注意，verticies_是一个有序的map，内部节点的顺序依据节点的id从小到大排列，而节点间id大小取决于节点间创建的先后顺序，越早创建则id越小，因此 verticies_中节点的顺序应该是 外参Pose节点，IMUpose节点与速度偏置节点交替？, 逆深度节点。
(3)、将所有的Pose节点放置于map idx_pose_vertices_中，将所有的landmark节点放置与
map idx_landmark_vertices_中，key为节点的id。
(4)、每个landmark节点的ordering_id_都增加ordering_poses_，使得每个landmark节点的状态变量排列在Pose节点之后。
疑问： Pose节点与速度偏置节点交替排列 ？？
2、void Problem::MakeHessian()
遍历当前添加的全部残差边，求出每条边关于节点状态的jacobian，构造出 Hessian_、b_矩阵。 构造完了后，再将之前的先验添加，如下:
// 如果先验存在 .....
if(H_prior_.rows() > 0)
{
MatXX H_prior_tmp = H_prior_;
VecX b_prior_tmp = b_prior_;
/// 遍历所有 POSE 顶点，将需要fix的节点 设置相应的先验维度为 0 .但是 除了外参节点可能fix 其他的节点都没有fix
for (auto vertex: verticies_) {
// Pose节点且Fix 因为先验信息不包含landmark 所以只考虑Pose
if (IsPoseVertex(vertex.second) && vertex.second->IsFixed() ) {
int idx = vertex.second->OrderingId();
int dim = vertex.second->LocalDimension();
H_prior_tmp.block(idx,0, dim, H_prior_tmp.cols()).setZero();
H_prior_tmp.block(0,idx, H_prior_tmp.rows(), dim).setZero();
b_prior_tmp.segment(idx,dim).setZero();
// std::cout << " fixed prior, set the Hprior and bprior part to zero, idx: "<<idx <<" dim: "<<dim<<std::endl;
}
}
// Hessian_ Pose节点的部分叠加先验 目前先验保存的内容是关于0-10帧的 扩充的15维 为 0
Hessian_.topLeftCorner(ordering_poses_, ordering_poses_) += H_prior_tmp;
b_.head(ordering_poses_) += b_prior_tmp;
}
由于先验矩阵 扩维了15，所以维度等于ordering_poses_，所以先验直接叠加到Hessian_、b_的左上角
最终获得添加了先验信息的 Hessian_，b_ 矩阵。
3、进行迭代求解
其中：
(1)、SolveLinearSystem() 该函数求解出状态的优化增量 delta_x_，采取先边缘化landmark状态，求解出Pose 状态增量 delta_x_pp ，再反求出landmark的状态增量 delta_x_ll，最后合并获得最终的 delta_x_ 。
(2)、UpdateStates() 在求解出delta_x_后对各个节点的状态进行更新，同时随着状态的变化，先验 b_prior_ 也要变化！！！
// update prior 先验的信息矩阵虽然不变，但是随着优化将状态不断的改变，先验的残差需要跟随变化
if (err_prior_.rows() > 0) {
// 记录当前的先验误差 如果本次迭代无效 则要通过这个变量 恢复之前的先验误差
b_prior_backup_ = b_prior_;
err_prior_backup_ = err_prior_;
// b_prior_ = b_prior_ - H_prior_*dx
// H_prior_ 扩维了15
b_prior_ -= H_prior_ * delta_x_.head(ordering_poses_); // update the error_prior
// b_prior_ = -J^T*error => error = -J^-T*b_prior_
err_prior_ = -Jt_prior_inv_ * b_prior_.head(ordering_poses_ - 15); // Jt_prior_inv_没扩维 所以要-15
}
先验 b_prior_ 如下所示:
其中J为残差关于状态的jacobian, 由于残差已经被丢弃所以无法更新.
P为观测量的协方差矩阵, 也无法更新.
而rB为残差, 优化迭代时, 状态被更新, 所以这个残差也可以更新,
因此总体的更新可以写成下式子:
状态更新后，IsGoodStepInLM()判断迭代是否有效，如果有效，则重新构造 MakeHessian(),此时在新的状态点计算jacobian以及残差。如果无效，则恢复原来的状态RollbackStates()，改变阻尼系数重新计算SolveLinearSystem()，直到优化完成。
while (!stop && (iter < iterations)) {
std::cout << "iter: " << iter << " , chi= " << currentChi_ << " , Lambda= " << currentLambda_ << std::endl;
bool oneStepSuccess = false;
int false_cnt = 0;
while (!oneStepSuccess && false_cnt < 10) // 不断尝试 Lambda, 直到成功迭代一步
{
// 第四步，解线性方程 获得 增量 delta_x_
SolveLinearSystem();
// 优化退出条件1： delta_x_ 很小则退出
// if (delta_x_.squaredNorm() <= 1e-6 || false_cnt > 10)
// TODO:: 退出条件还是有问题, 好多次误差都没变化了，还在迭代计算，应该搞一个误差不变了就中止
// if ( false_cnt > 10)
// {
// stop = true;
// break;
// }
// 更新状态量
UpdateStates();
// 判断当前步是否可行以及 LM 的 lambda 怎么更新, chi2 也计算一下
oneStepSuccess = IsGoodStepInLM();
// 后续处理，
if (oneStepSuccess) {
// 在新线性化点 构建 hessian
MakeHessian();
false_cnt = 0;
} else {
false_cnt ++;
RollbackStates(); // 误差没下降，回滚
}
}
iter++;
// 前后两次的误差已经不再变化
if(last_chi_ - currentChi_ < 1e-5)
{
std::cout << "sqrt(currentChi_) <= stopThresholdLM_" << std::endl;
stop = true;
}
last_chi_ = currentChi_; // 记录本次总残差
}
double2vector()
void Estimator::double2vector()
去除求解的状态在零空间上的变化。
边缘化
边缘化有两种方式：
一、marg掉最老帧。 二、marg掉次新帧。 如下：
// 如果是marg掉最后一帧
if (marginalization_flag == MARGIN_OLD)
{
vector2double();
MargOldFrame();
}
else
{ // marg掉次新帧
if (Hprior_.rows() > 0)
{
vector2double();
MargNewFrame();
}
}
MargOldFrame()
void Estimator::MargOldFrame()
功能：marg掉最后一帧。
说明： 当次新帧为关键帧，我们marg掉最老帧。目的是将最老帧相关的约束，转移到其他的帧，所以首先我们需要获取最老帧的全部约束即边，也就是预积分边与重投影边，然后将这些观测构建出信息矩阵，最后marg掉最老帧节点以及最老帧相连接的landmark节点得到先验信息矩阵，这样就完成了将最老帧相关的约束转移给了滑动窗口其他帧。
流程：
0、构建一个图优化Problem对象，注意后面添加的顶点、边都是添加到这个problem里面。
backend::Problem problem(backend::Problem::ProblemType::SLAM_PROBLEM);
1、添加外参数Pose节点。
2、将滑动窗口所有帧的Pose节点与速度偏置节点添加。
3、添加第0个节点到第1个节点的IMU预积分约束边。
4、所有被第0帧观测到的特征点构建逆深度节点，第0帧与其他观测到该特征点的帧构建重投影残差边。
5、如果存在老先验矩阵，老先验矩阵也应该参与marg，即将老先验继续传递，老先验矩阵的维度只包含10帧的Pose节点以及外参节点，此时需要将老先验矩阵扩维15，这样才能和包含11帧Pose节点的信息矩阵叠加, 老先验的处理如下：
// 老先验矩阵如果存在 则也要参与marg 即将之前的先验信息继续传递
{
// 已经有 Prior 了 那么marg之前需要将这个先验信息矩阵添加
if (Hprior_.rows() > 0)
{
problem.SetHessianPrior(Hprior_); // 告诉这个 problem H_prior_
problem.SetbPrior(bprior_);
problem.SetErrPrior(errprior_);
problem.SetJtPrior(Jprior_inv_);
//因为 老先验信息矩阵只包含10帧的节点 而添加老先验矩阵时 当时的矩阵包含11帧的节点 所以这里要扩维15
problem.ExtendHessiansPriorSize(15);
}
else
{ // 第一次marg 则没有先验信息矩阵
// 构造先验信息矩阵 维度为MargOldFrame（）中添加的全部pose节点维度
Hprior_ = MatXX(pose_dim, pose_dim);
Hprior_.setZero();
// 构造先验残差 维度为MargOldFrame（）中添加的全部pose节点维度
bprior_ = VecX(pose_dim);
bprior_.setZero();
problem.SetHessianPrior(Hprior_); // 告诉这个 problem 设置 problem H_prior_
problem.SetbPrior(bprior_); // 设置 problem b_prior_
}
}
6、 执行marg - Marginalize(), marg的节点是最老帧的Pose 以及V bias节点以及与最老帧连接的landmark节点 , 注意要叠加老先验矩阵, Marginalize()的细节在下。
7、获得marg后的先验信息，赋值给 Hprior_、bprior_、errprior_、Jprior_inv_。
MargNewFrame()
marg掉窗口次新帧，marg次新帧和marg最老帧有很大不同，它不会添加次新帧的任何的边来构建信息矩阵，，而是直接将原有的先验信息矩阵进行marg，因为，次新帧与最新帧的观测很相似，直接丢弃不会损失太多的约束。
0、构建一个图优化Problem对象，注意后面添加的顶点、边都是添加到这个problem里面。
backend::Problem problem(backend::Problem::ProblemType::SLAM_PROBLEM);
1、添加外参数Pose节点。
2、将滑动窗口所有帧的Pose节点与速度偏置节点添加。
3、处理老先验矩阵。
// 先验
{
// 已经有 Prior 了
if (Hprior_.rows() > 0)
{
problem.SetHessianPrior(Hprior_); // 告诉这个 problem
problem.SetbPrior(bprior_);
problem.SetErrPrior(errprior_);
problem.SetJtPrior(Jprior_inv_);
problem.ExtendHessiansPriorSize(15); // 但是这个 prior 还是之前的维度，需要扩展下装新的pose
}
else
{
Hprior_ = MatXX(pose_dim, pose_dim);
Hprior_.setZero();
bprior_ = VecX(pose_dim);
bprior_.setZero();
}
}
4、 Marginalize()，marg掉窗口次新帧的Pose与速度偏置节点，这里marg只是将老先验信息矩阵添加，并对其操作。
5、获得marg后的先验信息，赋值给 Hprior_、bprior_、errprior_、Jprior_inv_。
Marginalize()
这个函数是边缘化的核心！最终求得先验H_prior_、b_prior_、err_prior_， 该函数流程如下：
1、SetOrdering() 对当前Problem中的节点状态变量进行排列，为构建信息矩阵做准备。
2、如果marg最老帧，则获取与最老帧Pose节点连接的所有边，遍历这些边构造出信息矩阵。
/// 找到第0帧的所有约束 - 连接的预积分边与重投影边
std::vector<shared_ptr<Edge>> marg_edges = GetConnectedEdges(margVertexs[0]);
// std::cout << "pose dim: " << pose_dim <<std::endl;
int cols = ordering_generic_; // marg的矩阵维度为当前所有pose节点与landmark节点的维度总和
/// 构建误差 H 矩阵 H = H_marg + H_pp_prior
MatXX H_marg(MatXX::Zero(cols, cols));
VecX b_marg(VecX::Zero(cols));
int ii = 0;
// 遍历每一条边 构建H矩阵
for (auto edge: marg_edges) {
edge->ComputeResidual();
edge->ComputeJacobians();
auto jacobians = edge->Jacobians();
auto verticies = edge->Verticies();
ii++;
assert(jacobians.size() == verticies.size());
// 该边的顶点
for (size_t i = 0; i < verticies.size(); ++i) {
auto v_i = verticies[i];
auto jacobian_i = jacobians[i];
ulong index_i = v_i->OrderingId();
ulong dim_i = v_i->LocalDimension();
double drho;
MatXX robustInfo(edge->Information().rows(),edge->Information().cols());
edge->RobustInfo(drho,robustInfo);
for (size_t j = i; j < verticies.size(); ++j) {
auto v_j = verticies[j];
auto jacobian_j = jacobians[j];
ulong index_j = v_j->OrderingId();
ulong dim_j = v_j->LocalDimension();
// Jt W J
MatXX hessian = jacobian_i.transpose() * robustInfo * jacobian_j;
assert(hessian.rows() == v_i->LocalDimension() && hessian.cols() == v_j->LocalDimension());
// 所有的信息矩阵叠加起来
H_marg.block(index_i, index_j, dim_i, dim_j) += hessian;
if (j != i) {
// 对称的下三角
H_marg.block(index_j, index_i, dim_j, dim_i) += hessian.transpose();
}
}
b_marg.segment(index_i, dim_i) -= drho * jacobian_i.transpose() * edge->Information() * edge->Residual();
}
}
3、如果marg最老帧，则先marg掉landmark节点，图模型添加的landmark节点全部是与最老帧pose节点相连接的。
/// marg landmark
int reserve_size =ordering_poses_; // marg掉landmark后 剩下pose节点
if (ordering_landmarks_ > 0) {
int marg_size = ordering_landmarks_;
/*
* | Hpp Hpm |
* | Hmp Hmm |
*/
MatXX Hmm = H_marg.block(reserve_size, reserve_size, marg_size, marg_size);
MatXX Hpm = H_marg.block(0, reserve_size, reserve_size, marg_size);
MatXX Hmp = H_marg.block(reserve_size, 0, marg_size, reserve_size);
VecX bpp = b_marg.segment(0, reserve_size);
VecX bmm = b_marg.segment(reserve_size, marg_size);
// Hmm 是对角线矩阵，它的求逆可以直接为对角线块分别求逆，如果是逆深度，对角线块为1维的，则直接为对角线的倒数，这里可以加速
MatXX Hmm_inv(MatXX::Zero(marg_size, marg_size));
// TODO:: use openMP
for (auto iter: idx_landmark_vertices_) {
int idx = iter.second->OrderingId() - reserve_size;
// int size = iter.second->LocalDimension(); // 1
// Hmm_inv.block(idx, idx, size, size) = Hmm.block(idx, idx, size, size).inverse();
Hmm_inv(idx,idx) = 1/Hmm(idx,idx); // 直接倒数
}
// Hpp - Hpm*Hmm^-1*Hmp
MatXX tempH = Hpm * Hmm_inv;
MatXX Hpp = H_marg.block(0, 0, reserve_size, reserve_size) - tempH * Hmp;
bpp = bpp - tempH * bmm;
H_marg = Hpp;
b_marg = bpp;
}
4、老先验信息矩阵的处理，和老先验信息矩阵叠加，老先验不能丢，叠加后参与marg变成新的先验继续用。
// 添加老先验信息矩阵
if(H_prior_.rows() > 0)
{
H_marg += H_prior_; // H_prior_ 之前扩维了15 现在和H_marg维度相同了
b_marg += b_prior_;
}
5、marg掉需要marg的frame以及V，bias节点 。 首先将需要marg的节点移动到最后(更新H矩阵与b 矩阵) ，接着同时marg掉这些节点，得到 H_prior_、b_prior_ 矩阵。
6、对 H_prior_ 进行特征分解 H_prior_ = USUt ，求得先验残差的jacobian矩阵 , 然后求得先验残差 err_prior_。
// 对 H_prior_ 进行特征分解 H_prior_ = U*S*Ut
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(H_prior_);
Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0)); // S 获取特征值 小于eps的赋值0
Eigen::VectorXd S_inv = Eigen::VectorXd(
(saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0)); // S^-1
// H_prior_ = U*S*Ut = JtJ => J = sqrt(S)*Ut
Eigen::VectorXd S_sqrt = S.cwiseSqrt(); // sqrt(S)
Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt(); // sqrt(S^-1)
Jt_prior_inv_ = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose(); // sqrt(S^-1)*Ut
// 先验残差 因为 b_prior_ = -Jt*e => e = -Jt^-1*b_prior_ = -sqrt(S^-1)*Ut*b_prior_
err_prior_ = -Jt_prior_inv_ * b_prior_; // 在计算阻尼系数时使用
// J = sqrt(S)*Ut
MatXX J = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();
H_prior_ = J.transpose() * J; // JtJ = U*S*UT
MatXX tmp_h = MatXX( (H_prior_.array().abs() > 1e-9).select(H_prior_.array(),0) ); // H_prior_小于1e-9的元素直接赋0
H_prior_ = tmp_h;
原理如下：
疑问：
1、err_prior_ 有什么用？？？？？
滑动窗口
Vins中滑动窗口的函数是void Estimator::slideWindow()，边缘化后，我们获取了marg后的节点的先验信息矩阵，接着为了滑动窗口接受新的帧，我们需要将marg掉的帧的信息移除。移除的方式和marg的策略有关。
重点问题
1、进行优化时构造最小二乘信息矩阵H ，先验信息矩阵是如何融合的？ 每一次优化需要迭代很多步，每迭代一次H矩阵就要重新构建一次，这时先验信息矩阵如何处理？
答：
在void Estimator::problemSolve()中，如果有先验信息矩阵，则将其放置到problem类中，并扩维15
// 先验
{
// 已经有 Prior 了 在首次marg之前为空
if (Hprior_.rows() > 0)
{
// 外参数先验设置为 0. TODO:: 这个应该放到 solver 里去弄
// Hprior_.block(0,0,6,Hprior_.cols()).setZero();
// Hprior_.block(0,0,Hprior_.rows(),6).setZero();
problem.SetHessianPrior(Hprior_); // 告诉这个 problem 设置为 H_prior_
problem.SetbPrior(bprior_); // b_prior_
problem.SetErrPrior(errprior_); // err_prior_
problem.SetJtPrior(Jprior_inv_); // Jt_prior_inv_
// 扩大 H_prior_ 、b_prior_的维度 + 15 先验信息矩阵由于marg掉了一帧 只包含10帧的维度 需要扩展下装新的pose
problem.ExtendHessiansPriorSize(15);
}
}
然后进入 problem.Solve(), 在MakeHessian()中添加先验信息矩阵，将先验信息矩阵添加到左上角
// 如果先验存在 .....
if(H_prior_.rows() > 0)
{
MatXX H_prior_tmp = H_prior_;
VecX b_prior_tmp = b_prior_;
/// 遍历所有 POSE 顶点，将需要fix的节点 设置相应的先验维度为 0 . 但是 除了外参节点可能fix 其他的节点都没有fix
for (auto vertex: verticies_) {
// Pose节点且Fix 因为先验信息不包含landmark 所以只考虑Pose
if (IsPoseVertex(vertex.second) && vertex.second->IsFixed() ) {
int idx = vertex.second->OrderingId();
int dim = vertex.second->LocalDimension();
H_prior_tmp.block(idx,0, dim, H_prior_tmp.cols()).setZero();
H_prior_tmp.block(0,idx, H_prior_tmp.rows(), dim).setZero();
b_prior_tmp.segment(idx,dim).setZero();
// std::cout << " fixed prior, set the Hprior and bprior part to zero, idx: "<<idx <<" dim: "<<dim<<std::endl;
}
}
// Hessian_ Pose节点的部分叠加先验 目前先验保存的内容是关于0-10帧的 扩充了为 0的15维
// 注意这里 Hessian_ 包含的维度有Pose与landmark 而先验只有Pose的 所以取左上部分
Hessian_.topLeftCorner(ordering_poses_, ordering_poses_) += H_prior_tmp;
b_.head(ordering_poses_) += b_prior_tmp;
}
每次迭代时，要随着状态的更新同步更新先验残差 b_prior_ ，但是H_prior 不用变化。
2、每次marg的时候，对于旧先验信息矩阵是怎样处理的？
新的先验信息矩阵先marg掉landmark的部分，然后老先验信息矩阵扩维之后与新先验信息矩阵叠加。
3、非线性优化时，如果要使某一节点状态变量保持不变应该要怎么做，为什么？
4、什么是FEJ，如何在VINS中添加FEJ？
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
