VINS-Mono代码阅读笔记（⼗三）：pograph中四⾃由度位
姿优化
本篇笔记紧接着，来学习po_graph当中的紧耦合优化部分。
在重定位完成之后，进⾏位姿图优化是为了将已经产⽣的所有位姿统⼀到⼀个全局⼀致的配置当中。如论⽂中展⽰的下图所⽰，参考帧处于世界坐标系下，当相机运动的时候会相对于参考帧发⽣变化。⽽由于重⼒向量始终不会发⽣变化，所以从重⼒⽅向得到的⽔平⾯也不会发⽣变化，进⽽该⽔平⾯对应的两个向量也不会发⽣变换。所以，系统中需要计算并且优化的向量只有（也就是位置和旋转），这就是4⾃由度优化的由来。
1.加⼊关键帧到位姿图当中
通过代码可以发现，当滑动窗⼝完成⼀次边缘化（滑出最旧的帧或者滑动窗⼝中倒数第⼆帧）后，在后边的pubKeyframe函数中会将the cond latest frame关键帧的位姿信息作为topic发出来。po_graph节点中接收到这个topic后，构造出对应的关键帧并加⼊位姿图当中。
1）相关代码
vins-estimator中发送关键帧位姿的代码如下：
void pubKeyframe(const Estimator &estimator)
{
// pub camera po, 2D-3D points of keyframe
//estimator.marginalization_flag == 0，表⽰MARGIN_OLD，边缘化删除了最旧的关键帧
//estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR表⽰视觉和惯导初始化成功
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR && estimator.marginalization_flag =
= 0)
{
int i = WINDOW_SIZE - 2;
//Vector3d P = estimator.Ps[i] + estimator.Rs[i] * estimator.tic[0];
Vector3d P = estimator.Ps[i];
Quaterniond R = Quaterniond(estimator.Rs[i]);
nav_msgs::Odometry odometry;
odometry.header = estimator.Headers[WINDOW_SIZE - 2];
odometry.header.frame_id = "world";
odometry.po.po.position.x = P.x();
odometry.po.po.position.y = P.y();
odometry.po.po.position.z = P.z();
odometry.ientation.x = R.x();
odometry.ientation.y = R.y();
odometry.ientation.z = R.z();
odometry.ientation.w = R.w();
//printf("time: %f t: %f %f %f r: %f %f %f %f\n", odometry.Sec(), P.x(), P.y(), P.z(), R.w(), R.x(), R.y(), R.z()); pub_keyframe_po.publish(odometry);
⽽在po-graph中将该关键帧加⼊位姿图的代码如下：
/**
* process线程⼊⼝函数
*/
void process()
{
.......
//创建关键帧
KeyFrame* keyframe = new KeyFrame(po_msg->Sec(), frame_index,
T, R,
青年节英语
image,point_3d, point_2d_uv, point_2d_normal, point_id, quence);
m_process.lock();
2012年英语四级start_flag = 1;
//位姿图中加⼊关键帧，flag_detect_loop设置为1
promotionpograph.addKeyFrame(keyframe, 1);
2）位姿图中的顶点和边
每⼀个关键帧在位姿图中作为⼀个顶点存在，它和其他关键帧以序列变和闭环边两种类型的边进⾏连接，如下图所⽰：
序列边（Sequential Edge）:
⼀个关键帧将与其前边的多个关键帧建⽴序列边（如上图所⽰）。⼀个序列边表⽰两个关键帧之间的相对变换，这个可以直接从VIO中得出。假设关键帧和其前边的⼀个关键帧，两个关键帧构成的序列边只包含相对位置和偏航⾓，则这两个值表达如下：
闭环边（Loop-Closure Edge）：
如果⼀个关键帧有闭环连接，那么它在位姿图中和闭环帧之间的连接为闭环边。相似的，闭环边只包括⼀个4⾃由度的相对位姿变换，定义和上边的序列变相同。闭环边的值，是通过重定位得到的。
2.四⾃由度的位姿图优化
我们定义关键帧和之间的边的最⼩化残差如下：
这⾥的和两个值，是从单⽬VIO中估计得到的roll和pitch的⾓度，是固定的。
序列边和闭环边的所有图通过最⼩化下⾯的损失函数来进⾏优化：
这⾥是所有序列变的集合，是所有闭环边的集合。尽管紧耦合的重定位已经帮助消除了错误的闭环，在这⾥增加另⼀个Huber核函数以进⼀步减少任何可能的错误闭环的影响。
优化部分在po_graph节点构造pograph对象的时候，在构造函数当中新启了⼀个线程来完成。Poxingwen
Graph的构造函数代码如下：
/**
* PoGraph构造函数
laden*/
PoGraph::PoGraph()
{
pograph_visualization = new CameraPoVisualization(1.0, 0.0, 1.0, 1.0);
pograph_visualization->tScale(0.1);
pograph_visualization->tLineWidth(0.01);
//创建位姿优化线程
t_optimization = std::thread(&PoGraph::optimize4DoF, this);
earliest_loop_index = -1;
t_drift = Eigen::Vector3d(0, 0, 0);
yaw_drift = 0;
r_drift = Eigen::Matrix3d::Identity();
w_t_vio = Eigen::Vector3d(0, 0, 0);
w_r_vio = Eigen::Matrix3d::Identity();
global_index = 0;
quence_cnt = 0;
quence_loop.push_back(0);
ba_quence = 1;
}
位姿优化的线程⼊⼝函数为optimize4DoF，optimize4DoF代码如下：
/**
* 位姿图中的优化，这⾥是4个⾃由度的位姿优化
*/
void PoGraph::optimize4DoF()
{
while(true)
{
int cur_index = -1;
int first_looped_index = -1;
m_optimize_buf.lock();
/
/从优化队列当中获取最新的⼀个关键帧的index
while(!pty())
{
cur_index = optimize_buf.front();
//earliest_loop_index当中存放的是数据库中第⼀个和滑动窗⼝中关键帧形成闭环的关键帧的index first_looped_index = earliest_loop_index;
optimize_buf.pop();
}
m_optimize_buf.unlock();
//optimize_buf中取出来的cur_index都是闭环帧的index
usgsif (cur_index != -1)
{
printf("optimize po graph \n");
TicToc tmp_t;
m_keyframelist.lock();
KeyFrame* cur_kf = getKeyFrame(cur_index);
//max_length为要优化的变量最⼤个数
int max_length = cur_index + 1;
// w^t_i w^q_i
double t_array[max_length][3];//平移数组，其中存放每个关键帧的平移向量
Quaterniond q_array[max_length];//旋转数组，其中存放每个关键帧的旋转四元数
double euler_array[max_length][3];
double quence_array[max_length];
ceres::Problem problem;
奥巴马父亲节演讲ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
//options.minimizer_progress_to_stdout = true;
//options.max_solver_time_in_conds = SOLVER_TIME * 3;
options.max_num_iterations = 5;
ceres::Solver::Summary summary;
ceres::LossFunction *loss_function;
loss_function = new ceres::HuberLoss(0.1);
//loss_function = new ceres::CauchyLoss(1.0);
/
/AngleLocalParameterization类的主要作⽤是指定yaw⾓优化变量的迭代更新，重载了括号运算
ceres::LocalParameterization* angle_local_parameterization =
AngleLocalParameterization::Create();
list<KeyFrame*>::iterator it;
int i = 0;
//遍历关键帧列表
for (it = keyframelist.begin(); it != d(); it++)
{
//first_looped_index为第⼀次闭环帧的index，需要优化的关键帧为从第⼀次闭环帧到当前帧间的所有关键帧
if ((*it)->index < first_looped_index)
continue;
(*it)->local_index = i;
hugoQuaterniond tmp_q;
Matrix3d tmp_r;
Vector3d tmp_t;
//获取关键帧it的位姿
(*it)->getVioPo(tmp_t, tmp_r);
tmp_q = tmp_r;
t_array[i][0] = tmp_t(0);
t_array[i][1] = tmp_t(1);
t_array[i][2] = tmp_t(2);
q_array[i] = tmp_q;
/
/将矩阵转换为向量
Vector3d euler_angle = Utility::R2ypr(RotationMatrix());
euler_array[i][0] = euler_angle.x();
euler_array[i][1] = euler_angle.y();
euler_array[i][2] = euler_angle.z();
quence_array[i] = (*it)->quence;
//将关键帧列表中所有index>=first_looped_index的关键帧的位姿加⼊到参数块当中
problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);
problem.AddParameterBlock(t_array[i], 3);
//设置约束：如果该帧是最早的闭环帧的情况下，则固定它的位姿
if ((*it)->index == first_looped_index || (*it)->quence == 0)
{
problem.SetParameterBlockConstant(euler_array[i]);
problem.SetParameterBlockConstant(t_array[i]);
}
//add edge 这⾥添加的是序列边，是指通过VIO计算的两帧之间的相对位姿，每帧分别与其前边最多四帧构成序列边 /**
* 顺序边的测量⽅程：p_{ij}^{i} = {R_i^w}^{-1} (p_j^w - p_i^w)
* \hat{ψ}_ij = \hat{ψ}_j − \hat{ψ̂}_i
* 两个关键帧之间的相对位姿，由两个关键帧之间的VIO位姿估计变换得到
* |------------------------------------|
* | |----------------------------|
* | | |-------------------|
* | | | |---------|
* |帧1| |帧2| |帧3| |帧4| |帧5|
*/
for (int j = 1; j < 5; j++)
我爱你英语
{
if (i - j >= 0 && quence_array[i] == quence_array[i-j])
{
Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());
nsm
//p_j^w - p_i^w 计算平移量的偏差
