目录
基本流程
1. 判断次新帧是否是关键帧，从而确定边缘化选项
边缘项包括(MARGIN_OLD / MARGIN_SECOND_NEW）
// 判断次新帧是否是关键帧
if (f_manager.addFeatureCheckParallax(frame_count, image, td))
marginalization_flag = MARGIN_OLD; // 是关键帧，marg掉老帧
else
marginalization_flag = MARGIN_SECOND_NEW; // 不是关键帧，marg掉前一帧2. 如果是MARGIN_OLD，就要边缘化掉老帧
边缘化掉最老帧以及其看到的路标点和IMU数据，转化为先验。
对应的代码为：
if (marginalization_flag == MARGIN_OLD)
{
vector2double(); // 系统数据类型转换：将向量变成double数组指针的形式
MargOldFrame();
...
}3.当次新帧不是关键帧时，边缘化次新帧
保留次新帧的IMU测量，丢弃该帧的视觉测量，将上一次先验残差项传递给marginalization_info
此时，marginalization_flag = MARGIN_SECOND_NEW；
else
{
if (Hprior_.rows() > 0)
{
vector2double();
MargNewFrame();
...
}
}4. 详细代码细节
① vector2double()：
此函数将向量转变成数组指针的形式，生成的优化变量包括：
para_Pose（6维，相机位姿）、
para_SpeedBias（9维，相机速度、加速度偏置、角速度偏置）、
para_Ex_Pose（6维、相机IMU外参）、
para_Feature（1维，特征点深度）、
para_Td（1维，标定同步时间）
在后面进行边缘化操作时这些优化变量都是当做整体看待。
② MargOldFrame()边缘化老帧操作
1. 构建problem
首先构建problem，依次加入外参数节点，IMU信息的相关节点，视觉节点和约束边，设置先验信息，然后设置需要边缘化掉的节点，进行边缘化操作
void Estimator::MargOldFrame()
{
backend::LossFunction *lossfunction;
lossfunction = new backend::CauchyLoss(1.0);
// step1. 构建 problem
backend::Problem problem(backend::Problem::ProblemType::SLAM_PROBLEM);
vector<shared_ptr<backend::VertexPose>> vertexCams_vec;
vector<shared_ptr<backend::VertexSpeedBias>> vertexVB_vec;
int pose_dim = 0;1.1 先把外参数节点加入图优化
这个节点在以后一直会被用到，所以我们把他放在第一个
shared_ptr<backend::VertexPose> vertexExt(new backend::VertexPose());
{
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]; // R,t
vertexExt->SetParameters(pose); // 设置外参数值
problem.AddVertex(vertexExt);
pose_dim += vertexExt->LocalDimension(); // 外参数化维度
}1.2 再把滑窗内每一帧的P.Q.V.bias 加入图优化
for (int i = 0; i < WINDOW_SIZE + 1; i++)
{
// 滑动窗口内每一帧的位姿，PQ
shared_ptr<backend::VertexPose> vertexCam(new backend::VertexPose());
Eigen::VectorXd pose(7);
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);// R,t
problem.AddVertex(vertexCam);
pose_dim += vertexCam->LocalDimension();
// 滑窗内每一帧的IMU 的速度 加速度、角速度的偏置
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(); // 两部分看成一个整体
}
1.3 IMU 初始时刻，从第一帧积起来的第二帧时刻的IMU预积分
{
if (pre_integrations[1]->sum_dt < 10.0)
{
std::shared_ptr<backend::EdgeImu> imuEdge(new backend::EdgeImu(pre_integrations[1]));
std::vector<std::shared_ptr<backend::Vertex>> edge_vertex;
edge_vertex.push_back(vertexCams_vec[0]);
edge_vertex.push_back(vertexVB_vec[0]);
edge_vertex.push_back(vertexCams_vec[1]);
edge_vertex.push_back(vertexVB_vec[1]);
imuEdge->SetVertex(edge_vertex);
problem.AddEdge(imuEdge);
}
} 1.4 视觉因子的处理
包括获取每个特征的逆深度以及对于与同一个路标点共视的所有帧，计算第一帧(最老帧)与各帧之间的重投影误差、信息矩阵…
{
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;
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
if (imu_i != 0)
continue;
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);
// 遍历所有的观测
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;
// 对于与同一个路标点共视的所有帧，计算第一帧(最老帧)与各帧之间的重投影误差
std::shared_ptr<backend::EdgeReprojection> edge(new backend::EdgeReprojection(pts_i, pts_j));
std::vector<std::shared_ptr<backend::Vertex>> edge_vertex;
edge_vertex.push_back(verterxPoint); // 逆深度
edge_vertex.push_back(vertexCams_vec[imu_i]); // 两帧的PQ
edge_vertex.push_back(vertexCams_vec[imu_j]);
edge_vertex.push_back(vertexExt); // 外参
edge->SetVertex(edge_vertex);
edge->SetInformation(project_sqrt_info_.transpose() * project_sqrt_info_);
edge->SetLossFunction(lossfunction);
problem.AddEdge(edge);
}
}
}1.5 先验
{
// 已经有 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();
problem.SetHessianPrior(Hprior_); // 告诉这个 problem
problem.SetbPrior(bprior_);
}
}2. 传入要边缘化掉的最老帧信息,进行边缘化
std::vector<std::shared_ptr<backend::Vertex>> marg_vertex;
// 2 传入要边缘化掉的最老帧信息,进行边缘化
marg_vertex.push_back(vertexCams_vec[0]); // //这里的0即为最老帧对应的索引 // PQ
marg_vertex.push_back(vertexVB_vec[0]); // V,bias
problem.Marginalize(marg_vertex, pose_dim);// 边缘化处理 边 缘 化 处 理 ！ ！ ！ 重 点 ！ ！ ！ \red{边缘化处理！！！重点！！！} 边缘化处理！！！重点！！！
传入要边缘化的顶点和所有状态向量的维度和。
bool Problem::Marginalize(const std::vector<std::shared_ptr<Vertex> > margVertexs, int pose_dim);// 边缘化处理 3. 先验信息更新
Hprior_ = problem.GetHessianPrior();
bprior_ = problem.GetbPrior();
errprior_ = problem.GetErrPrior();
Jprior_inv_ = problem.GetJtPrior();③ MargNewFrame()边缘化次新帧操作
1. 构建problem
没有加入视觉约束边，以及IMU预积分
2.传入要边缘化的次新帧信息，进行边缘化
std::vector<std::shared_ptr<backend::Vertex>> marg_vertex;
// 把窗口倒数第二个帧 marg 掉
marg_vertex.push_back(vertexCams_vec[WINDOW_SIZE - 1]);//注意这里的索引，次新帧
marg_vertex.push_back(vertexVB_vec[WINDOW_SIZE - 1]);
problem.Marginalize(marg_vertex, pose_dim);3. 先验信息更新
同上，略。
边缘化Marginalize()代码解析
step1. SetOrdering()
重新计算所有要优化变量的总维度，若是SLAM问题则需要分别统计pose和landmark的维数。
void Problem::SetOrdering() {
// 每次重新计数
ordering_poses_ = 0;
ordering_generic_ = 0;
ordering_landmarks_ = 0;
// Note:: verticies_ 是 map 类型的, 顺序是按照 id 号排序的
for (auto vertex: verticies_) {
ordering_generic_ += vertex.second->LocalDimension(); // 所有的优化变量总维数
if (problemType_ == ProblemType::SLAM_PROBLEM) // 如果是 slam 问题，还要分别统计 pose 和 landmark 的维数，后面会对他们进行排序
{
AddOrderingSLAM(vertex.second);
}
}
if (problemType_ == ProblemType::SLAM_PROBLEM) {
// 这里要把 landmark 的 ordering 加上 pose 的数量，就保持了 landmark 在后,而 pose 在前
ulong all_pose_dimension = ordering_poses_;
for (auto landmarkVertex : idx_landmark_vertices_) {
landmarkVertex.second->SetOrderingId(
landmarkVertex.second->OrderingId() + all_pose_dimension
);
}
}
// CHECK_EQ(CheckOrdering(), true);
}step2. GetConnectedEdges(margVertexs[0])
找到所有与最老帧相关联的边，包括与路标点(共视关系)和IMU(预积分)的。
std::vector<shared_ptr<Edge>> marg_edges = GetConnectedEdges(margVertexs[0]);详细代码为：
vector<shared_ptr<Edge>> Problem::GetConnectedEdges(std::shared_ptr<Vertex> vertex) {
vector<shared_ptr<Edge>> edges;
auto range = vertexToEdge_.equal_range(vertex->Id());
for (auto iter = range.first; iter != range.second; ++iter) {
// 并且这个edge还需要存在，而不是已经被remove了
if (edges_.find(iter->second->Id()) == edges_.end())
continue;
edges.emplace_back(iter->second);
}
return edges;
}range.first是与该顶点第一个关联的边，range.second是最后一个关联的边？？
step3. 重新设置路标点的顺序
对于marg_edges所关联的特征，重新设定其顺序：pose_dim + marg_landmark_size，这里的pose_dim是增加了所有节点后的维度。
std::unordered_map<int, shared_ptr<Vertex>> margLandmark;
// 构建 Hessian 的时候 pose 的顺序不变，landmark的顺序要重新设定
int marg_landmark_size = 0;
// std::cout << "\n marg edge 1st id: "<< marg_edges.front()->Id() << " end id: "<<marg_edges.back()->Id()<<std::endl;
for (size_t i = 0; i < marg_edges.size(); ++i) {
// std::cout << "marg edge id: "<< marg_edges[i]->Id() <<std::endl;
auto verticies = marg_edges[i]->Verticies(); // 与要边缘化边相关联的顶点
for (auto iter : verticies) {
// 是与最老边关联(共视)的路标点？？对路标点重新排序--放在pose后面
if (IsLandmarkVertex(iter) && margLandmark.find(iter->Id()) == margLandmark.end()) { // end()指向map的最后一个元素的
iter->SetOrderingId(pose_dim + marg_landmark_size);
margLandmark.insert(make_pair(iter->Id(), iter));
marg_landmark_size += iter->LocalDimension();
}
}
}step4. 构建H矩阵和b矩阵
int cols = pose_dim + marg_landmark_size;
/// 构建误差 H 矩阵 H = H_marg + H_pp_prior
MatXX H_marg(MatXX::Zero(cols, cols));
VecX b_marg(VecX::Zero(cols));接下来，对于每个要边缘化的边，计算其残差和雅克比矩阵，然后叠加所有节点对之间的 J i T W J j J_i^TWJ_j JiTWJj构造H和b。
int ii = 0;
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); // 输出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();
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();
}
} 【备注】相关带鲁棒核函数的信息矩阵和H、b矩阵的知识点以及代码解析，可以看我的另一篇博客单目BA求解代码解析中的【补充】鲁棒核函数的H矩阵以及b部分。
step5. 针对所关联的视觉特征的边缘化
- 对H、b进行分块，为舒尔补操作做准备。
构造出的形式如下：
[ H p p H p m H m p H m m ] [ δ x p δ x m ] = [ b p p b m m ] \left[ \begin{matrix} H_{pp} &H_{pm} \\ H_{mp} &H_{mm} \\ \end{matrix} \right] \left[ \begin{matrix} \delta x_p \\ \delta x_m \\ \end{matrix} \right] = \left[ \begin{matrix} b_{pp} \\ b_{mm} \\ \end{matrix} \right] [HppHmpHpmHmm][δxpδxm]=[bppbmm]
int reserve_size = pose_dim;
if (marg_landmark_size > 0)
{
int marg_size = marg_landmark_size;
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: margLandmark) {
int idx = iter.second->OrderingId() - reserve_size;
int size = iter.second->LocalDimension();
Hmm_inv.block(idx, idx, size, size) = Hmm.block(idx, idx, size, size).inverse();
}
MatXX tempH = Hpm * Hmm_inv;
MatXX Hpp = H_marg.block(0, 0, reserve_size, reserve_size) - tempH * Hmp; // 公式(5)等式左边的系数矩阵
bpp = bpp - tempH * bmm; // 公式(5)等式右边部分
H_marg = Hpp;
b_marg = bpp;step6. 针对帧和speedbias进行边缘化
在边缘化掉视觉信息特征的H_marg、b_marg上继续进行对帧和speedbias的边缘化。
- 需要把将要边缘化的节点移动到H_marg的右下角
图示：
[
H
p
p
H
p
m
H
m
p
H
m
m
]
−
>
H
p
p
=
H
p
p
−
H
p
m
∗
H
m
m
−
1
∗
H
m
p
−
>
H
p
p
、
b
p
p
−
>
移
动
要
边
缘
化
的
节
点
到
右
下
角
\left[ \begin{matrix} \red{H_{pp}} &\green{H_{pm}} \\ \green{H_{mp}} &\green{H_{mm}} \\ \end{matrix} \right]-> H_{pp} = H_{pp}-H_{pm}*H_{mm}^{-1}*H_{mp}-> H_{pp}、b_{pp}->移动要边缘化的节点到右下角
[HppHmpHpmHmm]−>Hpp=Hpp−Hpm∗Hmm−1∗Hmp−>Hpp、bpp−>移动要边缘化的节点到右下角
最终矩阵形式为：
A_mm表示要边缘化掉的所有帧和speedbias信息。
相关代码：
/// marg frame and speedbias
int marg_dim = 0;
// index 大的先移动
for (int k = margVertexs.size() -1 ; k >= 0; --k)
{
int idx = margVertexs[k]->OrderingId();
int dim = margVertexs[k]->LocalDimension();
// std::cout << k << " "<<idx << std::endl;
marg_dim += dim;
// move the marg pose to the Hmm bottown right
// 将 row i 移动矩阵最下面
Eigen::MatrixXd temp_rows = H_marg.block(idx, 0, dim, reserve_size);
Eigen::MatrixXd temp_botRows = H_marg.block(idx + dim, 0, reserve_size - idx - dim, reserve_size);
H_marg.block(idx, 0, reserve_size - idx - dim, reserve_size) = temp_botRows;
H_marg.block(reserve_size - dim, 0, dim, reserve_size) = temp_rows;
// 将 col i 移动矩阵最右边
Eigen::MatrixXd temp_cols = H_marg.block(0, idx, reserve_size, dim);
Eigen::MatrixXd temp_rightCols = H_marg.block(0, idx + dim, reserve_size, reserve_size - idx - dim);
H_marg.block(0, idx, reserve_size, reserve_size - idx - dim) = temp_rightCols;
H_marg.block(0, reserve_size - dim, reserve_size, dim) = temp_cols;
Eigen::VectorXd temp_b = b_marg.segment(idx, dim);
Eigen::VectorXd temp_btail = b_marg.segment(idx + dim, reserve_size - idx - dim);
b_marg.segment(idx, reserve_size - idx - dim) = temp_btail;
b_marg.segment(reserve_size - dim, dim) = temp_b;
}- 舒尔补操作，最后产生的就是先验
double eps = 1e-8;
int m2 = marg_dim;
int n2 = reserve_size - marg_dim; // marg pose
Eigen::MatrixXd Amm = 0.5 * (H_marg.block(n2, n2, m2, m2) + H_marg.block(n2, n2, m2, m2).transpose()); // 保证数值求解是对称矩阵
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm); // 求解矩阵的特征值和特征向量
Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd( // 奇异值分解？
(saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() *
saes.eigenvectors().transpose();
Eigen::VectorXd bmm2 = b_marg.segment(n2, m2);
Eigen::MatrixXd Arm = H_marg.block(0, n2, n2, m2);
Eigen::MatrixXd Amr = H_marg.block(n2, 0, m2, n2);
Eigen::MatrixXd Arr = H_marg.block(0, 0, n2, n2);
Eigen::VectorXd brr = b_marg.segment(0, n2);
Eigen::MatrixXd tempB = Arm * Amm_inv;
H_prior_ = Arr - tempB * Amr;
b_prior_ = brr - tempB * bmm2;
step7. 为利用ceres进行优化，更新先验残差项
相关知识点可以查看这篇博客——VINS-MONO边缘化策略。摘自这篇参考博客，
边缘化步骤总结如下：
第一步为前面对视觉特征、相关帧和speedbias的边缘化，得到
H
0
∗
H_0^*
H0∗,也就是代码中的H_prior_、b_prior_
① 求解
(
J
T
)
+
(J^T)^+
(JT)+得到残差e，重新获得
H
H
H
相关代码如下：
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(H_prior_); // 求解矩阵的特征值和特征向量
Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0));
Eigen::VectorXd S_inv = Eigen::VectorXd(
(saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0));
Eigen::VectorXd S_sqrt = S.cwiseSqrt(); // S^(1/2)
Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();
Jt_prior_inv_ = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose(); // (J^T)^+
err_prior_ = -Jt_prior_inv_ * b_prior_; // e0
MatXX J = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();
H_prior_ = J.transpose() * J; // H
MatXX tmp_h = MatXX( (H_prior_.array().abs() > 1e-9).select(H_prior_.array(),0) );
H_prior_ = tmp_h; // H 先验矩阵step7. 移除相关的边缘化的节点和边
for (size_t k = 0; k < margVertexs.size(); ++k) {
RemoveVertex(margVertexs[k]); // 移除该顶点对应的边
}
for (auto landmarkVertex: margLandmark) {
RemoveVertex(landmarkVertex.second);
}RemoveVertex():
bool Problem::RemoveVertex(std::shared_ptr<Vertex> vertex) {
//check if the vertex is in map_verticies_
if (verticies_.find(vertex->Id()) == verticies_.end()) {
// LOG(WARNING) << "The vertex " << vertex->Id() << " is not in the problem!" << endl;
return false;
}
// 这里要 remove 该顶点对应的 edge.
vector<shared_ptr<Edge>> remove_edges = GetConnectedEdges(vertex);
for (size_t i = 0; i < remove_edges.size(); i++) {
RemoveEdge(remove_edges[i]);
}
if (IsPoseVertex(vertex))
idx_pose_vertices_.erase(vertex->Id());
else
idx_landmark_vertices_.erase(vertex->Id());
vertex->SetOrderingId(-1); // used to debug
verticies_.erase(vertex->Id());
vertexToEdge_.erase(vertex->Id());
return true;
}
bool Problem::RemoveEdge(std::shared_ptr<Edge> edge) {
//check if the edge is in map_edges_
if (edges_.find(edge->Id()) == edges_.end()) {
// LOG(WARNING) << "The edge " << edge->Id() << " is not in the problem!" << endl;
return false;
}
edges_.erase(edge->Id());
return true;
}【注意】边缘化求解出的H_prior_、b_prior_是作为后面优化求解的先验矩阵。先验、未涉及边缘化的信息再加上新的测量信息，组成新的信息矩阵。
5.滑窗预移动
调整参数块在下一次窗口中对应的位置，注意，这里只是相当于将指针进行了一次移动，指针对应的数据还是旧数据，因此需要结合后面调用的slideWindow()函数才能实现真正的滑窗移动。
5.1 若边缘化的是老帧
边缘化老帧后，调整参数块在下一次窗口中对应的位置，整体往前移一格(删除最老帧)，注意这里只是指针，后面slideWindow中会赋新值，这里只是提前占座。
相关代码：
std::unordered_map<long, double *> addr_shift; // prior 中对应的保留下来的参数地址
// 从1开始，因为第一帧的状态要剔除
for (int i = 1; i <= WINDOW_SIZE; i++) // 整体前移一格，去掉最老帧
{
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
}
for (int i = 0; i < NUM_OF_CAM; i++)
addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i]; // 外参保持不变
if (ESTIMATE_TD)
{
addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];
}5.2 若边缘化的是次新帧
调整参数块在下一次窗口中对应的位置（去掉次新帧）
相关代码：
std::unordered_map<long, double *> addr_shift;
for (int i = 0; i <= WINDOW_SIZE; i++)
{
if (i == WINDOW_SIZE - 1)
continue;
else if (i == WINDOW_SIZE) // 删除次新帧
{
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
}
else
{
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i];
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i];
}
}
for (int i = 0; i < NUM_OF_CAM; i++)
addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
if (ESTIMATE_TD)
{
addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];
}【备注】continue：跳过本次循环体中余下尚未执行的语句,立即进行下一次的循环条件判定,可以理解为仅结束本次循环 -----hhhhh小儿科了~~~
6.实现滑窗的更新
相关解析可见我的另一篇博客——thd_BackEnd线程(内容|代码)滑窗更新部分。
至此，边缘化内容全部完成！！！撒花★,°:.☆(￣▽￣)/$:.°★ 。
6919
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
