BundleAdjustment函数

1、理论部分（待更新）

2、代码详解

//BA优化
// BundleAdjustment( vpKFs //关键帧,vpMP //地图点,nIterations //迭代次数,pbStopFlag //停止标志, nLoopKF   //回环帧数量, bRobust   //是否进行鲁棒核函数);
void Optimizer::BundleAdjustment(const vector<KeyFrame *> &vpKFs, const vector<MapPoint *> &vpMP,int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
{//判断每个地图点是否更新vector<bool> vbNotIncludedMP;vbNotIncludedMP.resize(vpMP.size());//构造优化器g2o::SparseOptimizer optimizer;//线性求解器 Hx = bg2o::BlockSolver_6_3::LinearSolverType * linearSolver;//分配空间linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);//LM算法进行求解g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);optimizer.setAlgorithm(solver);//用于停止求解if(pbStopFlag)optimizer.setForceStopFlag(pbStopFlag);long unsigned int maxKFid = 0;// Set KeyFrame vertices//建立关键帧的顶点//遍历所有关键帧for(size_t i=0; i<vpKFs.size(); i++){//提取每一个关键帧KeyFrame* pKF = vpKFs[i];//若为ie坏帧剔除if(pKF->isBad())continue;//定义顶点从 李代数的形式g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();//导入要估计的数据（位姿）vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));//导入索引vSE3->setId(pKF->mnId);//固定第一帧的位置vSE3->setFixed(pKF->mnId==0);//添加顶点optimizer.addVertex(vSE3);//若索引最大则替换当前最大索引  if(pKF->mnId>maxKFid)maxKFid=pKF->mnId;}//鲁棒核函数const float thHuber2D = sqrt(5.99);const float thHuber3D = sqrt(7.815);// Set MapPoint vertices//建立地图点的顶点 for(size_t i=0; i<vpMP.size(); i++){MapPoint* pMP = vpMP[i];if(pMP->isBad())continue;g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));//索引位于位姿之后const int id = pMP->mnId+maxKFid+1;vPoint->setId(id);//边缘化（加速求解）vPoint->setMarginalized(true);optimizer.addVertex(vPoint);//获取观测值const map<KeyFrame*,size_t> observations = pMP->GetObservations();int nEdges = 0;//SET EDGES//创建边//遍历所有观测值for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++){//获取观测的关键帧KeyFrame* pKF = mit->first;//如果该帧为坏帧，或者索引大于最大索引，那便跳过if(pKF->isBad() || pKF->mnId>maxKFid)continue;nEdges++;//获取关键帧中的关键点const cv::KeyPoint &kpUn = pKF->mvKeysUn[mit->second];// 单目或RGBD相机if(pKF->mvuRight[mit->second]<0){//观测像素点Eigen::Matrix<double,2,1> obs;obs << kpUn.pt.x, kpUn.pt.y;g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();//优化的顶点0 地图点e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));//优化的顶点1 位姿e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));//导入观测e->setMeasurement(obs);//定义信息矩阵const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);//鲁棒核函数if(bRobust){g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(thHuber2D);//结构里有结构}//导入相机、内参e->fx = pKF->fx;e->fy = pKF->fy;e->cx = pKF->cx;e->cy = pKF->cy;//添加边optimizer.addEdge(e);}else //双目{Eigen::Matrix<double,3,1> obs;const float kp_ur = pKF->mvuRight[mit->second];obs << kpUn.pt.x, kpUn.pt.y, kp_ur;g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));e->setMeasurement(obs);const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;e->setInformation(Info);if(bRobust){g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(thHuber3D);}e->fx = pKF->fx;e->fy = pKF->fy;e->cx = pKF->cx;e->cy = pKF->cy;e->bf = pKF->mbf;optimizer.addEdge(e);}}//如果边的数量为0，则清除顶点if(nEdges==0){optimizer.removeVertex(vPoint);vbNotIncludedMP[i]=true;}else{vbNotIncludedMP[i]=false;}}//添加边结束// Optimize!//开始进行优化optimizer.initializeOptimization();optimizer.optimize(nIterations);// Recover optimized data//得到优化后的数据//Keyframes 关键帧//遍历每一个关键帧并提取优化后的位姿for(size_t i=0; i<vpKFs.size(); i++){KeyFrame* pKF = vpKFs[i];if(pKF->isBad())continue;//提取估计好的位姿g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));g2o::SE3Quat SE3quat = vSE3->estimate();//不发生回环if(nLoopKF==0){//无回环就默认当前位姿pKF->SetPose(Converter::toCvMat(SE3quat));}else//若出现回环要提取估计值进行回环检测在更新位姿{pKF->mTcwGBA.create(4,4,CV_32F);Converter::toCvMat(SE3quat).copyTo(pKF->mTcwGBA);pKF->mnBAGlobalForKF = nLoopKF;}}//Keyframes 关键帧//Points 地图点特征点//遍历地图点for(size_t i=0; i<vpMP.size(); i++){//没有优化的地图点跳过if(vbNotIncludedMP[i])continue;//提取地图点MapPoint* pMP = vpMP[i];//如果位坏的地图点跳过if(pMP->isBad())continue;//转换类型提取地图点g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));//判定是否回环if(nLoopKF==0){//无回环就默认当前位姿pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));pMP->UpdateNormalAndDepth();}else//若出现回环要提取估计值进行回环检测{pMP->mPosGBA.create(3,1,CV_32F);Converter::toCvMat(vPoint->estimate()).copyTo(pMP->mPosGBA);pMP->mnBAGlobalForKF = nLoopKF;}}//Points 地图点特征点}//BA优化

ORB-SLAM2学习笔记——BundleAdjustment函数相关推荐

python函数是一段具有特定功能的语句组_Python学习笔记(五)函数和代码复用
本文将为您描述Python学习笔记(五)函数和代码复用,具体完成步骤: 函数能提高应用的模块性,和代码的重复利用率.在很多高级语言中,都可以使用函数实现多种功能.在之前的学习中,相信你已经知道Pyth ...
Python学习笔记：函数（Function）
Python学习笔记:函数(Function) 一.函数基本概念函数是Python里组织与重用代码最重要的方法.一般来说,如果你期望多次重复相同或相似的代码,写一个可重用的函数可能是值得的.函数通过 ...
php中声明一个函数,php学习笔记之函数声明
/* 函数定义: * 1.函数是一个被命名的 * 2.独立的代码段 * 3.函数执行特定任务 * 4.并可以给调用它的程序返回一个值 * * 函数的优点: * 1.提高程序的重用性 * 2.提高程序的 ...
Matlab学习笔记 figure函数
Matlab学习笔记 figure函数 matlab中的 figure 命令,能够创建一个用来显示图形输出的一个窗口对象.每一个这样的窗口都有一些属性,例如窗口的尺寸.位置,等等.下面一一介绍它们. ...
JAVA学习笔记五---函数
JAVA学习笔记五---函数 5.1 方法的学习编写一个程序,求圆的周长和面积. package practice; /*** 编写一个程序,求圆的周长和面积.* @author iszhangyo ...
MySQL学习笔记—自定义函数
MySQL学习笔记-自定义函数注释语法: MySQL服务器支持3种注释风格: 从'#'字符从行尾. 从'– '序列到行尾.请注意'– '(双破折号)注释风格要求第2个破折号后面至少跟一个空格符(例如 ...
matlab机器人工具箱学习笔记——ikine函数
matlab机器人工具箱学习笔记--ikine函数 ikine函数用法使用实例链接: https://blog.csdn.net/weixin_42596724/article/details/8 ...
《JavaScript语言精粹》学习笔记（函数（2））
<JavaScript语言精粹>学习笔记(函数(2)) 函数(Functions) 参数(Arguments) 当参数被调用时,会得到一个"免费"的参数数组argume ...
matlab 调用子函数返回值,matlab学习笔记13_1 函数返回值
一起来学matlab-matlab学习笔记13函数 13_1 函数返回值觉得有用的话,欢迎一起讨论相互学习~Follow Me 函数返回一个值返回值不必使用return语句,而是直接将需要返回的变 ...
Python学习笔记12_函数
Python学习笔记12_函数文章目录 Python学习笔记12_函数 1.函数定义 2.函数调用 3.函数的参数 3.1.可更改对象和不可更改对象参数 3.2.必需参数(位置参数) 3.3.关键字 ...

ORB-SLAM2学习笔记——BundleAdjustment函数

BundleAdjustment函数

1、理论部分（待更新）

2、代码详解

ORB-SLAM2学习笔记——BundleAdjustment函数相关推荐

最新文章

热门文章