点击上方“3D视觉工坊”,选择“星标”
干货第一时间送达
作者丨 lovely_yoshino
来源丨古月居
0. 前言
随着路径的不断延伸,机器人的建图过程会存在不断地累计误差。而传统的以gmapping为代表的使用粒子滤波进行定位的slam建图方式。以及ORB-SLAM为代表包含的局部优化和全局优化来调整外。但是这些处理方式只能减缓误差累计的程度,无法消除,而现在最为常用消除累计误差的方法就是利用回环检测来优化位姿。
当新的关键帧加入到优化模型时,在关键帧附近进行一次局部优化。
在全局优化中,所有的关键帧(除了第一帧)和三维点都参与优化。
1. 回环检测
回环检测作为近年来slam行业必备的部分,是指机器人识别曾到达某场景,使得地图闭环的能力。
回环检测之所以能成为一个难点,是因为:如果回环检测成功,可以显著地减小累积误差,帮助机器人更精准、快速的进行避障导航工作。而错误的检测结果可能使地图变得很糟糕。因此,回环检测在大面积、大场景地图构建上是非常有必要的
词袋模型(bag of words,BoW)早期是一种文本表征方法,后引入到计算机视觉领域,逐渐成为一种很有效的图像特征建模方法。它通过提取图像特征,再将特征进行分类构建视觉字典,然后采用视觉字典中的单词集合可以表征任一幅图像。
换句话说,通过BOW可以把一张图片表示成一个向量。这对判断图像间的关联很有帮助,所以目前比较流行的回环解决方案都是采用的BoW及其基础上衍生的算法IAB-MAP、FAB-MAP是在滤波框架下计算回环概率,RTAB-MAP采用关键帧比较相似性,DLoopDetector(在DBoW2基础上开发的回环检测库)采用连续帧的相似性检测判断是否存在回环。回环检测主要由BoW模块、算法模块、验证模块三部分组成。
词袋模型(BoW模块)
每一帧都可以用单词来描述,也就是这一帧中有哪些单词,这里只关心了有没有,而不必关心具体在哪里。只有两帧中单词种类相近才可能构成回环。
(a)图像预处理,假设训练集有M幅图像,将图像标准化为patch,统一格式和规格;
(b)特征提取,假设M幅图像,对每一幅图像提取特征,共提取出N个SIFT特征;
(c)特征聚类,采用K-means算法把N个对象分为K个簇 (视觉单词表),使簇内具有较高的相似度,而簇间相似度较低;
(d)统计得到图像的码本,每幅图像以单词表为规范对该幅图像的每一个SIFT特征点计算它与单词表中每个单词的距 离,最近的加1,便得到该幅图像的码本;还需要码本矢量归一化,因为每一幅图像的SIFT特征个数不定,所以需要归一化。
相似度计算(算法模块)
现在有了字典,但还有一点需要注意。我们利用单词表示图像,目的是发现回环,也就是两帧图像具有较高的相似性。那其实不同的单词对这一目的的贡献性是不同的。例如,我这篇文章中有很多“我们”这个词,但这个词并不能告诉我们太多信息。
而有些单词例如“回环”、“K-means”就比较具有代表性,能大概告诉我们这句话讲了什么。因此不同的单词应该具有不同的权重。
(a)贝叶斯估计方法。采用BoW描述机器人每一位置的场景图像,估计已获取图像与对应位置的先验概率,对当前时刻计算该新场景图像与已访问位置匹配的后验概率,概率大于阈值则标记为闭环。
(b)相似性方法。有了字典以后,给定任意特征点f_i,只要在字典树中逐层查找,最后都能找到与之对应的单词w_i。通常字典足够大,可以说它们来自同一类物体。但是这种方法对所有单词都是同样对待,常规的做法是采用TF-IDF(term frequency-inverse document frequency)
IDF(Inverse Document Frequency):描述单词在字典中出现的频率(构建字典时),越低越具有代表性
n为所有描述子数, n_i为该单词出现次数。ln的作用大概是降低量级,毕竟n很大。
TF(Term Frequency):单词在单帧图像中出现的频率,越高越具有代表性
n为一帧图像中所有单词数, n_i为一帧图像中该单词出现次数。
因此将一帧图像转化为单词表示时,我们要计算其单词的权重:
因此一帧图像A由单词 w_i,次数n_i以及权重η_i组成。
验证模块(回环处理)
回环的判断也并没有这么简单,含有很多的筛选环节,毕竟错误的回环将带来巨大灾难,宁可不要。例如某一位姿附近连续多次(ORB-SLAM中为3次)与历史中某一位姿附近出现回环才判断为回环;回环候选帧仍然要匹配,匹配点足够才为回环。方式主要有两种:
(a)时间一致性。正确的回环往往存在时间上的连续性,所以如果之后一段时间内能用同样的方法找到回环,则认为当前回环是正确的,也叫做顺序一致性约束。
(b)结构一致性校验。对回环检测到的两帧进行特征匹配并估计相机运动,因为各个特征点在空间中的位置是唯一不变的,与之前的估计误差比较大小。
然后下面主要就会涉及如何对找到的回环进行优化了。
2. SLAM优化
闭环纠正
在判断出现回环后,两帧计算Sim3变换(因为有尺度漂移),也就是从历史帧直接推算当前位姿。下面我们以ORB-SLAM2为代表减少形成闭环时的Sim3优化。
单目SLAM一般都会发生尺度(scale)漂移,因此Sim3上的优化是必要的。相对于SE3,Sim3的自由度要多一个,而且优化的目标是矫正尺度因子,因此优化并没有加入更多的变量。
//BRIEF 形成闭环时进行Sim3优化//Sim3用于回环检测时计算关键帧与回环帧之间的关系 R 、t、 s//相似变换群Sim3int Optimizer::OptimizeSim3(KeyFrame *pKF1, //关键帧1 KeyFrame *pKF2, //关键帧2 vector<MapPoint *> &vpMatches1,//两帧匹配关系 g2o::Sim3 &g2oS12, //两个关键帧间的Sim3变换 const float th2, //核函数阈值 const bool bFixScale//是否进行尺度优化 ){ //创建优化器 g2o::SparseOptimizer optimizer; g2o::BlockSolverX::LinearSolverType * linearSolver; linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>(); g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); //使用LM算法 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); optimizer.setAlgorithm(solver); // Calibration //得到两帧相机内参 const cv::Mat &K1 = pKF1->mK; const cv::Mat &K2 = pKF2->mK; // Camera poses //得到两帧的旋转与平移位姿 const cv::Mat R1w = pKF1->GetRotation(); const cv::Mat t1w = pKF1->GetTranslation(); const cv::Mat R2w = pKF2->GetRotation(); const cv::Mat t2w = pKF2->GetTranslation(); // Set Sim3 vertex g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap(); vSim3->_fix_scale=bFixScale; //两帧之间的位姿变化为待估计量 vSim3->setEstimate(g2oS12); vSim3->setId(0); //不进行固定参数 vSim3->setFixed(false); //提取相机内参参数 vSim3->_principle_point1[0] = K1.at<float>(0,2); vSim3->_principle_point1[1] = K1.at<float>(1,2); vSim3->_focal_length1[0] = K1.at<float>(0,0); vSim3->_focal_length1[1] = K1.at<float>(1,1); vSim3->_principle_point2[0] = K2.at<float>(0,2); vSim3->_principle_point2[1] = K2.at<float>(1,2); vSim3->_focal_length2[0] = K2.at<float>(0,0); vSim3->_focal_length2[1] = K2.at<float>(1,1); //加入图优化顶点 optimizer.addVertex(vSim3); // Set MapPoint vertices //建立地图点 顶点 const int N = vpMatches1.size(); const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches(); //定义两帧的公共地图点 vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12; vector<g2o::EdgeInverseSim3ProjectXYZ*> vpEdges21; //定义边的尺寸 vector<size_t> vnIndexEdge; //分配空间 vnIndexEdge.reserve(2*N); vpEdges12.reserve(2*N); vpEdges21.reserve(2*N); const float deltaHuber = sqrt(th2); int nCorrespondences = 0; //遍历所有匹配点 for(int i=0; i<N; i++) { if(!vpMatches1[i]) continue; MapPoint* pMP1 = vpMapPoints1[i]; MapPoint* pMP2 = vpMatches1[i]; const int id1 = 2*i+1; const int id2 = 2*(i+1); const int i2 = pMP2->GetIndexInKeyFrame(pKF2); //如果匹配的两帧看到的地图点都存在 if(pMP1 && pMP2) { //判断是不是好点 if(!pMP1->isBad() && !pMP2->isBad() && i2>=0) { //建立优化器 优化位姿1 g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ(); //提取世界坐标转化相机坐标 cv::Mat P3D1w = pMP1->GetWorldPos(); cv::Mat P3D1c = R1w*P3D1w + t1w; //待优化为相机位姿 vPoint1->setEstimate(Converter::toVector3d(P3D1c)); vPoint1->setId(id1); //固定原点坐标 vPoint1->setFixed(true); optimizer.addVertex(vPoint1); //优化位姿2 g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ(); cv::Mat P3D2w = pMP2->GetWorldPos(); cv::Mat P3D2c = R2w*P3D2w + t2w; vPoint2->setEstimate(Converter::toVector3d(P3D2c)); vPoint2->setId(id2); vPoint2->setFixed(true); optimizer.addVertex(vPoint2); } else continue; } else continue; //成功一个点加一个 nCorrespondences++; // Set edge x1 = S12*X2 //建立边,就是计算重投影误差 //提取像素坐标 Eigen::Matrix<double,2,1> obs1; const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i]; obs1 << kpUn1.pt.x, kpUn1.pt.y; //建立边,也就是误差 g2o::EdgeSim3ProjectXYZ* e12 = new g2o::EdgeSim3ProjectXYZ(); //连接的两个顶点 e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2))); e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0))); //观测值 e12->setMeasurement(obs1); //信息矩阵 const float &invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave]; e12->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare1); //鲁棒核 g2o::RobustKernelHuber* rk1 = new g2o::RobustKernelHuber; e12->setRobustKernel(rk1); rk1->setDelta(deltaHuber); //添加边 optimizer.addEdge(e12); // Set edge x2 = S21*X1 //反向投影在进行优化 Eigen::Matrix<double,2,1> obs2; const cv::KeyPoint &kpUn2 = pKF2->mvKeysUn[i2]; obs2 << kpUn2.pt.x, kpUn2.pt.y; g2o::EdgeInverseSim3ProjectXYZ* e21 = new g2o::EdgeInverseSim3ProjectXYZ(); e21->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id1))); e21->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0))); e21->setMeasurement(obs2); float invSigmaSquare2 = pKF2->mvInvLevelSigma2[kpUn2.octave]; e21->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare2); g2o::RobustKernelHuber* rk2 = new g2o::RobustKernelHuber; e21->setRobustKernel(rk2); rk2->setDelta(deltaHuber); optimizer.addEdge(e21); vpEdges12.push_back(e12); vpEdges21.push_back(e21); vnIndexEdge.push_back(i); } // Optimize! //开始优化 optimizer.initializeOptimization(); optimizer.optimize(5); // Check inliers //清除外点和误差过大的点 int nBad=0; for(size_t i=0; i<vpEdges12.size();i++) { g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i]; g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i]; if(!e12 || !e21) continue; if(e12->chi2()>th2 || e21->chi2()>th2) { size_t idx = vnIndexEdge[i]; vpMatches1[idx]=static_cast<MapPoint*>(NULL); optimizer.removeEdge(e12); optimizer.removeEdge(e21); vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL); vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(NULL); nBad++; } } //确定迭代次数 int nMoreIterations; if(nBad>0) nMoreIterations=10; else nMoreIterations=5; if(nCorrespondences-nBad<10) return 0; // Optimize again only with inliers //再次优化,只优化内点 optimizer.initializeOptimization(); optimizer.optimize(nMoreIterations); int nIn = 0; for(size_t i=0; i<vpEdges12.size();i++) { g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i]; g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i]; if(!e12 || !e21) continue; if(e12->chi2()>th2 || e21->chi2()>th2) { size_t idx = vnIndexEdge[i]; vpMatches1[idx]=static_cast<MapPoint*>(NULL); } else nIn++; } // Recover optimized Sim3 //恢复出位姿变化关系 g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0)); g2oS12= vSim3_recov->estimate(); return nIn;}
当我们用第m帧推算了回环帧n的位姿时,使得n的位姿漂移误差较小,但其实同时可以用第n帧来计算n-1帧的位姿,使n-1帧的位姿漂移误差也减小。因此,这里还要有一个位姿传播。
另外我们可以优化所有的位姿,也就是进行一个位姿图优化(由位姿变换构建位姿约束)。
最后,我们还可以进行一起全局所有变量的BA优化。
// 初始化g2o::LinearSolverEigen< g2o::BlockSolver_6_3::PoseMatrixType >* linearSolver = new g2o::LinearSolverEigen< g2o::BlockSolver_6_3::PoseMatrixType >();g2o::BlockSolver_6_3* blockSolver = new g2o::BlockSolver_6_3(linearSolver);g2o::OptimizationAlgorithmLevenberg* solver= new g2o::OptimizationAlgorithmLevenberg( blockSolver ); optimizer.setAlgorithm( solver );optimizer.setVerbose( false );// 初始顶点 g2o::VertexSE3* v = new g2o::VertexSE3(); v->setId( cutIndex ); v->setEstimate( Eigen::Isometry3d::Identity() ); v->setFixed( true ); optimizer.addVertex( v )// 添加新的节点(顶点)g2o::VertexSE3* vNext = new g2o::VertexSE3();vNext->setId(currFrame.frameID);vNext->setEstimate( Eigen::Isometry3d::Identity());optimizer.addVertex(vNext);// 添加边的数据g2o::EdgeSE3* edgeNew = new g2o::EdgeSE3();edgeNew->setVertex(0, optimizer.vertex(pousFrame.frameID));edgeNew->setVertex(1, optimizer.vertex(currFrame.frameID));edgeNew->setRobustKernel( new g2o::RobustKernelHuber() );//edgeNew->setInformation( Eigen::Matrix2d::Identity()); //the size is worthy to research// 信息矩阵// 两个节点的转换矩阵Eigen::Isometry3d T = cvMat2Eigen( m_result.rotaMatrix, m_result.tranMatrix ); edgeNew->setMeasurement(T);optimizer.addEdge(edgeNew);// 开始优化optimizer.initializeOptimization();optimizer.optimize(10);
3. 参考链接
https://baijiahao.baidu.com/s?id=1627227355343777871&wfr=spider&for=pc
https://zhuanlan.zhihu.com/p/45573552
https://blog.csdn.net/stihy/article/details/62219842
本文仅做学术分享,如有侵权,请联系删文。
3D视觉精品课程推荐:
2.面向自动驾驶领域的3D点云目标检测全栈学习路线!(单模态+多模态/数据+代码)
3.彻底搞透视觉三维重建:原理剖析、代码讲解、及优化改进
4.国内首个面向工业级实战的点云处理课程
5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
6.彻底搞懂视觉-惯性SLAM:基于VINS-Fusion正式开课啦
7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)
重磅!3DCVer-学术论文写作投稿 交流群已成立
扫码添加小助手微信,可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群,旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。
同时也可申请加入我们的细分方向交流群,目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。
一定要备注:研究方向+学校/公司+昵称,例如:”3D视觉 + 上海交大 + 静静“。请按照格式备注,可快速被通过且邀请进群。原创投稿也请联系。
▲长按加微信群或投稿
▲长按关注公众号
3D视觉从入门到精通知识星球:针对3D视觉领域的视频课程(三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等)、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕,更有各类大厂的算法工程人员进行技术指导。与此同时,星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息,打造成集技术与就业为一体的铁杆粉丝聚集区,近4000星球成员为创造更好的AI世界共同进步,知识星球入口:
学习3D视觉核心技术,扫描查看介绍,3天内无条件退款
圈里有高质量教程资料、答疑解惑、助你高效解决问题
觉得有用,麻烦给个赞和在看~