https://www.guyuehome.com/35679
1. 单目尺度漂移
单目SLAM的尺度漂移,简单来说就是单目获得的单张图片无法知道拍摄物体到相机的距离,必须通过对极几何求的两张图片的位姿,然后三角化出来尺度信息。而这些都依赖的是两帧图像之间的逆深度求解。
-
任选两张图(设为第0帧和第1帧),先提取关键点并匹配,再根据对极几何求本质矩阵E,再从E中恢复R,t。这个在单目VO里属于初始化的部分,其中得到的t是个单位向量,因为任意缩放t,极线约束都成立。你可以对t进行任意缩放。缩放完后,设第0帧的位置为相机坐标系原点,则第1帧的位置就可以由R, t得到。虽然这个t不是真值,但是可以固定下来。然后根据两帧的位置,可以对图像帧0和1中匹配的三维点进行三角化。(多点获取本质矩阵;恢复R,t;并根据R,t三角化计算出伪深度; )。
-
对之后的图像帧,就不再是利用对极几何求R,t了。依旧是先提取关键点再匹配哈, 但是这次匹配的上一帧的特征中,有些是已经被三角化过的,因此可以像之前答主说的用运动模型加BA求该图像帧与上一帧的位置关系,也可以用PnP求R,t。求出来R,t之后,再三角化该帧与上一帧的没被三角化过的匹配点。
我们可以发现在尺度漂移就是出现在第二步。因为对于新的图像帧,都是先计算其R,t,再利用该R,t三角化与之前图像帧匹配的点。问题就是,如果第一步算的R,t有误差,那么第二步三角化的三维点的深度就存在误差,也就是说其深度会存在一个缩放。对于新来的每一帧,都会有这样的问题。
2. SE3与SIM3对比
首先是欧式变换:
然后是相似变换(推导过程同上,故省略):
最后说两句,相似变换其实就只是个简单的数学变换而已,所以想思考为什么的时候,应该从数学上来思考才能获得更严谨的论证。相似变换的特点是改变原本物体的尺度比例(保形状),所以这个S就是要乘在R上,如果在t上,那只是对改变物体的平移量,并不改变形状的尺度比例,上面两个图已经说得很清楚了。这样我们可以发现通过逆深度的缩放将会被考虑在内。
$\color{red}{如果我们确定了标准的景深则不需要sim(3)的处理形式,直接se(3)即可得到一样的结果。}$
https://blog.csdn.net/weixin_51326570/article/details/115514771
2代码注释
//BRIEF 形成闭环时进行Sim3优化 //sim3用于回环检测时计算关键帧与回环帧之间的关系 R 、t、 s //相似变换群sim3 int 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; }
说明
https://zhehangt.github.io/2018/11/01/SLAM/ORBSLAM/ORBSLAM2LoopClosingRefine/
摘要
单目SLAM在运行的过程中,不但会积累旋转和平移上的误差,还会产生尺度上的漂移。ORB-SLAM2中,对于单目的尺度漂移,回环优化的时候会做特殊的处理,即Sim3的优化。
之前对于ORB-SLAM2中的回环优化流程进行过梳理,但未针对Sim3这部分深入理解,半年后补坑。
ComputeSim3()
在单目SLAM中,由于存在尺度漂移,在计算当前帧和回环帧之间的位姿关系时,将引入sim3变换,即在se3的基础上引入的尺度。
假设在当前帧和回环帧中,我们通过词袋模型找到了一系列的匹配点,那么如何通过计算一个sim3
的变换,把当前帧的位姿转换到一个正确的位姿?
在ORB-SLAM2中,计算这个sim3变换主要经历了如下几个步骤。
利用[1]中的公式,可以只用3对匹配点即可求解这个sim3的闭合解。这部分操作在void Sim3Solver::ComputeSim3(cv::Mat &P1, cv::Mat &P2)
中。闭合解的求解公式就不展开讲了,有兴趣的可以查看参考文献。
但这个sim3变换并不一定是准确的,因为当前帧和回环帧之间的匹配点是有可能存在外点的。因此在进行sim3闭合解求解时,利用Ransac迭代了5次,找出其中内点最多的一次sim3变换的求解。
2.即使在第一步中通过Ransac得到了相对准确的sim3变换。但由于只用了3对匹配点,我们仍然认为这个sim3变换存在比较大的误差。因此ORB-SLAM2中又进行了一次基于sim3的BA计算。具体过程是这样的。
首先基于步骤1中得到的sim3变换,对当前帧和回环帧中的ORB特征相互投影,从而找到更多的匹配点。这部分代码在ORBmatcher::SearchBySim3(...)
当前帧和回环帧之间有了更多的匹配点之后,既可以建立关于sim3的当前帧和回环帧中ORB特帧点的重投影误差,优化得到更加准确的sim3变换。这部分代码在Optimizer::OptimizeSim3()
中。
ORB-SLAM2中,这部分优化是借助于g2o
进行的。在这个优化问题中,待优化的变量为sim3变换,对应到g2o
中的顶点为g2o::VertexSim3Expmap
。这是个7维的变量(3维旋转、3维平移、1维尺度)。
约束关系为匹配点的重投影误差,包括投影到当前帧和投影到回环帧两种,对应到g2o
中的边为g2o::EdgeSim3ProjectXYZ
和g2o::EdgeInverseSim3ProjectXYZ
。这是个2维的重投影误差。
当前帧到回环帧投影残差的计算主要的代码为_error = obs-v1->cam_map1(project(v1->estimate().map(v2->estimate())))
。其中obs
为回环帧中的特征点图像坐标,v1
为sim3
变换,v2
为特征点在当前帧下相机坐标系下的三维空间坐标。
回环帧到当前帧的计算方法也是类似的_error = obs-v1->cam_map2(project(v1->estimate().inverse().map(v2->estimate())))
。只是对sim3
变换进行了取逆操作。sim3
的取逆公式为
其实匹配点的三维坐标也作为
g2o
的顶点g2o::VertexSBAPointXYZ
被加入到了优化问题中,只不过这些顶点是固定不优化的,可以理解成是观测值。
3.构建了步骤2中的优化问题之后,除了直接优化,ORB-SLAM2中还有一些提高优化结果准确性的操作。
首先是对步骤2中的优化问题迭代5次得到一个优化结果,基于这个优化结果得到每对匹配点的投影误差,如果投影误差大于10个像素,则认为这个匹配是个误匹配,将其从优化问题中删除。通过这种方式减少误匹配对优化结果的影响。
再去除误匹配后会再次进行优化。
如果最终当前帧和回环帧之间存在超过20个匹配点,则认为这个当前帧和回环帧的sim3
变换是有效的。
后续操作就是利用这个sim3
变换和回环帧的局部视图中的所有地图点,寻找更多的地图点匹配关系。如果最终匹配点超过40个,则认为当前帧和回环帧确实存在回环关系。
这里要重点说明一个公式。给定一个当前帧到回环帧的sim3
变换,如何把表示当前帧的se3
通过这个sim3
的变换得到新的se3
。一般的做法是把的se3
提升到sim3
,其中s=1
。然后在sim3
上做变换。在g2o
的代码中,两个sim3
变换是写成如下的方式
g2o::Sim3 gScm(R,t,s); //当前帧到回环帧的sim3变换 g2o::Sim3 gSmw(CpKF->GetRotation(),pKF->GetTranslation(),1.0); //当前帧的sim3,其中s=1 mg2oScw = gScm*gSmw; Sim3 operator *(const Sim3& other) const { Sim3 ret; ret.r = r*other.r; ret.t=s*(r*other.t)+t; ret.s=s*other.s; return ret; }
CorrectLoop()
当计算得到对当前帧需要修正的sim3变换后,后面就需要利用这个sim3
变换进行回环优化。
首先对于当前帧局部视图中的关键帧,利用当前帧和这些关键帧之间的位姿关系,即可以得到这些关键帧通过回环帧修正的位姿,这个位姿与mg2oScw
相同,保存形式为sim3
,保存在g2oCorrectedSiw
中。这部分代码为
cv::Mat Tic = Tiw*Twc; //i是当前帧,c是回环帧 cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3); cv::Mat tic = Tic.rowRange(0,3).col(3); g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0); g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw; //Pose corrected with the Sim3 of the loop closure CorrectedSim3[pKFi]=g2oCorrectedSiw;
之后即可进行PoseGraph
优化。这里的PoseGraph优化是Sim3
上的PoseGraph
,即每个关键帧待优化的变量是7维变量,除了3维旋转,3维平移外,还有1维尺度。
这部分代码在Optimizer::OptimizeEssentialGraph(...)
中。
对于当前帧局部视图中的关键帧,尺度的初始值为ComputeSim3()
中计算得到的s
,其他关键帧的初始值为1。Sim3
上的PoseGraph的残差与SE3
上的区别不大,在g2o
中是这样写的
void computeError() { const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[0]); const VertexSim3Expmap* v2 = static_cast<const VertexSim3Expmap*>(_vertices[1]); Sim3 C(_measurement); Sim3 error_=C*v1->estimate()*v2->estimate().inverse(); _error = error_.log(); }
要注意的是sim3
上的求逆操作和对数映射操作,与se3
上是不同的。
在sim3
进行PoseGraph求解之后,得到的位姿全部用sim3
来表示,这里就存在一个从sim3
到se3
的转换,
即Sim3:[sR, t;0, 1] -> SE3:[R, t/s;0, 1]
。
其他部分
上述的整个sim3回环优化过程省略了两部分的内容。
1.把当前帧和其局部视图下的关键帧通过sim3变换到一个正确的位姿之后,还需要对这些帧中能观测到的地图点进行更新和融合,并且重新建立局部视图关系。
2.进行了sim3的PoseGraph后,还会进行一次 Global BA 的优化,以期得到更加准确的关键帧位姿和地图点。
这部分内容与sim3的联系不是很大,因此没有展开详述,有兴趣的直接撸代码。