ORB-SLAM2学习笔记——闭环时Sim3优化
1、理论部分(待更新)
2、代码详解
int Optimizer::OptimizeSim3(KeyFrame *pKF1,
KeyFrame *pKF2,
vector<MapPoint *> &vpMatches1,
g2o::Sim3 &g2oS12,
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);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
const cv::Mat &K1 = pKF1->mK;
const cv::Mat &K2 = pKF2->mK;
const cv::Mat R1w = pKF1->GetRotation();
const cv::Mat t1w = pKF1->GetTranslation();
const cv::Mat R2w = pKF2->GetRotation();
const cv::Mat t2w = pKF2->GetTranslation();
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);
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)
{
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);
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++;
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);
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);
}
optimizer.initializeOptimization();
optimizer.optimize(5);
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;
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++;
}
g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));
g2oS12= vSim3_recov->estimate();
return nIn;
}