VINS-Mono代码阅读笔记（十三）：posegraph中四自由度位姿优化
2024-06-08 19:41:46
本篇笔记紧接着VINS-Mono代码阅读笔记（十二）：将关键帧加入位姿图当中，来学习pose_graph当中的紧耦合优化部分。
在重定位完成之后，进行位姿图优化是为了将已经产生的所有位姿统一到一个全局一致的配置当中。如论文中展示的下图所示，参考帧处于世界坐标系下，当相机运动的时候会相对于参考帧发生变化。而由于重力向量始终不会发生变化，所以从重力方向得到的水平面也不会发生变化，进而该水平面对应的
两个向量也不会发生变换。所以，系统中需要计算并且优化的向量只有
（也就是位置和旋转），这就是4自由度优化的由来。
1.加入关键帧到位姿图当中
通过代码可以发现，当滑动窗口完成一次边缘化（滑出最旧的帧或者滑动窗口中倒数第二帧）后，在后边的pubKeyframe函数中会将the second latest frame关键帧的位姿信息作为topic发出来。pose_graph节点中接收到这个topic后，构造出对应的关键帧并加入位姿图当中。
1）相关代码
vins-estimator中发送关键帧位姿的代码如下：
void pubKeyframe(const Estimator &estimator)
{// pub camera pose, 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.pose.pose.position.x = P.x();odometry.pose.pose.position.y = P.y();odometry.pose.pose.position.z = P.z();odometry.pose.pose.orientation.x = R.x();odometry.pose.pose.orientation.y = R.y();odometry.pose.pose.orientation.z = R.z();odometry.pose.pose.orientation.w = R.w();//printf("time: %f t: %f %f %f r: %f %f %f %f\n", odometry.header.stamp.toSec(), P.x(), P.y(), P.z(), R.w(), R.x(), R.y(), R.z());pub_keyframe_pose.publish(odometry);而在pose-graph中将该关键帧加入位姿图的代码如下：
/*** process线程入口函数
*/
void process()
{.......//创建关键帧KeyFrame* keyframe = new KeyFrame(pose_msg->header.stamp.toSec(), frame_index, T, R, image,point_3d, point_2d_uv, point_2d_normal, point_id, sequence);m_process.lock();start_flag = 1;//位姿图中加入关键帧，flag_detect_loop设置为1posegraph.addKeyFrame(keyframe, 1);2）位姿图中的顶点和边
每一个关键帧在位姿图中作为一个顶点存在，它和其他关键帧以序列变和闭环边两种类型的边进行连接，如下图所示：
序列边（Sequential Edge）:
一个关键帧将与其前边的多个关键帧建立序列边（如上图所示）。一个序列边表示两个关键帧之间的相对变换，这个可以直接从VIO中得出。假设关键帧和其前边的一个关键帧
,两个关键帧构成的序列边只包含相对位置
和偏航角
，则这两个值表达如下：
闭环边（Loop-Closure Edge）：
如果一个关键帧有闭环连接，那么它在位姿图中和闭环帧之间的连接为闭环边。相似的，闭环边只包括一个4自由度的相对位姿变换，定义和上边的序列变相同。闭环边的值，是通过重定位得到的。
2.四自由度的位姿图优化
我们定义关键帧和
之间的边的最小化残差如下：
这里的和
两个值，是从单目VIO中估计得到的roll和pitch的角度，是固定的。
序列边和闭环边的所有图通过最小化下面的损失函数来进行优化：
这里是所有序列变的集合，
是所有闭环边的集合。尽管紧耦合的重定位已经帮助消除了错误的闭环，在这里增加另一个Huber核函数
以进一步减少任何可能的错误闭环的影响。
优化部分在pose_graph节点构造posegraph对象的时候，在构造函数当中新启了一个线程来完成。PoseGraph的构造函数代码如下：
/*** PoseGraph构造函数
*/
PoseGraph::PoseGraph()
{posegraph_visualization = new CameraPoseVisualization(1.0, 0.0, 1.0, 1.0);posegraph_visualization->setScale(0.1);posegraph_visualization->setLineWidth(0.01);//创建位姿优化线程t_optimization = std::thread(&PoseGraph::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;sequence_cnt = 0;sequence_loop.push_back(0);base_sequence = 1;}位姿优化的线程入口函数为optimize4DoF，optimize4DoF代码如下：
/*** 位姿图中的优化，这里是4个自由度的位姿优化
*/
void PoseGraph::optimize4DoF()
{while(true){int cur_index = -1;int first_looped_index = -1;m_optimize_buf.lock();//从优化队列当中获取最新的一个关键帧的indexwhile(!optimize_buf.empty()){cur_index = optimize_buf.front();//earliest_loop_index当中存放的是数据库中第一个和滑动窗口中关键帧形成闭环的关键帧的indexfirst_looped_index = earliest_loop_index;optimize_buf.pop();}m_optimize_buf.unlock();//optimize_buf中取出来的cur_index都是闭环帧的indexif (cur_index != -1){printf("optimize pose 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_idouble t_array[max_length][3];//平移数组，其中存放每个关键帧的平移向量Quaterniond q_array[max_length];//旋转数组，其中存放每个关键帧的旋转四元数double euler_array[max_length][3];double sequence_array[max_length];ceres::Problem problem;ceres::Solver::Options options;options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;//options.minimizer_progress_to_stdout = true;//options.max_solver_time_in_seconds = 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 != keyframelist.end(); it++){//first_looped_index为第一次闭环帧的index,需要优化的关键帧为从第一次闭环帧到当前帧间的所有关键帧if ((*it)->index < first_looped_index)continue;(*it)->local_index = i;Quaterniond tmp_q;Matrix3d tmp_r;Vector3d tmp_t;//获取关键帧it的位姿(*it)->getVioPose(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(tmp_q.toRotationMatrix());euler_array[i][0] = euler_angle.x();euler_array[i][1] = euler_angle.y();euler_array[i][2] = euler_angle.z();sequence_array[i] = (*it)->sequence;//将关键帧列表中所有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)->sequence == 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 && sequence_array[i] == sequence_array[i-j]){Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());//p̂_j^w - p̂_i^w 计算平移量的偏差Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);//p̂_{ij}^{i} = {R̂_i^w}^{-1} (p̂_j^w - p̂_i^w)//p̂ ij= (R̂ iw ) (p̂ jw − p̂ iw )relative_t = q_array[i-j].inverse() * relative_t;//ψ̂ _ij = ψ̂ _j − ψ̂ _idouble relative_yaw = euler_array[i][0] - euler_array[i-j][0];ceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(),relative_yaw, euler_conncected.y(), euler_conncected.z());problem.AddResidualBlock(cost_function, NULL, euler_array[i-j], t_array[i-j], euler_array[i], t_array[i]);}}//add loop edge//这里添加的是闭环边，是指检测到闭环的两帧if((*it)->has_loop){assert((*it)->loop_index >= first_looped_index);int connected_index = getKeyFrame((*it)->loop_index)->local_index;Vector3d euler_conncected = Utility::R2ypr(q_array[connected_index].toRotationMatrix());Vector3d relative_t;relative_t = (*it)->getLoopRelativeT();double relative_yaw = (*it)->getLoopRelativeYaw();ceres::CostFunction* cost_function = FourDOFWeightError::Create( relative_t.x(), relative_t.y(), relative_t.z(),relative_yaw, euler_conncected.y(), euler_conncected.z());problem.AddResidualBlock(cost_function, loss_function, euler_array[connected_index], t_array[connected_index], euler_array[i], t_array[i]);}if ((*it)->index == cur_index)break;i++;}m_keyframelist.unlock();//开始优化ceres::Solve(options, &problem, &summary);//std::cout << summary.BriefReport() << "\n";//printf("pose optimization time: %f \n", tmp_t.toc());/*for (int j = 0 ; j < i; j++){printf("optimize i: %d p: %f, %f, %f\n", j, t_array[j][0], t_array[j][1], t_array[j][2] );}*/m_keyframelist.lock();//优化完成，使用优化后的位姿来更新关键帧列表中index大于等于first_looped_index的所有关键帧的位姿i = 0;for (it = keyframelist.begin(); it != keyframelist.end(); it++){if ((*it)->index < first_looped_index)continue;Quaterniond tmp_q;//向量转换为矩阵tmp_q = Utility::ypr2R(Vector3d(euler_array[i][0], euler_array[i][1], euler_array[i][2]));Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);Matrix3d tmp_r = tmp_q.toRotationMatrix();(*it)-> updatePose(tmp_t, tmp_r);if ((*it)->index == cur_index)break;i++;}//根据计算出当前帧的drift，更新全部关键帧位姿Vector3d cur_t, vio_t;Matrix3d cur_r, vio_r;//获取优化后当前帧的位姿cur_t,cur_rcur_kf->getPose(cur_t, cur_r);//获取优化前有漂移的当前帧的位姿vio_t,vio_rcur_kf->getVioPose(vio_t, vio_r);m_drift.lock();yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));t_drift = cur_t - r_drift * vio_t;m_drift.unlock();//cout << "t_drift " << t_drift.transpose() << endl;//cout << "r_drift " << Utility::R2ypr(r_drift).transpose() << endl;//cout << "yaw drift " << yaw_drift << endl;it++;//下面代码为把当前关键帧it之后的关键帧的位姿通过求解的偏移量转换到world坐标系下for (; it != keyframelist.end(); it++){Vector3d P;Matrix3d R;(*it)->getVioPose(P, R);P = r_drift * P + t_drift;R = r_drift * R;(*it)->updatePose(P, R);}m_keyframelist.unlock();//优化完后更新pathupdatePath();}std::chrono::milliseconds dura(2000);std::this_thread::sleep_for(dura);}
}这里除了梳理优化的函数中的代码流程，还需要对比以上的残差公式来看一下序列边和闭环边的残差计算。FourDOFError类中的operator()中是序列边的残差计算代码。
struct FourDOFError
{FourDOFError(double t_x, double t_y, double t_z, double relative_yaw, double pitch_i, double roll_i):t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw), pitch_i(pitch_i), roll_i(roll_i){}template <typename T>bool operator()(const T* const yaw_i, const T* ti, const T* yaw_j, const T* tj, T* residuals) const{T t_w_ij[3];t_w_ij[0] = tj[0] - ti[0];t_w_ij[1] = tj[1] - ti[1];t_w_ij[2] = tj[2] - ti[2];// euler to rotationT w_R_i[9];YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);// rotation transposeT i_R_w[9];//求出旋转矩阵的转置RotationMatrixTranspose(w_R_i, i_R_w);// rotation matrix rotate pointT t_i_ij[3];RotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);//计算残差residuals[0] = (t_i_ij[0] - T(t_x));residuals[1] = (t_i_ij[1] - T(t_y));residuals[2] = (t_i_ij[2] - T(t_z));residuals[3] = NormalizeAngle(yaw_j[0] - yaw_i[0] - T(relative_yaw));return true;}static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,const double relative_yaw, const double pitch_i, const double roll_i) {return (new ceres::AutoDiffCostFunction<FourDOFError, 4, 1, 3, 1, 3>(new FourDOFError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));}double t_x, t_y, t_z;double relative_yaw, pitch_i, roll_i;};FourDOFWeightError类中的operator()是闭环边的残差计算代码。
struct FourDOFWeightError
{FourDOFWeightError(double t_x, double t_y, double t_z, double relative_yaw, double pitch_i, double roll_i):t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw), pitch_i(pitch_i), roll_i(roll_i){weight = 1;}template <typename T>bool operator()(const T* const yaw_i, const T* ti, const T* yaw_j, const T* tj, T* residuals) const{T t_w_ij[3];t_w_ij[0] = tj[0] - ti[0];t_w_ij[1] = tj[1] - ti[1];t_w_ij[2] = tj[2] - ti[2];// euler to rotationT w_R_i[9];YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);// rotation transposeT i_R_w[9];RotationMatrixTranspose(w_R_i, i_R_w);// rotation matrix rotate pointT t_i_ij[3];RotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);//计算残差residuals[0] = (t_i_ij[0] - T(t_x)) * T(weight);residuals[1] = (t_i_ij[1] - T(t_y)) * T(weight);residuals[2] = (t_i_ij[2] - T(t_z)) * T(weight);residuals[3] = NormalizeAngle((yaw_j[0] - yaw_i[0] - T(relative_yaw))) * T(weight) / T(10.0);return true;}static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,const double relative_yaw, const double pitch_i, const double roll_i) {return (new ceres::AutoDiffCostFunction<FourDOFWeightError, 4, 1, 3, 1, 3>(new FourDOFWeightError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));}double t_x, t_y, t_z;double relative_yaw, pitch_i, roll_i;double weight;};VINS-Mono代码阅读笔记（十四）：posegraph的存储和加载
VINS-Mono代码阅读笔记（十三）：posegraph中四自由度位姿优化相关推荐
- VINS-Mono代码阅读笔记（十四）：posegraph的存储和加载
本篇笔记紧接着VINS-Mono代码阅读笔记(十三):posegraph中四自由度位姿优化,来分析位姿图的存储和加载. 完整(也是理想的)的SLAM的使用应该是这样的:搭载有SLAM程序的移动设备在一 ...
- VINS-Mono代码阅读笔记（十）：vins_estimator中的非线性优化
本篇笔记紧接着VINS-Mono代码阅读笔记(九):vins_estimator中的相机-IMU对齐,初始化完成之后就获得了要优化的变量的初始值,后边就是做后端优化处理了.这部分对应论文中第VI部分, ...
- ffmpeg中的http协议相关代码阅读笔记
ffmpeg中的http协议相关代码阅读笔记 今天闲来无事,尝试看了下ffmpeg中的相关http协议传输处理代码 先简单说下这个代码在整个仓库里面的位置: ffmpeg/libavformat/ht ...
- C++ Primer Plus 6th代码阅读笔记
C++ Primer Plus 6th代码阅读笔记 第一章没什么代码 第二章代码 carrots.cpp : cout 可以拼接输出,cin.get()接受输入 convert.cpp 函数原型放在主 ...
- [置顶] Linux协议栈代码阅读笔记（一）
Linux协议栈代码阅读笔记(一) (基于linux-2.6.21.7) (一)用户态通过诸如下面的C库函数访问协议栈服务 int socket(int domain, int type, int p ...
- linux 协议栈 位置,[置顶] Linux协议栈代码阅读笔记（一）
Linux协议栈代码阅读笔记(一) (基于linux-2.6.21.7) (一)用户态通过诸如下面的C库函数访问协议栈服务 int socket(int domain, int type, int p ...
- BNN Pytorch代码阅读笔记
BNN Pytorch代码阅读笔记 这篇博客来写一下我对BNN(二值化神经网络)pytorch代码的理解,我是第一次阅读项目代码,所以想仔细的自己写一遍,把细节理解透彻,希望也能帮到大家! 论文链接: ...
- 菜鸟笔记-DuReader阅读理解基线模型代码阅读笔记（八）—— 模型训练-训练
系列目录: 菜鸟笔记-DuReader阅读理解基线模型代码阅读笔记(一)--数据 菜鸟笔记-DuReader阅读理解基线模型代码阅读笔记(二)-- 介绍及分词 菜鸟笔记-DuReader阅读理解基线模 ...
- leveldb代码阅读笔记（一）
leveldb代码阅读笔记 above all leveldb是一个单机的键值存储的内存数据库,其内部使用了 LSM tree 作为底层存储结构,支持多版本数据控制,代码设计巧妙且简洁高效,十分值得作 ...
- [原创]fetchmail代码阅读笔记---ESMTP的认证方式
fetchmail代码阅读笔记---ESMTP的认证方式 作者: 默难 ( monnand@gmail.com ) 0 引言 fetchmail是Eric S. Raymond组织编写的一款全功 ...
最新文章
- 单链表-逆置单链表（头插法且双指针）
- JavaScript深入之变量对象
- dmz“非军事化区”
- PCB布线技术 很好很强大
- java调用python项目实战_Java调用Python
- EventLoopGroup 与Reactor 关联
- android 批量查找view,Android Recyclerview实现多选,单选,全选,反选,批量删除的功能
- Oracle 基础篇 --- 索引选项
- bool转nsnumber ios_iOS开发之NSDecimalNumber的使用,货币计算/精确数值计算/保留位数等...
- Setup linux 网卡故障,linux网络配置、测试及故障诊断方法和工具二
- mysql三大范式 答案_数据库逻辑设计之三大范式通俗理解，一看就懂，书上说的太晦涩...
- Android开发笔记（四十三）点击事件
- canvas绘图粒子扩散效果【原创】
- 小学期实践心得（1）
- 中国老工业基地吉林对外贸易重回历史高水平
- xgboost4j jar包下载
- 冒泡法排序c语言代码大全,C语言实现选择排序、冒泡排序和快速排序的代码示例...
- T61 拆机4短报警 续
- 泛微oa java 源码_FineReport和泛微OA(Ecology)的单点登录集成方案
- jconsole使用