diff --git a/src/Optimizer.cc b/src/Optimizer.cc index 8fbb877..ea85f1b 100644 --- a/src/Optimizer.cc +++ b/src/Optimizer.cc @@ -1,25 +1,23 @@ /** -* This file is part of ORB-SLAM3 -* -* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. -* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. -* -* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public -* License as published by the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even -* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License along with ORB-SLAM3. -* If not, see . -*/ - + * This file is part of ORB-SLAM3 + * + * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. + * + * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public + * License as published by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even + * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with ORB-SLAM3. + * If not, see . + */ #include "Optimizer.h" - #include #include @@ -37,798 +35,37 @@ #include "G2oTypes.h" #include "Converter.h" -#include +#include #include "OptimizableTypes.h" - namespace ORB_SLAM3 { -bool sortByVal(const pair &a, const pair &b) +bool sortByVal(const pair &a, const pair &b) { return (a.second < b.second); } -void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust) -{ - vector vpKFs = pMap->GetAllKeyFrames(); - vector vpMP = pMap->GetAllMapPoints(); - BundleAdjustment(vpKFs,vpMP,nIterations,pbStopFlag, nLoopKF, bRobust); -} - - -void Optimizer::BundleAdjustment(const vector &vpKFs, const vector &vpMP, - int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust) -{ - vector vbNotIncludedMP; - vbNotIncludedMP.resize(vpMP.size()); - - Map* pMap = vpKFs[0]->GetMap(); - - g2o::SparseOptimizer optimizer; - g2o::BlockSolver_6_3::LinearSolverType * linearSolver; - - linearSolver = new g2o::LinearSolverEigen(); - - g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver); - - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); - optimizer.setAlgorithm(solver); - optimizer.setVerbose(false); - - if(pbStopFlag) - optimizer.setForceStopFlag(pbStopFlag); - - long unsigned int maxKFid = 0; - - const int nExpectedSize = (vpKFs.size())*vpMP.size(); - - vector vpEdgesMono; - vpEdgesMono.reserve(nExpectedSize); - - vector vpEdgesBody; - vpEdgesBody.reserve(nExpectedSize); - - vector vpEdgeKFMono; - vpEdgeKFMono.reserve(nExpectedSize); - - vector vpEdgeKFBody; - vpEdgeKFBody.reserve(nExpectedSize); - - vector vpMapPointEdgeMono; - vpMapPointEdgeMono.reserve(nExpectedSize); - - vector vpMapPointEdgeBody; - vpMapPointEdgeBody.reserve(nExpectedSize); - - vector vpEdgesStereo; - vpEdgesStereo.reserve(nExpectedSize); - - vector vpEdgeKFStereo; - vpEdgeKFStereo.reserve(nExpectedSize); - - vector vpMapPointEdgeStereo; - vpMapPointEdgeStereo.reserve(nExpectedSize); - - - // Set KeyFrame vertices - - for(size_t i=0; iisBad()) - continue; - g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap(); - Sophus::SE3 Tcw = pKF->GetPose(); - vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(),Tcw.translation().cast())); - vSE3->setId(pKF->mnId); - vSE3->setFixed(pKF->mnId==pMap->GetInitKFid()); - 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; iisBad()) - continue; - g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ(); - vPoint->setEstimate(pMP->GetWorldPos().cast()); - const int id = pMP->mnId+maxKFid+1; - vPoint->setId(id); - vPoint->setMarginalized(true); - optimizer.addVertex(vPoint); - - const map> observations = pMP->GetObservations(); - - int nEdges = 0; - //SET EDGES - for(map>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++) - { - KeyFrame* pKF = mit->first; - if(pKF->isBad() || pKF->mnId>maxKFid) - continue; - if(optimizer.vertex(id) == NULL || optimizer.vertex(pKF->mnId) == NULL) - continue; - nEdges++; - - const int leftIndex = get<0>(mit->second); - - if(leftIndex != -1 && pKF->mvuRight[get<0>(mit->second)]<0) - { - const cv::KeyPoint &kpUn = pKF->mvKeysUn[leftIndex]; - - Eigen::Matrix obs; - obs << kpUn.pt.x, kpUn.pt.y; - - ORB_SLAM3::EdgeSE3ProjectXYZ* e = new ORB_SLAM3::EdgeSE3ProjectXYZ(); - - e->setVertex(0, dynamic_cast(optimizer.vertex(id))); - e->setVertex(1, dynamic_cast(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->pCamera = pKF->mpCamera; - - optimizer.addEdge(e); - - vpEdgesMono.push_back(e); - vpEdgeKFMono.push_back(pKF); - vpMapPointEdgeMono.push_back(pMP); - } - else if(leftIndex != -1 && pKF->mvuRight[leftIndex] >= 0) //Stereo observation - { - const cv::KeyPoint &kpUn = pKF->mvKeysUn[leftIndex]; - - Eigen::Matrix obs; - const float kp_ur = pKF->mvuRight[get<0>(mit->second)]; - obs << kpUn.pt.x, kpUn.pt.y, kp_ur; - - g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ(); - - e->setVertex(0, dynamic_cast(optimizer.vertex(id))); - e->setVertex(1, dynamic_cast(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); - - vpEdgesStereo.push_back(e); - vpEdgeKFStereo.push_back(pKF); - vpMapPointEdgeStereo.push_back(pMP); - } - - if(pKF->mpCamera2){ - int rightIndex = get<1>(mit->second); - - if(rightIndex != -1 && rightIndex < pKF->mvKeysRight.size()){ - rightIndex -= pKF->NLeft; - - Eigen::Matrix obs; - cv::KeyPoint kp = pKF->mvKeysRight[rightIndex]; - obs << kp.pt.x, kp.pt.y; - - ORB_SLAM3::EdgeSE3ProjectXYZToBody *e = new ORB_SLAM3::EdgeSE3ProjectXYZToBody(); - - e->setVertex(0, dynamic_cast(optimizer.vertex(id))); - e->setVertex(1, dynamic_cast(optimizer.vertex(pKF->mnId))); - e->setMeasurement(obs); - const float &invSigma2 = pKF->mvInvLevelSigma2[kp.octave]; - e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); - - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; - e->setRobustKernel(rk); - rk->setDelta(thHuber2D); - - Sophus::SE3f Trl = pKF-> GetRelativePoseTrl(); - e->mTrl = g2o::SE3Quat(Trl.unit_quaternion().cast(), Trl.translation().cast()); - - e->pCamera = pKF->mpCamera2; - - optimizer.addEdge(e); - vpEdgesBody.push_back(e); - vpEdgeKFBody.push_back(pKF); - vpMapPointEdgeBody.push_back(pMP); - } - } - } - - - - if(nEdges==0) - { - optimizer.removeVertex(vPoint); - vbNotIncludedMP[i]=true; - } - else - { - vbNotIncludedMP[i]=false; - } - } - - // Optimize! - optimizer.setVerbose(false); - optimizer.initializeOptimization(); - optimizer.optimize(nIterations); - Verbose::PrintMess("BA: End of the optimization", Verbose::VERBOSITY_NORMAL); - - // Recover optimized data - //Keyframes - for(size_t i=0; iisBad()) - continue; - g2o::VertexSE3Expmap* vSE3 = static_cast(optimizer.vertex(pKF->mnId)); - - g2o::SE3Quat SE3quat = vSE3->estimate(); - if(nLoopKF==pMap->GetOriginKF()->mnId) - { - pKF->SetPose(Sophus::SE3f(SE3quat.rotation().cast(), SE3quat.translation().cast())); - } - else - { - pKF->mTcwGBA = Sophus::SE3d(SE3quat.rotation(),SE3quat.translation()).cast(); - pKF->mnBAGlobalForKF = nLoopKF; - - Sophus::SE3f mTwc = pKF->GetPoseInverse(); - Sophus::SE3f mTcGBA_c = pKF->mTcwGBA * mTwc; - Eigen::Vector3f vector_dist = mTcGBA_c.translation(); - double dist = vector_dist.norm(); - if(dist > 1) - { - int numMonoBadPoints = 0, numMonoOptPoints = 0; - int numStereoBadPoints = 0, numStereoOptPoints = 0; - vector vpMonoMPsOpt, vpStereoMPsOpt; - - for(size_t i2=0, iend=vpEdgesMono.size(); i2isBad()) - continue; - - if(e->chi2()>5.991 || !e->isDepthPositive()) - { - numMonoBadPoints++; - - } - else - { - numMonoOptPoints++; - vpMonoMPsOpt.push_back(pMP); - } - - } - - for(size_t i2=0, iend=vpEdgesStereo.size(); i2isBad()) - continue; - - if(e->chi2()>7.815 || !e->isDepthPositive()) - { - numStereoBadPoints++; - } - else - { - numStereoOptPoints++; - vpStereoMPsOpt.push_back(pMP); - } - } - } - } - } - - //Points - for(size_t i=0; iisBad()) - continue; - g2o::VertexSBAPointXYZ* vPoint = static_cast(optimizer.vertex(pMP->mnId+maxKFid+1)); - - if(nLoopKF==pMap->GetOriginKF()->mnId) - { - pMP->SetWorldPos(vPoint->estimate().cast()); - pMP->UpdateNormalAndDepth(); - } - else - { - pMP->mPosGBA = vPoint->estimate().cast(); - pMP->mnBAGlobalForKF = nLoopKF; - } - } -} - -void Optimizer::FullInertialBA(Map *pMap, int its, const bool bFixLocal, const long unsigned int nLoopId, bool *pbStopFlag, bool bInit, float priorG, float priorA, Eigen::VectorXd *vSingVal, bool *bHess) -{ - long unsigned int maxKFid = pMap->GetMaxKFid(); - const vector vpKFs = pMap->GetAllKeyFrames(); - const vector vpMPs = pMap->GetAllMapPoints(); - - // Setup optimizer - g2o::SparseOptimizer optimizer; - g2o::BlockSolverX::LinearSolverType * linearSolver; - - linearSolver = new g2o::LinearSolverEigen(); - - g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); - - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); - solver->setUserLambdaInit(1e-5); - optimizer.setAlgorithm(solver); - optimizer.setVerbose(false); - - if(pbStopFlag) - optimizer.setForceStopFlag(pbStopFlag); - - int nNonFixed = 0; - - // Set KeyFrame vertices - KeyFrame* pIncKF; - for(size_t i=0; imnId>maxKFid) - continue; - VertexPose * VP = new VertexPose(pKFi); - VP->setId(pKFi->mnId); - pIncKF=pKFi; - bool bFixed = false; - if(bFixLocal) - { - bFixed = (pKFi->mnBALocalForKF>=(maxKFid-1)) || (pKFi->mnBAFixedForKF>=(maxKFid-1)); - if(!bFixed) - nNonFixed++; - VP->setFixed(bFixed); - } - optimizer.addVertex(VP); - - if(pKFi->bImu) - { - VertexVelocity* VV = new VertexVelocity(pKFi); - VV->setId(maxKFid+3*(pKFi->mnId)+1); - VV->setFixed(bFixed); - optimizer.addVertex(VV); - if (!bInit) - { - VertexGyroBias* VG = new VertexGyroBias(pKFi); - VG->setId(maxKFid+3*(pKFi->mnId)+2); - VG->setFixed(bFixed); - optimizer.addVertex(VG); - VertexAccBias* VA = new VertexAccBias(pKFi); - VA->setId(maxKFid+3*(pKFi->mnId)+3); - VA->setFixed(bFixed); - optimizer.addVertex(VA); - } - } - } - - if (bInit) - { - VertexGyroBias* VG = new VertexGyroBias(pIncKF); - VG->setId(4*maxKFid+2); - VG->setFixed(false); - optimizer.addVertex(VG); - VertexAccBias* VA = new VertexAccBias(pIncKF); - VA->setId(4*maxKFid+3); - VA->setFixed(false); - optimizer.addVertex(VA); - } - - if(bFixLocal) - { - if(nNonFixed<3) - return; - } - - // IMU links - for(size_t i=0;imPrevKF) - { - Verbose::PrintMess("NOT INERTIAL LINK TO PREVIOUS FRAME!", Verbose::VERBOSITY_NORMAL); - continue; - } - - if(pKFi->mPrevKF && pKFi->mnId<=maxKFid) - { - if(pKFi->isBad() || pKFi->mPrevKF->mnId>maxKFid) - continue; - if(pKFi->bImu && pKFi->mPrevKF->bImu) - { - pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias()); - g2o::HyperGraph::Vertex* VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); - g2o::HyperGraph::Vertex* VV1 = optimizer.vertex(maxKFid+3*(pKFi->mPrevKF->mnId)+1); - - g2o::HyperGraph::Vertex* VG1; - g2o::HyperGraph::Vertex* VA1; - g2o::HyperGraph::Vertex* VG2; - g2o::HyperGraph::Vertex* VA2; - if (!bInit) - { - VG1 = optimizer.vertex(maxKFid+3*(pKFi->mPrevKF->mnId)+2); - VA1 = optimizer.vertex(maxKFid+3*(pKFi->mPrevKF->mnId)+3); - VG2 = optimizer.vertex(maxKFid+3*(pKFi->mnId)+2); - VA2 = optimizer.vertex(maxKFid+3*(pKFi->mnId)+3); - } - else - { - VG1 = optimizer.vertex(4*maxKFid+2); - VA1 = optimizer.vertex(4*maxKFid+3); - } - - g2o::HyperGraph::Vertex* VP2 = optimizer.vertex(pKFi->mnId); - g2o::HyperGraph::Vertex* VV2 = optimizer.vertex(maxKFid+3*(pKFi->mnId)+1); - - if (!bInit) - { - if(!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2 || !VG2 || !VA2) - { - cout << "Error" << VP1 << ", "<< VV1 << ", "<< VG1 << ", "<< VA1 << ", " << VP2 << ", " << VV2 << ", "<< VG2 << ", "<< VA2 <mpImuPreintegrated); - ei->setVertex(0,dynamic_cast(VP1)); - ei->setVertex(1,dynamic_cast(VV1)); - ei->setVertex(2,dynamic_cast(VG1)); - ei->setVertex(3,dynamic_cast(VA1)); - ei->setVertex(4,dynamic_cast(VP2)); - ei->setVertex(5,dynamic_cast(VV2)); - - g2o::RobustKernelHuber* rki = new g2o::RobustKernelHuber; - ei->setRobustKernel(rki); - rki->setDelta(sqrt(16.92)); - - optimizer.addEdge(ei); - - if (!bInit) - { - EdgeGyroRW* egr= new EdgeGyroRW(); - egr->setVertex(0,VG1); - egr->setVertex(1,VG2); - Eigen::Matrix3d InfoG = pKFi->mpImuPreintegrated->C.block<3,3>(9,9).cast().inverse(); - egr->setInformation(InfoG); - egr->computeError(); - optimizer.addEdge(egr); - - EdgeAccRW* ear = new EdgeAccRW(); - ear->setVertex(0,VA1); - ear->setVertex(1,VA2); - Eigen::Matrix3d InfoA = pKFi->mpImuPreintegrated->C.block<3,3>(12,12).cast().inverse(); - ear->setInformation(InfoA); - ear->computeError(); - optimizer.addEdge(ear); - } - } - else - cout << pKFi->mnId << " or " << pKFi->mPrevKF->mnId << " no imu" << endl; - } - } - - if (bInit) - { - g2o::HyperGraph::Vertex* VG = optimizer.vertex(4*maxKFid+2); - g2o::HyperGraph::Vertex* VA = optimizer.vertex(4*maxKFid+3); - - // Add prior to comon biases - Eigen::Vector3f bprior; - bprior.setZero(); - - EdgePriorAcc* epa = new EdgePriorAcc(bprior); - epa->setVertex(0,dynamic_cast(VA)); - double infoPriorA = priorA; // - epa->setInformation(infoPriorA*Eigen::Matrix3d::Identity()); - optimizer.addEdge(epa); - - EdgePriorGyro* epg = new EdgePriorGyro(bprior); - epg->setVertex(0,dynamic_cast(VG)); - double infoPriorG = priorG; // - epg->setInformation(infoPriorG*Eigen::Matrix3d::Identity()); - optimizer.addEdge(epg); - } - - const float thHuberMono = sqrt(5.991); - const float thHuberStereo = sqrt(7.815); - - const unsigned long iniMPid = maxKFid*5; - - vector vbNotIncludedMP(vpMPs.size(),false); - - for(size_t i=0; isetEstimate(pMP->GetWorldPos().cast()); - unsigned long id = pMP->mnId+iniMPid+1; - vPoint->setId(id); - vPoint->setMarginalized(true); - optimizer.addVertex(vPoint); - - const map> observations = pMP->GetObservations(); - - - bool bAllFixed = true; - - //Set edges - for(map>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) - { - KeyFrame* pKFi = mit->first; - - if(pKFi->mnId>maxKFid) - continue; - - if(!pKFi->isBad()) - { - const int leftIndex = get<0>(mit->second); - cv::KeyPoint kpUn; - - if(leftIndex != -1 && pKFi->mvuRight[get<0>(mit->second)]<0) // Monocular observation - { - kpUn = pKFi->mvKeysUn[leftIndex]; - Eigen::Matrix obs; - obs << kpUn.pt.x, kpUn.pt.y; - - EdgeMono* e = new EdgeMono(0); - - g2o::OptimizableGraph::Vertex* VP = dynamic_cast(optimizer.vertex(pKFi->mnId)); - if(bAllFixed) - if(!VP->fixed()) - bAllFixed=false; - - e->setVertex(0, dynamic_cast(optimizer.vertex(id))); - e->setVertex(1, VP); - e->setMeasurement(obs); - const float invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; - - e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); - - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; - e->setRobustKernel(rk); - rk->setDelta(thHuberMono); - - optimizer.addEdge(e); - } - else if(leftIndex != -1 && pKFi->mvuRight[leftIndex] >= 0) // stereo observation - { - kpUn = pKFi->mvKeysUn[leftIndex]; - const float kp_ur = pKFi->mvuRight[leftIndex]; - Eigen::Matrix obs; - obs << kpUn.pt.x, kpUn.pt.y, kp_ur; - - EdgeStereo* e = new EdgeStereo(0); - - g2o::OptimizableGraph::Vertex* VP = dynamic_cast(optimizer.vertex(pKFi->mnId)); - if(bAllFixed) - if(!VP->fixed()) - bAllFixed=false; - - e->setVertex(0, dynamic_cast(optimizer.vertex(id))); - e->setVertex(1, VP); - e->setMeasurement(obs); - const float invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; - - e->setInformation(Eigen::Matrix3d::Identity()*invSigma2); - - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; - e->setRobustKernel(rk); - rk->setDelta(thHuberStereo); - - optimizer.addEdge(e); - } - - if(pKFi->mpCamera2){ // Monocular right observation - int rightIndex = get<1>(mit->second); - - if(rightIndex != -1 && rightIndex < pKFi->mvKeysRight.size()){ - rightIndex -= pKFi->NLeft; - - Eigen::Matrix obs; - kpUn = pKFi->mvKeysRight[rightIndex]; - obs << kpUn.pt.x, kpUn.pt.y; - - EdgeMono *e = new EdgeMono(1); - - g2o::OptimizableGraph::Vertex* VP = dynamic_cast(optimizer.vertex(pKFi->mnId)); - if(bAllFixed) - if(!VP->fixed()) - bAllFixed=false; - - e->setVertex(0, dynamic_cast(optimizer.vertex(id))); - e->setVertex(1, VP); - e->setMeasurement(obs); - const float invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; - e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); - - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; - e->setRobustKernel(rk); - rk->setDelta(thHuberMono); - - optimizer.addEdge(e); - } - } - } - } - - if(bAllFixed) - { - optimizer.removeVertex(vPoint); - vbNotIncludedMP[i]=true; - } - } - - if(pbStopFlag) - if(*pbStopFlag) - return; - - - optimizer.initializeOptimization(); - optimizer.optimize(its); - - - // Recover optimized data - //Keyframes - for(size_t i=0; imnId>maxKFid) - continue; - VertexPose* VP = static_cast(optimizer.vertex(pKFi->mnId)); - if(nLoopId==0) - { - Sophus::SE3f Tcw(VP->estimate().Rcw[0].cast(), VP->estimate().tcw[0].cast()); - pKFi->SetPose(Tcw); - } - else - { - pKFi->mTcwGBA = Sophus::SE3f(VP->estimate().Rcw[0].cast(),VP->estimate().tcw[0].cast()); - pKFi->mnBAGlobalForKF = nLoopId; - - } - if(pKFi->bImu) - { - VertexVelocity* VV = static_cast(optimizer.vertex(maxKFid+3*(pKFi->mnId)+1)); - if(nLoopId==0) - { - pKFi->SetVelocity(VV->estimate().cast()); - } - else - { - pKFi->mVwbGBA = VV->estimate().cast(); - } - - VertexGyroBias* VG; - VertexAccBias* VA; - if (!bInit) - { - VG = static_cast(optimizer.vertex(maxKFid+3*(pKFi->mnId)+2)); - VA = static_cast(optimizer.vertex(maxKFid+3*(pKFi->mnId)+3)); - } - else - { - VG = static_cast(optimizer.vertex(4*maxKFid+2)); - VA = static_cast(optimizer.vertex(4*maxKFid+3)); - } - - Vector6d vb; - vb << VG->estimate(), VA->estimate(); - IMU::Bias b (vb[3],vb[4],vb[5],vb[0],vb[1],vb[2]); - if(nLoopId==0) - { - pKFi->SetNewBias(b); - } - else - { - pKFi->mBiasGBA = b; - } - } - } - - //Points - for(size_t i=0; i(optimizer.vertex(pMP->mnId+iniMPid+1)); - - if(nLoopId==0) - { - pMP->SetWorldPos(vPoint->estimate().cast()); - pMP->UpdateNormalAndDepth(); - } - else - { - pMP->mPosGBA = vPoint->estimate().cast(); - pMP->mnBAGlobalForKF = nLoopId; - } - - } - - pMap->IncreaseChangeIndex(); -} - +/**************************************以下为单帧优化**************************************************************/ int Optimizer::PoseOptimization(Frame *pFrame) { g2o::SparseOptimizer optimizer; - g2o::BlockSolver_6_3::LinearSolverType * linearSolver; + g2o::BlockSolver_6_3::LinearSolverType *linearSolver; linearSolver = new g2o::LinearSolverDense(); - g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver); + g2o::BlockSolver_6_3 *solver_ptr = new g2o::BlockSolver_6_3(linearSolver); - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); optimizer.setAlgorithm(solver); - int nInitialCorrespondences=0; + int nInitialCorrespondences = 0; // Set Frame vertex - g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap(); + g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap(); Sophus::SE3 Tcw = pFrame->GetPose(); - vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(),Tcw.translation().cast())); + vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(), Tcw.translation().cast())); vSE3->setId(0); vSE3->setFixed(false); optimizer.addVertex(vSE3); @@ -836,7 +73,7 @@ int Optimizer::PoseOptimization(Frame *pFrame) // Set MapPoint vertices const int N = pFrame->N; - vector vpEdgesMono; + vector vpEdgesMono; vector vpEdgesMono_FHR; vector vnIndexEdgeMono, vnIndexEdgeRight; vpEdgesMono.reserve(N); @@ -844,7 +81,7 @@ int Optimizer::PoseOptimization(Frame *pFrame) vnIndexEdgeMono.reserve(N); vnIndexEdgeRight.reserve(N); - vector vpEdgesStereo; + vector vpEdgesStereo; vector vnIndexEdgeStereo; vpEdgesStereo.reserve(N); vnIndexEdgeStereo.reserve(N); @@ -853,333 +90,1222 @@ int Optimizer::PoseOptimization(Frame *pFrame) const float deltaStereo = sqrt(7.815); { - unique_lock lock(MapPoint::mGlobalMutex); + unique_lock lock(MapPoint::mGlobalMutex); - for(int i=0; imvpMapPoints[i]; - if(pMP) + for (int i = 0; i < N; i++) { - //Conventional SLAM - if(!pFrame->mpCamera2){ - // Monocular observation - if(pFrame->mvuRight[i]<0) + MapPoint *pMP = pFrame->mvpMapPoints[i]; + if (pMP) + { + // Conventional SLAM + if (!pFrame->mpCamera2) + { + // Monocular observation + if (pFrame->mvuRight[i] < 0) + { + nInitialCorrespondences++; + pFrame->mvbOutlier[i] = false; + + Eigen::Matrix obs; + const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i]; + obs << kpUn.pt.x, kpUn.pt.y; + + ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose *e = new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(0))); + e->setMeasurement(obs); + const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(deltaMono); + + e->pCamera = pFrame->mpCamera; + e->Xw = pMP->GetWorldPos().cast(); + + optimizer.addEdge(e); + + vpEdgesMono.push_back(e); + vnIndexEdgeMono.push_back(i); + } + else // Stereo observation + { + nInitialCorrespondences++; + pFrame->mvbOutlier[i] = false; + + Eigen::Matrix obs; + const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i]; + const float &kp_ur = pFrame->mvuRight[i]; + obs << kpUn.pt.x, kpUn.pt.y, kp_ur; + + g2o::EdgeStereoSE3ProjectXYZOnlyPose *e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(0))); + e->setMeasurement(obs); + const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]; + Eigen::Matrix3d Info = Eigen::Matrix3d::Identity() * invSigma2; + e->setInformation(Info); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(deltaStereo); + + e->fx = pFrame->fx; + e->fy = pFrame->fy; + e->cx = pFrame->cx; + e->cy = pFrame->cy; + e->bf = pFrame->mbf; + e->Xw = pMP->GetWorldPos().cast(); + + optimizer.addEdge(e); + + vpEdgesStereo.push_back(e); + vnIndexEdgeStereo.push_back(i); + } + } + // SLAM with respect a rigid body + else { nInitialCorrespondences++; + + cv::KeyPoint kpUn; + + if (i < pFrame->Nleft) + { // Left camera observation + kpUn = pFrame->mvKeys[i]; + + pFrame->mvbOutlier[i] = false; + + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y; + + ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose *e = new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(0))); + e->setMeasurement(obs); + const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(deltaMono); + + e->pCamera = pFrame->mpCamera; + e->Xw = pMP->GetWorldPos().cast(); + + optimizer.addEdge(e); + + vpEdgesMono.push_back(e); + vnIndexEdgeMono.push_back(i); + } + else + { + kpUn = pFrame->mvKeysRight[i - pFrame->Nleft]; + + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y; + + pFrame->mvbOutlier[i] = false; + + ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody *e = new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(0))); + e->setMeasurement(obs); + const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(deltaMono); + + e->pCamera = pFrame->mpCamera2; + e->Xw = pMP->GetWorldPos().cast(); + + e->mTrl = g2o::SE3Quat(pFrame->GetRelativePoseTrl().unit_quaternion().cast(), pFrame->GetRelativePoseTrl().translation().cast()); + + optimizer.addEdge(e); + + vpEdgesMono_FHR.push_back(e); + vnIndexEdgeRight.push_back(i); + } + } + } + } + } + + if (nInitialCorrespondences < 3) + return 0; + + // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier + // At the next optimization, outliers are not included, but at the end they can be classified as inliers again. + const float chi2Mono[4] = {5.991, 5.991, 5.991, 5.991}; + const float chi2Stereo[4] = {7.815, 7.815, 7.815, 7.815}; + const int its[4] = {10, 10, 10, 10}; + + int nBad = 0; + for (size_t it = 0; it < 4; it++) + { + Tcw = pFrame->GetPose(); + vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(), Tcw.translation().cast())); + + optimizer.initializeOptimization(0); + optimizer.optimize(its[it]); + + nBad = 0; + for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) + { + ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose *e = vpEdgesMono[i]; + + const size_t idx = vnIndexEdgeMono[i]; + + if (pFrame->mvbOutlier[idx]) + { + e->computeError(); + } + + const float chi2 = e->chi2(); + + if (chi2 > chi2Mono[it]) + { + pFrame->mvbOutlier[idx] = true; + e->setLevel(1); + nBad++; + } + else + { + pFrame->mvbOutlier[idx] = false; + e->setLevel(0); + } + + if (it == 2) + e->setRobustKernel(0); + } + + for (size_t i = 0, iend = vpEdgesMono_FHR.size(); i < iend; i++) + { + ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody *e = vpEdgesMono_FHR[i]; + + const size_t idx = vnIndexEdgeRight[i]; + + if (pFrame->mvbOutlier[idx]) + { + e->computeError(); + } + + const float chi2 = e->chi2(); + + if (chi2 > chi2Mono[it]) + { + pFrame->mvbOutlier[idx] = true; + e->setLevel(1); + nBad++; + } + else + { + pFrame->mvbOutlier[idx] = false; + e->setLevel(0); + } + + if (it == 2) + e->setRobustKernel(0); + } + + for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++) + { + g2o::EdgeStereoSE3ProjectXYZOnlyPose *e = vpEdgesStereo[i]; + + const size_t idx = vnIndexEdgeStereo[i]; + + if (pFrame->mvbOutlier[idx]) + { + e->computeError(); + } + + const float chi2 = e->chi2(); + + if (chi2 > chi2Stereo[it]) + { + pFrame->mvbOutlier[idx] = true; + e->setLevel(1); + nBad++; + } + else + { + e->setLevel(0); + pFrame->mvbOutlier[idx] = false; + } + + if (it == 2) + e->setRobustKernel(0); + } + + if (optimizer.edges().size() < 10) + break; + } + + // Recover optimized pose and return number of inliers + g2o::VertexSE3Expmap *vSE3_recov = static_cast(optimizer.vertex(0)); + g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate(); + Sophus::SE3 pose(SE3quat_recov.rotation().cast(), + SE3quat_recov.translation().cast()); + pFrame->SetPose(pose); + + return nInitialCorrespondences - nBad; +} + +int Optimizer::PoseInertialOptimizationLastKeyFrame(Frame *pFrame, bool bRecInit) +{ + g2o::SparseOptimizer optimizer; + g2o::BlockSolverX::LinearSolverType *linearSolver; + + linearSolver = new g2o::LinearSolverDense(); + + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); + + g2o::OptimizationAlgorithmGaussNewton *solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr); + optimizer.setVerbose(false); + optimizer.setAlgorithm(solver); + + int nInitialMonoCorrespondences = 0; + int nInitialStereoCorrespondences = 0; + int nInitialCorrespondences = 0; + + // Set Frame vertex + VertexPose *VP = new VertexPose(pFrame); + VP->setId(0); + VP->setFixed(false); + optimizer.addVertex(VP); + VertexVelocity *VV = new VertexVelocity(pFrame); + VV->setId(1); + VV->setFixed(false); + optimizer.addVertex(VV); + VertexGyroBias *VG = new VertexGyroBias(pFrame); + VG->setId(2); + VG->setFixed(false); + optimizer.addVertex(VG); + VertexAccBias *VA = new VertexAccBias(pFrame); + VA->setId(3); + VA->setFixed(false); + optimizer.addVertex(VA); + + // Set MapPoint vertices + const int N = pFrame->N; + const int Nleft = pFrame->Nleft; + const bool bRight = (Nleft != -1); + + vector vpEdgesMono; + vector vpEdgesStereo; + vector vnIndexEdgeMono; + vector vnIndexEdgeStereo; + vpEdgesMono.reserve(N); + vpEdgesStereo.reserve(N); + vnIndexEdgeMono.reserve(N); + vnIndexEdgeStereo.reserve(N); + + const float thHuberMono = sqrt(5.991); + const float thHuberStereo = sqrt(7.815); + + { + unique_lock lock(MapPoint::mGlobalMutex); + + for (int i = 0; i < N; i++) + { + MapPoint *pMP = pFrame->mvpMapPoints[i]; + if (pMP) + { + cv::KeyPoint kpUn; + + // Left monocular observation + // 这里说的Left monocular包含两种情况：1.单目情况 2.两个相机情况下的相机1 + if ((!bRight && pFrame->mvuRight[i] < 0) || i < Nleft) + { + //如果是两个相机情况下的相机1 + if (i < Nleft) // pair left-right + kpUn = pFrame->mvKeys[i]; + else + kpUn = pFrame->mvKeysUn[i]; + + nInitialMonoCorrespondences++; pFrame->mvbOutlier[i] = false; - Eigen::Matrix obs; - const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i]; + Eigen::Matrix obs; obs << kpUn.pt.x, kpUn.pt.y; - ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose* e = new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose(); + EdgeMonoOnlyPose *e = new EdgeMonoOnlyPose(pMP->GetWorldPos(), 0); - e->setVertex(0, dynamic_cast(optimizer.vertex(0))); + e->setVertex(0, VP); e->setMeasurement(obs); - const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]; - e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; + // Add here uncerteinty + const float unc2 = pFrame->mpCamera->uncertainty2(obs); + + const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); - rk->setDelta(deltaMono); - - e->pCamera = pFrame->mpCamera; - e->Xw = pMP->GetWorldPos().cast(); + rk->setDelta(thHuberMono); optimizer.addEdge(e); vpEdgesMono.push_back(e); vnIndexEdgeMono.push_back(i); } - else // Stereo observation + // Stereo observation + else if (!bRight) { - nInitialCorrespondences++; + nInitialStereoCorrespondences++; pFrame->mvbOutlier[i] = false; - Eigen::Matrix obs; - const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i]; - const float &kp_ur = pFrame->mvuRight[i]; + kpUn = pFrame->mvKeysUn[i]; + const float kp_ur = pFrame->mvuRight[i]; + Eigen::Matrix obs; obs << kpUn.pt.x, kpUn.pt.y, kp_ur; - g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose(); + EdgeStereoOnlyPose *e = new EdgeStereoOnlyPose(pMP->GetWorldPos()); - e->setVertex(0, dynamic_cast(optimizer.vertex(0))); + e->setVertex(0, VP); e->setMeasurement(obs); - const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]; - Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2; - e->setInformation(Info); - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; + // Add here uncerteinty + const float unc2 = pFrame->mpCamera->uncertainty2(obs.head(2)); + + const float &invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2; + e->setInformation(Eigen::Matrix3d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); - rk->setDelta(deltaStereo); - - e->fx = pFrame->fx; - e->fy = pFrame->fy; - e->cx = pFrame->cx; - e->cy = pFrame->cy; - e->bf = pFrame->mbf; - e->Xw = pMP->GetWorldPos().cast(); + rk->setDelta(thHuberStereo); optimizer.addEdge(e); vpEdgesStereo.push_back(e); vnIndexEdgeStereo.push_back(i); } - } - //SLAM with respect a rigid body - else{ - nInitialCorrespondences++; - - cv::KeyPoint kpUn; - - if (i < pFrame->Nleft) { //Left camera observation - kpUn = pFrame->mvKeys[i]; + // Right monocular observation + if (bRight && i >= Nleft) + { + nInitialMonoCorrespondences++; pFrame->mvbOutlier[i] = false; + kpUn = pFrame->mvKeysRight[i - Nleft]; Eigen::Matrix obs; obs << kpUn.pt.x, kpUn.pt.y; - ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose *e = new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose(); + EdgeMonoOnlyPose *e = new EdgeMonoOnlyPose(pMP->GetWorldPos(), 1); - e->setVertex(0, dynamic_cast(optimizer.vertex(0))); + e->setVertex(0, VP); e->setMeasurement(obs); - const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]; + + // Add here uncerteinty + const float unc2 = pFrame->mpCamera->uncertainty2(obs); + + const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2; e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); - rk->setDelta(deltaMono); - - e->pCamera = pFrame->mpCamera; - e->Xw = pMP->GetWorldPos().cast(); + rk->setDelta(thHuberMono); optimizer.addEdge(e); vpEdgesMono.push_back(e); vnIndexEdgeMono.push_back(i); } - else { - kpUn = pFrame->mvKeysRight[i - pFrame->Nleft]; + } + } + } + nInitialCorrespondences = nInitialMonoCorrespondences + nInitialStereoCorrespondences; + + KeyFrame *pKF = pFrame->mpLastKeyFrame; + VertexPose *VPk = new VertexPose(pKF); + VPk->setId(4); + VPk->setFixed(true); + optimizer.addVertex(VPk); + VertexVelocity *VVk = new VertexVelocity(pKF); + VVk->setId(5); + VVk->setFixed(true); + optimizer.addVertex(VVk); + VertexGyroBias *VGk = new VertexGyroBias(pKF); + VGk->setId(6); + VGk->setFixed(true); + optimizer.addVertex(VGk); + VertexAccBias *VAk = new VertexAccBias(pKF); + VAk->setId(7); + VAk->setFixed(true); + optimizer.addVertex(VAk); + + EdgeInertial *ei = new EdgeInertial(pFrame->mpImuPreintegrated); + + ei->setVertex(0, VPk); + ei->setVertex(1, VVk); + ei->setVertex(2, VGk); + ei->setVertex(3, VAk); + ei->setVertex(4, VP); + ei->setVertex(5, VV); + optimizer.addEdge(ei); + + EdgeGyroRW *egr = new EdgeGyroRW(); + egr->setVertex(0, VGk); + egr->setVertex(1, VG); + Eigen::Matrix3d InfoG = pFrame->mpImuPreintegrated->C.block<3, 3>(9, 9).cast().inverse(); + egr->setInformation(InfoG); + optimizer.addEdge(egr); + + EdgeAccRW *ear = new EdgeAccRW(); + ear->setVertex(0, VAk); + ear->setVertex(1, VA); + Eigen::Matrix3d InfoA = pFrame->mpImuPreintegrated->C.block<3, 3>(12, 12).cast().inverse(); + ear->setInformation(InfoA); + optimizer.addEdge(ear); + + // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier + // At the next optimization, outliers are not included, but at the end they can be classified as inliers again. + float chi2Mono[4] = {12, 7.5, 5.991, 5.991}; + float chi2Stereo[4] = {15.6, 9.8, 7.815, 7.815}; + + int its[4] = {10, 10, 10, 10}; + + int nBad = 0; + int nBadMono = 0; + int nBadStereo = 0; + int nInliersMono = 0; + int nInliersStereo = 0; + int nInliers = 0; + for (size_t it = 0; it < 4; it++) + { + optimizer.initializeOptimization(0); + optimizer.optimize(its[it]); + + nBad = 0; + nBadMono = 0; + nBadStereo = 0; + nInliers = 0; + nInliersMono = 0; + nInliersStereo = 0; + float chi2close = 1.5 * chi2Mono[it]; + + // For monocular observations + for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) + { + EdgeMonoOnlyPose *e = vpEdgesMono[i]; + + const size_t idx = vnIndexEdgeMono[i]; + + if (pFrame->mvbOutlier[idx]) + { + e->computeError(); + } + + const float chi2 = e->chi2(); + bool bClose = pFrame->mvpMapPoints[idx]->mTrackDepth < 10.f; + + if ((chi2 > chi2Mono[it] && !bClose) || (bClose && chi2 > chi2close) || !e->isDepthPositive()) + { + pFrame->mvbOutlier[idx] = true; + e->setLevel(1); + nBadMono++; + } + else + { + pFrame->mvbOutlier[idx] = false; + e->setLevel(0); + nInliersMono++; + } + + if (it == 2) + e->setRobustKernel(0); + } + + // For stereo observations + for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++) + { + EdgeStereoOnlyPose *e = vpEdgesStereo[i]; + + const size_t idx = vnIndexEdgeStereo[i]; + + if (pFrame->mvbOutlier[idx]) + { + e->computeError(); + } + + const float chi2 = e->chi2(); + + if (chi2 > chi2Stereo[it]) + { + pFrame->mvbOutlier[idx] = true; + e->setLevel(1); // not included in next optimization + nBadStereo++; + } + else + { + pFrame->mvbOutlier[idx] = false; + e->setLevel(0); + nInliersStereo++; + } + + if (it == 2) + e->setRobustKernel(0); + } + + nInliers = nInliersMono + nInliersStereo; + nBad = nBadMono + nBadStereo; + + if (optimizer.edges().size() < 10) + { + break; + } + } + + // If not too much tracks, recover not too bad points + if ((nInliers < 30) && !bRecInit) + { + nBad = 0; + const float chi2MonoOut = 18.f; + const float chi2StereoOut = 24.f; + EdgeMonoOnlyPose *e1; + EdgeStereoOnlyPose *e2; + for (size_t i = 0, iend = vnIndexEdgeMono.size(); i < iend; i++) + { + const size_t idx = vnIndexEdgeMono[i]; + e1 = vpEdgesMono[i]; + e1->computeError(); + if (e1->chi2() < chi2MonoOut) + pFrame->mvbOutlier[idx] = false; + else + nBad++; + } + for (size_t i = 0, iend = vnIndexEdgeStereo.size(); i < iend; i++) + { + const size_t idx = vnIndexEdgeStereo[i]; + e2 = vpEdgesStereo[i]; + e2->computeError(); + if (e2->chi2() < chi2StereoOut) + pFrame->mvbOutlier[idx] = false; + else + nBad++; + } + } + + // Recover optimized pose, velocity and biases + pFrame->SetImuPoseVelocity(VP->estimate().Rwb.cast(), VP->estimate().twb.cast(), VV->estimate().cast()); + Vector6d b; + b << VG->estimate(), VA->estimate(); + pFrame->mImuBias = IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2]); + + // Recover Hessian, marginalize keyFframe states and generate new prior for frame + Eigen::Matrix H; + H.setZero(); + + H.block<9, 9>(0, 0) += ei->GetHessian2(); + H.block<3, 3>(9, 9) += egr->GetHessian2(); + H.block<3, 3>(12, 12) += ear->GetHessian2(); + + int tot_in = 0, tot_out = 0; + for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) + { + EdgeMonoOnlyPose *e = vpEdgesMono[i]; + + const size_t idx = vnIndexEdgeMono[i]; + + if (!pFrame->mvbOutlier[idx]) + { + H.block<6, 6>(0, 0) += e->GetHessian(); + tot_in++; + } + else + tot_out++; + } + + for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++) + { + EdgeStereoOnlyPose *e = vpEdgesStereo[i]; + + const size_t idx = vnIndexEdgeStereo[i]; + + if (!pFrame->mvbOutlier[idx]) + { + H.block<6, 6>(0, 0) += e->GetHessian(); + tot_in++; + } + else + tot_out++; + } + + pFrame->mpcpi = new ConstraintPoseImu(VP->estimate().Rwb, VP->estimate().twb, VV->estimate(), VG->estimate(), VA->estimate(), H); + + return nInitialCorrespondences - nBad; +} + +int Optimizer::PoseInertialOptimizationLastFrame(Frame *pFrame, bool bRecInit) +{ + g2o::SparseOptimizer optimizer; + g2o::BlockSolverX::LinearSolverType *linearSolver; + + linearSolver = new g2o::LinearSolverDense(); + + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); + + g2o::OptimizationAlgorithmGaussNewton *solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr); + optimizer.setAlgorithm(solver); + optimizer.setVerbose(false); + + int nInitialMonoCorrespondences = 0; + int nInitialStereoCorrespondences = 0; + int nInitialCorrespondences = 0; + + // Set Current Frame vertex + VertexPose *VP = new VertexPose(pFrame); + VP->setId(0); + VP->setFixed(false); + optimizer.addVertex(VP); + VertexVelocity *VV = new VertexVelocity(pFrame); + VV->setId(1); + VV->setFixed(false); + optimizer.addVertex(VV); + VertexGyroBias *VG = new VertexGyroBias(pFrame); + VG->setId(2); + VG->setFixed(false); + optimizer.addVertex(VG); + VertexAccBias *VA = new VertexAccBias(pFrame); + VA->setId(3); + VA->setFixed(false); + optimizer.addVertex(VA); + + // Set MapPoint vertices + const int N = pFrame->N; + const int Nleft = pFrame->Nleft; + const bool bRight = (Nleft != -1); + + vector vpEdgesMono; + vector vpEdgesStereo; + vector vnIndexEdgeMono; + vector vnIndexEdgeStereo; + vpEdgesMono.reserve(N); + vpEdgesStereo.reserve(N); + vnIndexEdgeMono.reserve(N); + vnIndexEdgeStereo.reserve(N); + + const float thHuberMono = sqrt(5.991); + const float thHuberStereo = sqrt(7.815); + + { + unique_lock lock(MapPoint::mGlobalMutex); + + for (int i = 0; i < N; i++) + { + MapPoint *pMP = pFrame->mvpMapPoints[i]; + if (pMP) + { + cv::KeyPoint kpUn; + // Left monocular observation + // 这里说的Left monocular包含两种情况：1.单目情况 2.两个相机情况下的相机1 + if ((!bRight && pFrame->mvuRight[i] < 0) || i < Nleft) + { + //如果是两个相机情况下的相机1 + if (i < Nleft) // pair left-right + kpUn = pFrame->mvKeys[i]; + else + kpUn = pFrame->mvKeysUn[i]; + + nInitialMonoCorrespondences++; + pFrame->mvbOutlier[i] = false; Eigen::Matrix obs; obs << kpUn.pt.x, kpUn.pt.y; - pFrame->mvbOutlier[i] = false; + EdgeMonoOnlyPose *e = new EdgeMonoOnlyPose(pMP->GetWorldPos(), 0); - ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody *e = new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody(); - - e->setVertex(0, dynamic_cast(optimizer.vertex(0))); + e->setVertex(0, VP); e->setMeasurement(obs); - const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]; + + // Add here uncerteinty + const float unc2 = pFrame->mpCamera->uncertainty2(obs); + + const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2; e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); - rk->setDelta(deltaMono); - - e->pCamera = pFrame->mpCamera2; - e->Xw = pMP->GetWorldPos().cast(); - - e->mTrl = g2o::SE3Quat(pFrame->GetRelativePoseTrl().unit_quaternion().cast(), pFrame->GetRelativePoseTrl().translation().cast()); + rk->setDelta(thHuberMono); optimizer.addEdge(e); - vpEdgesMono_FHR.push_back(e); - vnIndexEdgeRight.push_back(i); + vpEdgesMono.push_back(e); + vnIndexEdgeMono.push_back(i); + } + // Stereo observation + else if (!bRight) + { + nInitialStereoCorrespondences++; + pFrame->mvbOutlier[i] = false; + + kpUn = pFrame->mvKeysUn[i]; + const float kp_ur = pFrame->mvuRight[i]; + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y, kp_ur; + + EdgeStereoOnlyPose *e = new EdgeStereoOnlyPose(pMP->GetWorldPos()); + + e->setVertex(0, VP); + e->setMeasurement(obs); + + // Add here uncerteinty + const float unc2 = pFrame->mpCamera->uncertainty2(obs.head(2)); + + const float &invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2; + e->setInformation(Eigen::Matrix3d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberStereo); + + optimizer.addEdge(e); + + vpEdgesStereo.push_back(e); + vnIndexEdgeStereo.push_back(i); + } + + // Right monocular observation + if (bRight && i >= Nleft) + { + nInitialMonoCorrespondences++; + pFrame->mvbOutlier[i] = false; + + kpUn = pFrame->mvKeysRight[i - Nleft]; + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y; + + EdgeMonoOnlyPose *e = new EdgeMonoOnlyPose(pMP->GetWorldPos(), 1); + + e->setVertex(0, VP); + e->setMeasurement(obs); + + // Add here uncerteinty + const float unc2 = pFrame->mpCamera->uncertainty2(obs); + + const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave] / unc2; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberMono); + + optimizer.addEdge(e); + + vpEdgesMono.push_back(e); + vnIndexEdgeMono.push_back(i); } } } } - } - if(nInitialCorrespondences<3) - return 0; + nInitialCorrespondences = nInitialMonoCorrespondences + nInitialStereoCorrespondences; + + // Set Previous Frame Vertex + Frame *pFp = pFrame->mpPrevFrame; + + VertexPose *VPk = new VertexPose(pFp); + VPk->setId(4); + VPk->setFixed(false); + optimizer.addVertex(VPk); + VertexVelocity *VVk = new VertexVelocity(pFp); + VVk->setId(5); + VVk->setFixed(false); + optimizer.addVertex(VVk); + VertexGyroBias *VGk = new VertexGyroBias(pFp); + VGk->setId(6); + VGk->setFixed(false); + optimizer.addVertex(VGk); + VertexAccBias *VAk = new VertexAccBias(pFp); + VAk->setId(7); + VAk->setFixed(false); + optimizer.addVertex(VAk); + + EdgeInertial *ei = new EdgeInertial(pFrame->mpImuPreintegratedFrame); + + ei->setVertex(0, VPk); + ei->setVertex(1, VVk); + ei->setVertex(2, VGk); + ei->setVertex(3, VAk); + ei->setVertex(4, VP); + ei->setVertex(5, VV); + optimizer.addEdge(ei); + + EdgeGyroRW *egr = new EdgeGyroRW(); + egr->setVertex(0, VGk); + egr->setVertex(1, VG); + Eigen::Matrix3d InfoG = pFrame->mpImuPreintegrated->C.block<3, 3>(9, 9).cast().inverse(); + egr->setInformation(InfoG); + optimizer.addEdge(egr); + + EdgeAccRW *ear = new EdgeAccRW(); + ear->setVertex(0, VAk); + ear->setVertex(1, VA); + Eigen::Matrix3d InfoA = pFrame->mpImuPreintegrated->C.block<3, 3>(12, 12).cast().inverse(); + ear->setInformation(InfoA); + optimizer.addEdge(ear); + + if (!pFp->mpcpi) + Verbose::PrintMess("pFp->mpcpi does not exist!!!\nPrevious Frame " + to_string(pFp->mnId), Verbose::VERBOSITY_NORMAL); + + EdgePriorPoseImu *ep = new EdgePriorPoseImu(pFp->mpcpi); + + ep->setVertex(0, VPk); + ep->setVertex(1, VVk); + ep->setVertex(2, VGk); + ep->setVertex(3, VAk); + g2o::RobustKernelHuber *rkp = new g2o::RobustKernelHuber; + ep->setRobustKernel(rkp); + rkp->setDelta(5); + optimizer.addEdge(ep); // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier // At the next optimization, outliers are not included, but at the end they can be classified as inliers again. - const float chi2Mono[4]={5.991,5.991,5.991,5.991}; - const float chi2Stereo[4]={7.815,7.815,7.815, 7.815}; - const int its[4]={10,10,10,10}; + const float chi2Mono[4] = {5.991, 5.991, 5.991, 5.991}; + const float chi2Stereo[4] = {15.6f, 9.8f, 7.815f, 7.815f}; + const int its[4] = {10, 10, 10, 10}; - int nBad=0; - for(size_t it=0; it<4; it++) + int nBad = 0; + int nBadMono = 0; + int nBadStereo = 0; + int nInliersMono = 0; + int nInliersStereo = 0; + int nInliers = 0; + for (size_t it = 0; it < 4; it++) { - Tcw = pFrame->GetPose(); - vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(),Tcw.translation().cast())); - optimizer.initializeOptimization(0); optimizer.optimize(its[it]); - nBad=0; - for(size_t i=0, iend=vpEdgesMono.size(); imvpMapPoints[idx]->mTrackDepth < 10.f; - if(pFrame->mvbOutlier[idx]) + if (pFrame->mvbOutlier[idx]) { e->computeError(); } const float chi2 = e->chi2(); - if(chi2>chi2Mono[it]) - { - pFrame->mvbOutlier[idx]=true; + if ((chi2 > chi2Mono[it] && !bClose) || (bClose && chi2 > chi2close) || !e->isDepthPositive()) + { + pFrame->mvbOutlier[idx] = true; e->setLevel(1); - nBad++; + nBadMono++; } else { - pFrame->mvbOutlier[idx]=false; + pFrame->mvbOutlier[idx] = false; e->setLevel(0); + nInliersMono++; } - if(it==2) + if (it == 2) e->setRobustKernel(0); } - for(size_t i=0, iend=vpEdgesMono_FHR.size(); imvbOutlier[idx]) - { - e->computeError(); - } - - const float chi2 = e->chi2(); - - if(chi2>chi2Mono[it]) - { - pFrame->mvbOutlier[idx]=true; - e->setLevel(1); - nBad++; - } - else - { - pFrame->mvbOutlier[idx]=false; - e->setLevel(0); - } - - if(it==2) - e->setRobustKernel(0); - } - - for(size_t i=0, iend=vpEdgesStereo.size(); imvbOutlier[idx]) + if (pFrame->mvbOutlier[idx]) { e->computeError(); } const float chi2 = e->chi2(); - if(chi2>chi2Stereo[it]) + if (chi2 > chi2Stereo[it]) { - pFrame->mvbOutlier[idx]=true; + pFrame->mvbOutlier[idx] = true; e->setLevel(1); - nBad++; + nBadStereo++; } else - { + { + pFrame->mvbOutlier[idx] = false; e->setLevel(0); - pFrame->mvbOutlier[idx]=false; + nInliersStereo++; } - if(it==2) + if (it == 2) e->setRobustKernel(0); } - if(optimizer.edges().size()<10) + nInliers = nInliersMono + nInliersStereo; + nBad = nBadMono + nBadStereo; + + if (optimizer.edges().size() < 10) + { break; - } + } + } - // Recover optimized pose and return number of inliers - g2o::VertexSE3Expmap* vSE3_recov = static_cast(optimizer.vertex(0)); - g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate(); - Sophus::SE3 pose(SE3quat_recov.rotation().cast(), - SE3quat_recov.translation().cast()); - pFrame->SetPose(pose); + if ((nInliers < 30) && !bRecInit) + { + nBad = 0; + const float chi2MonoOut = 18.f; + const float chi2StereoOut = 24.f; + EdgeMonoOnlyPose *e1; + EdgeStereoOnlyPose *e2; + for (size_t i = 0, iend = vnIndexEdgeMono.size(); i < iend; i++) + { + const size_t idx = vnIndexEdgeMono[i]; + e1 = vpEdgesMono[i]; + e1->computeError(); + if (e1->chi2() < chi2MonoOut) + pFrame->mvbOutlier[idx] = false; + else + nBad++; + } + for (size_t i = 0, iend = vnIndexEdgeStereo.size(); i < iend; i++) + { + const size_t idx = vnIndexEdgeStereo[i]; + e2 = vpEdgesStereo[i]; + e2->computeError(); + if (e2->chi2() < chi2StereoOut) + pFrame->mvbOutlier[idx] = false; + else + nBad++; + } + } - return nInitialCorrespondences-nBad; + nInliers = nInliersMono + nInliersStereo; + + // Recover optimized pose, velocity and biases + pFrame->SetImuPoseVelocity(VP->estimate().Rwb.cast(), VP->estimate().twb.cast(), VV->estimate().cast()); + Vector6d b; + b << VG->estimate(), VA->estimate(); + pFrame->mImuBias = IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2]); + + // Recover Hessian, marginalize previous frame states and generate new prior for frame + Eigen::Matrix H; + H.setZero(); + + H.block<24, 24>(0, 0) += ei->GetHessian(); + + Eigen::Matrix Hgr = egr->GetHessian(); + H.block<3, 3>(9, 9) += Hgr.block<3, 3>(0, 0); + H.block<3, 3>(9, 24) += Hgr.block<3, 3>(0, 3); + H.block<3, 3>(24, 9) += Hgr.block<3, 3>(3, 0); + H.block<3, 3>(24, 24) += Hgr.block<3, 3>(3, 3); + + Eigen::Matrix Har = ear->GetHessian(); + H.block<3, 3>(12, 12) += Har.block<3, 3>(0, 0); + H.block<3, 3>(12, 27) += Har.block<3, 3>(0, 3); + H.block<3, 3>(27, 12) += Har.block<3, 3>(3, 0); + H.block<3, 3>(27, 27) += Har.block<3, 3>(3, 3); + + H.block<15, 15>(0, 0) += ep->GetHessian(); + + int tot_in = 0, tot_out = 0; + for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) + { + EdgeMonoOnlyPose *e = vpEdgesMono[i]; + + const size_t idx = vnIndexEdgeMono[i]; + + if (!pFrame->mvbOutlier[idx]) + { + H.block<6, 6>(15, 15) += e->GetHessian(); + tot_in++; + } + else + tot_out++; + } + + for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++) + { + EdgeStereoOnlyPose *e = vpEdgesStereo[i]; + + const size_t idx = vnIndexEdgeStereo[i]; + + if (!pFrame->mvbOutlier[idx]) + { + H.block<6, 6>(15, 15) += e->GetHessian(); + tot_in++; + } + else + tot_out++; + } + + H = Marginalize(H, 0, 14); + + pFrame->mpcpi = new ConstraintPoseImu(VP->estimate().Rwb, VP->estimate().twb, VV->estimate(), VG->estimate(), VA->estimate(), H.block<15, 15>(15, 15)); + delete pFp->mpcpi; + pFp->mpcpi = NULL; + + return nInitialCorrespondences - nBad; } -void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap, int& num_fixedKF, int& num_OptKF, int& num_MPs, int& num_edges) +Eigen::MatrixXd Optimizer::Marginalize(const Eigen::MatrixXd &H, const int &start, const int &end) +{ + // Goal + // a | ab | ac a* | 0 | ac* + // ba | b | bc --> 0 | 0 | 0 + // ca | cb | c ca* | 0 | c* + + // Size of block before block to marginalize + const int a = start; + // Size of block to marginalize + const int b = end - start + 1; + // Size of block after block to marginalize + const int c = H.cols() - (end + 1); + + // Reorder as follows: + // a | ab | ac a | ac | ab + // ba | b | bc --> ca | c | cb + // ca | cb | c ba | bc | b + + Eigen::MatrixXd Hn = Eigen::MatrixXd::Zero(H.rows(), H.cols()); + if (a > 0) + { + Hn.block(0, 0, a, a) = H.block(0, 0, a, a); + Hn.block(0, a + c, a, b) = H.block(0, a, a, b); + Hn.block(a + c, 0, b, a) = H.block(a, 0, b, a); + } + if (a > 0 && c > 0) + { + Hn.block(0, a, a, c) = H.block(0, a + b, a, c); + Hn.block(a, 0, c, a) = H.block(a + b, 0, c, a); + } + if (c > 0) + { + Hn.block(a, a, c, c) = H.block(a + b, a + b, c, c); + Hn.block(a, a + c, c, b) = H.block(a + b, a, c, b); + Hn.block(a + c, a, b, c) = H.block(a, a + b, b, c); + } + Hn.block(a + c, a + c, b, b) = H.block(a, a, b, b); + + // Perform marginalization (Schur complement) + Eigen::JacobiSVD svd(Hn.block(a + c, a + c, b, b), Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::JacobiSVD::SingularValuesType singularValues_inv = svd.singularValues(); + for (int i = 0; i < b; ++i) + { + if (singularValues_inv(i) > 1e-6) + singularValues_inv(i) = 1.0 / singularValues_inv(i); + else + singularValues_inv(i) = 0; + } + Eigen::MatrixXd invHb = svd.matrixV() * singularValues_inv.asDiagonal() * svd.matrixU().transpose(); + Hn.block(0, 0, a + c, a + c) = Hn.block(0, 0, a + c, a + c) - Hn.block(0, a + c, a + c, b) * invHb * Hn.block(a + c, 0, b, a + c); + Hn.block(a + c, a + c, b, b) = Eigen::MatrixXd::Zero(b, b); + Hn.block(0, a + c, a + c, b) = Eigen::MatrixXd::Zero(a + c, b); + Hn.block(a + c, 0, b, a + c) = Eigen::MatrixXd::Zero(b, a + c); + + // Inverse reorder + // a* | ac* | 0 a* | 0 | ac* + // ca* | c* | 0 --> 0 | 0 | 0 + // 0 | 0 | 0 ca* | 0 | c* + Eigen::MatrixXd res = Eigen::MatrixXd::Zero(H.rows(), H.cols()); + if (a > 0) + { + res.block(0, 0, a, a) = Hn.block(0, 0, a, a); + res.block(0, a, a, b) = Hn.block(0, a + c, a, b); + res.block(a, 0, b, a) = Hn.block(a + c, 0, b, a); + } + if (a > 0 && c > 0) + { + res.block(0, a + b, a, c) = Hn.block(0, a, a, c); + res.block(a + b, 0, c, a) = Hn.block(a, 0, c, a); + } + if (c > 0) + { + res.block(a + b, a + b, c, c) = Hn.block(a, a, c, c); + res.block(a + b, a, c, b) = Hn.block(a, a + c, c, b); + res.block(a, a + b, b, c) = Hn.block(a + c, a, b, c); + } + + res.block(a, a, b, b) = Hn.block(a + c, a + c, b, b); + + return res; +} + +/**************************************以下为局部地图优化**************************************************************/ + +void Optimizer::LocalBundleAdjustment( + KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int &num_fixedKF, int &num_OptKF, int &num_MPs, int &num_edges) { // Local KeyFrames: First Breath Search from Current Keyframe - list lLocalKeyFrames; + list lLocalKeyFrames; lLocalKeyFrames.push_back(pKF); pKF->mnBALocalForKF = pKF->mnId; - Map* pCurrentMap = pKF->GetMap(); + Map *pCurrentMap = pKF->GetMap(); - const vector vNeighKFs = pKF->GetVectorCovisibleKeyFrames(); - for(int i=0, iend=vNeighKFs.size(); i vNeighKFs = pKF->GetVectorCovisibleKeyFrames(); + for (int i = 0, iend = vNeighKFs.size(); i < iend; i++) { - KeyFrame* pKFi = vNeighKFs[i]; + KeyFrame *pKFi = vNeighKFs[i]; pKFi->mnBALocalForKF = pKF->mnId; - if(!pKFi->isBad() && pKFi->GetMap() == pCurrentMap) + if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap) lLocalKeyFrames.push_back(pKFi); } // Local MapPoints seen in Local KeyFrames num_fixedKF = 0; - list lLocalMapPoints; - set sNumObsMP; - for(list::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++) + list lLocalMapPoints; + set sNumObsMP; + for (list::iterator lit = lLocalKeyFrames.begin(), lend = lLocalKeyFrames.end(); lit != lend; lit++) { - KeyFrame* pKFi = *lit; - if(pKFi->mnId==pMap->GetInitKFid()) + KeyFrame *pKFi = *lit; + if (pKFi->mnId == pMap->GetInitKFid()) { num_fixedKF = 1; } - vector vpMPs = pKFi->GetMapPointMatches(); - for(vector::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++) + vector vpMPs = pKFi->GetMapPointMatches(); + for (vector::iterator vit = vpMPs.begin(), vend = vpMPs.end(); vit != vend; vit++) { - MapPoint* pMP = *vit; - if(pMP) - if(!pMP->isBad() && pMP->GetMap() == pCurrentMap) + MapPoint *pMP = *vit; + if (pMP) + if (!pMP->isBad() && pMP->GetMap() == pCurrentMap) { - if(pMP->mnBALocalForKF!=pKF->mnId) + if (pMP->mnBALocalForKF != pKF->mnId) { lLocalMapPoints.push_back(pMP); - pMP->mnBALocalForKF=pKF->mnId; + pMP->mnBALocalForKF = pKF->mnId; } } } } // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes - list lFixedCameras; - for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) + list lFixedCameras; + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) { - map> observations = (*lit)->GetObservations(); - for(map>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + map> observations = (*lit)->GetObservations(); + for (map>::iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) { - KeyFrame* pKFi = mit->first; + KeyFrame *pKFi = mit->first; - if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId ) - { - pKFi->mnBAFixedForKF=pKF->mnId; - if(!pKFi->isBad() && pKFi->GetMap() == pCurrentMap) + if (pKFi->mnBALocalForKF != pKF->mnId && pKFi->mnBAFixedForKF != pKF->mnId) + { + pKFi->mnBAFixedForKF = pKF->mnId; + if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap) lFixedCameras.push_back(pKFi); } } } num_fixedKF = lFixedCameras.size() + num_fixedKF; - - if(num_fixedKF == 0) + if (num_fixedKF == 0) { Verbose::PrintMess("LM-LBA: There are 0 fixed KF in the optimizations, LBA aborted", Verbose::VERBOSITY_NORMAL); return; @@ -1187,20 +1313,20 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap // Setup optimizer g2o::SparseOptimizer optimizer; - g2o::BlockSolver_6_3::LinearSolverType * linearSolver; + g2o::BlockSolver_6_3::LinearSolverType *linearSolver; linearSolver = new g2o::LinearSolverEigen(); - g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver); + g2o::BlockSolver_6_3 *solver_ptr = new g2o::BlockSolver_6_3(linearSolver); - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); if (pMap->IsInertial()) solver->setUserLambdaInit(100.0); optimizer.setAlgorithm(solver); optimizer.setVerbose(false); - if(pbStopFlag) + if (pbStopFlag) optimizer.setForceStopFlag(pbStopFlag); unsigned long maxKFid = 0; @@ -1210,66 +1336,66 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap pCurrentMap->msFixedKFs.clear(); // Set Local KeyFrame vertices - for(list::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++) + for (list::iterator lit = lLocalKeyFrames.begin(), lend = lLocalKeyFrames.end(); lit != lend; lit++) { - KeyFrame* pKFi = *lit; - g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap(); + KeyFrame *pKFi = *lit; + g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap(); Sophus::SE3 Tcw = pKFi->GetPose(); vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(), Tcw.translation().cast())); vSE3->setId(pKFi->mnId); - vSE3->setFixed(pKFi->mnId==pMap->GetInitKFid()); + vSE3->setFixed(pKFi->mnId == pMap->GetInitKFid()); optimizer.addVertex(vSE3); - if(pKFi->mnId>maxKFid) - maxKFid=pKFi->mnId; + if (pKFi->mnId > maxKFid) + maxKFid = pKFi->mnId; // DEBUG LBA pCurrentMap->msOptKFs.insert(pKFi->mnId); } num_OptKF = lLocalKeyFrames.size(); // Set Fixed KeyFrame vertices - for(list::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++) + for (list::iterator lit = lFixedCameras.begin(), lend = lFixedCameras.end(); lit != lend; lit++) { - KeyFrame* pKFi = *lit; - g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap(); + KeyFrame *pKFi = *lit; + g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap(); Sophus::SE3 Tcw = pKFi->GetPose(); - vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(),Tcw.translation().cast())); + vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(), Tcw.translation().cast())); vSE3->setId(pKFi->mnId); vSE3->setFixed(true); optimizer.addVertex(vSE3); - if(pKFi->mnId>maxKFid) - maxKFid=pKFi->mnId; + if (pKFi->mnId > maxKFid) + maxKFid = pKFi->mnId; // DEBUG LBA pCurrentMap->msFixedKFs.insert(pKFi->mnId); } // Set MapPoint vertices - const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size(); + const int nExpectedSize = (lLocalKeyFrames.size() + lFixedCameras.size()) * lLocalMapPoints.size(); - vector vpEdgesMono; + vector vpEdgesMono; vpEdgesMono.reserve(nExpectedSize); - vector vpEdgesBody; + vector vpEdgesBody; vpEdgesBody.reserve(nExpectedSize); - vector vpEdgeKFMono; + vector vpEdgeKFMono; vpEdgeKFMono.reserve(nExpectedSize); - vector vpEdgeKFBody; + vector vpEdgeKFBody; vpEdgeKFBody.reserve(nExpectedSize); - vector vpMapPointEdgeMono; + vector vpMapPointEdgeMono; vpMapPointEdgeMono.reserve(nExpectedSize); - vector vpMapPointEdgeBody; + vector vpMapPointEdgeBody; vpMapPointEdgeBody.reserve(nExpectedSize); - vector vpEdgesStereo; + vector vpEdgesStereo; vpEdgesStereo.reserve(nExpectedSize); - vector vpEdgeKFStereo; + vector vpEdgeKFStereo; vpEdgeKFStereo.reserve(nExpectedSize); - vector vpMapPointEdgeStereo; + vector vpMapPointEdgeStereo; vpMapPointEdgeStereo.reserve(nExpectedSize); const float thHuberMono = sqrt(5.991); @@ -1279,44 +1405,44 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap int nEdges = 0; - for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) { - MapPoint* pMP = *lit; - g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ(); + MapPoint *pMP = *lit; + g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ(); vPoint->setEstimate(pMP->GetWorldPos().cast()); - int id = pMP->mnId+maxKFid+1; + int id = pMP->mnId + maxKFid + 1; vPoint->setId(id); vPoint->setMarginalized(true); optimizer.addVertex(vPoint); nPoints++; - const map> observations = pMP->GetObservations(); + const map> observations = pMP->GetObservations(); - //Set edges - for(map>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + // Set edges + for (map>::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) { - KeyFrame* pKFi = mit->first; + KeyFrame *pKFi = mit->first; - if(!pKFi->isBad() && pKFi->GetMap() == pCurrentMap) + if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap) { const int leftIndex = get<0>(mit->second); // Monocular observation - if(leftIndex != -1 && pKFi->mvuRight[get<0>(mit->second)]<0) + if (leftIndex != -1 && pKFi->mvuRight[get<0>(mit->second)] < 0) { const cv::KeyPoint &kpUn = pKFi->mvKeysUn[leftIndex]; - Eigen::Matrix obs; + Eigen::Matrix obs; obs << kpUn.pt.x, kpUn.pt.y; - ORB_SLAM3::EdgeSE3ProjectXYZ* e = new ORB_SLAM3::EdgeSE3ProjectXYZ(); + ORB_SLAM3::EdgeSE3ProjectXYZ *e = new ORB_SLAM3::EdgeSE3ProjectXYZ(); - e->setVertex(0, dynamic_cast(optimizer.vertex(id))); - e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); e->setMeasurement(obs); const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; - e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); rk->setDelta(thHuberMono); @@ -1329,23 +1455,23 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap nEdges++; } - else if(leftIndex != -1 && pKFi->mvuRight[get<0>(mit->second)]>=0)// Stereo observation + else if (leftIndex != -1 && pKFi->mvuRight[get<0>(mit->second)] >= 0) // Stereo observation { const cv::KeyPoint &kpUn = pKFi->mvKeysUn[leftIndex]; - Eigen::Matrix obs; + Eigen::Matrix obs; const float kp_ur = pKFi->mvuRight[get<0>(mit->second)]; obs << kpUn.pt.x, kpUn.pt.y, kp_ur; - g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ(); + g2o::EdgeStereoSE3ProjectXYZ *e = new g2o::EdgeStereoSE3ProjectXYZ(); - e->setVertex(0, dynamic_cast(optimizer.vertex(id))); - e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); e->setMeasurement(obs); const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; - Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2; + Eigen::Matrix3d Info = Eigen::Matrix3d::Identity() * invSigma2; e->setInformation(Info); - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); rk->setDelta(thHuberStereo); @@ -1363,29 +1489,31 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap nEdges++; } - if(pKFi->mpCamera2){ + if (pKFi->mpCamera2) + { int rightIndex = get<1>(mit->second); - if(rightIndex != -1 ){ + if (rightIndex != -1) + { rightIndex -= pKFi->NLeft; - Eigen::Matrix obs; + Eigen::Matrix obs; cv::KeyPoint kp = pKFi->mvKeysRight[rightIndex]; obs << kp.pt.x, kp.pt.y; ORB_SLAM3::EdgeSE3ProjectXYZToBody *e = new ORB_SLAM3::EdgeSE3ProjectXYZToBody(); - e->setVertex(0, dynamic_cast(optimizer.vertex(id))); - e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); e->setMeasurement(obs); const float &invSigma2 = pKFi->mvInvLevelSigma2[kp.octave]; - e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); rk->setDelta(thHuberMono); - Sophus::SE3f Trl = pKFi-> GetRelativePoseTrl(); + Sophus::SE3f Trl = pKFi->GetRelativePoseTrl(); e->mTrl = g2o::SE3Quat(Trl.unit_quaternion().cast(), Trl.translation().cast()); e->pCamera = pKFi->mpCamera2; @@ -1403,93 +1531,92 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap } num_edges = nEdges; - if(pbStopFlag) - if(*pbStopFlag) + if (pbStopFlag) + if (*pbStopFlag) return; optimizer.initializeOptimization(); optimizer.optimize(10); - vector > vToErase; - vToErase.reserve(vpEdgesMono.size()+vpEdgesBody.size()+vpEdgesStereo.size()); + vector> vToErase; + vToErase.reserve(vpEdgesMono.size() + vpEdgesBody.size() + vpEdgesStereo.size()); - // Check inlier observations - for(size_t i=0, iend=vpEdgesMono.size(); iisBad()) + if (pMP->isBad()) continue; - if(e->chi2()>5.991 || !e->isDepthPositive()) + if (e->chi2() > 5.991 || !e->isDepthPositive()) { - KeyFrame* pKFi = vpEdgeKFMono[i]; - vToErase.push_back(make_pair(pKFi,pMP)); + KeyFrame *pKFi = vpEdgeKFMono[i]; + vToErase.push_back(make_pair(pKFi, pMP)); } } - for(size_t i=0, iend=vpEdgesBody.size(); iisBad()) + if (pMP->isBad()) continue; - if(e->chi2()>5.991 || !e->isDepthPositive()) + if (e->chi2() > 5.991 || !e->isDepthPositive()) { - KeyFrame* pKFi = vpEdgeKFBody[i]; - vToErase.push_back(make_pair(pKFi,pMP)); + KeyFrame *pKFi = vpEdgeKFBody[i]; + vToErase.push_back(make_pair(pKFi, pMP)); } } - for(size_t i=0, iend=vpEdgesStereo.size(); iisBad()) + if (pMP->isBad()) continue; - if(e->chi2()>7.815 || !e->isDepthPositive()) + if (e->chi2() > 7.815 || !e->isDepthPositive()) { - KeyFrame* pKFi = vpEdgeKFStereo[i]; - vToErase.push_back(make_pair(pKFi,pMP)); + KeyFrame *pKFi = vpEdgeKFStereo[i]; + vToErase.push_back(make_pair(pKFi, pMP)); } } - // Get Map Mutex unique_lock lock(pMap->mMutexMapUpdate); - if(!vToErase.empty()) + if (!vToErase.empty()) { - for(size_t i=0;iEraseMapPointMatch(pMPi); pMPi->EraseObservation(pKFi); } } // Recover optimized data - //Keyframes - for(list::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++) + // Keyframes + for (list::iterator lit = lLocalKeyFrames.begin(), lend = lLocalKeyFrames.end(); lit != lend; lit++) { - KeyFrame* pKFi = *lit; - g2o::VertexSE3Expmap* vSE3 = static_cast(optimizer.vertex(pKFi->mnId)); + KeyFrame *pKFi = *lit; + g2o::VertexSE3Expmap *vSE3 = static_cast(optimizer.vertex(pKFi->mnId)); g2o::SE3Quat SE3quat = vSE3->estimate(); Sophus::SE3f Tiw(SE3quat.rotation().cast(), SE3quat.translation().cast()); pKFi->SetPose(Tiw); } - //Points - for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) + // Points + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) { - MapPoint* pMP = *lit; - g2o::VertexSBAPointXYZ* vPoint = static_cast(optimizer.vertex(pMP->mnId+maxKFid+1)); + MapPoint *pMP = *lit; + g2o::VertexSBAPointXYZ *vPoint = static_cast(optimizer.vertex(pMP->mnId + maxKFid + 1)); pMP->SetWorldPos(vPoint->estimate().cast()); pMP->UpdateNormalAndDepth(); } @@ -1497,913 +1624,32 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap pMap->IncreaseChangeIndex(); } - -void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, - const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, - const LoopClosing::KeyFrameAndPose &CorrectedSim3, - const map > &LoopConnections, const bool &bFixScale) -{ - // Setup optimizer - g2o::SparseOptimizer optimizer; - optimizer.setVerbose(false); - g2o::BlockSolver_7_3::LinearSolverType * linearSolver = - new g2o::LinearSolverEigen(); - g2o::BlockSolver_7_3 * solver_ptr= new g2o::BlockSolver_7_3(linearSolver); - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); - - solver->setUserLambdaInit(1e-16); - optimizer.setAlgorithm(solver); - - const vector vpKFs = pMap->GetAllKeyFrames(); - const vector vpMPs = pMap->GetAllMapPoints(); - - const unsigned int nMaxKFid = pMap->GetMaxKFid(); - - vector > vScw(nMaxKFid+1); - vector > vCorrectedSwc(nMaxKFid+1); - vector vpVertices(nMaxKFid+1); - - vector vZvectors(nMaxKFid+1); // For debugging - Eigen::Vector3d z_vec; - z_vec << 0.0, 0.0, 1.0; - - const int minFeat = 100; - - // Set KeyFrame vertices - for(size_t i=0, iend=vpKFs.size(); iisBad()) - continue; - g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap(); - - const int nIDi = pKF->mnId; - - LoopClosing::KeyFrameAndPose::const_iterator it = CorrectedSim3.find(pKF); - - if(it!=CorrectedSim3.end()) - { - vScw[nIDi] = it->second; - VSim3->setEstimate(it->second); - } - else - { - Sophus::SE3d Tcw = pKF->GetPose().cast(); - g2o::Sim3 Siw(Tcw.unit_quaternion(),Tcw.translation(),1.0); - vScw[nIDi] = Siw; - VSim3->setEstimate(Siw); - } - - if(pKF->mnId==pMap->GetInitKFid()) - VSim3->setFixed(true); - - VSim3->setId(nIDi); - VSim3->setMarginalized(false); - VSim3->_fix_scale = bFixScale; - - optimizer.addVertex(VSim3); - vZvectors[nIDi]=vScw[nIDi].rotation()*z_vec; // For debugging - - vpVertices[nIDi]=VSim3; - } - - - set > sInsertedEdges; - - const Eigen::Matrix matLambda = Eigen::Matrix::Identity(); - - // Set Loop edges - int count_loop = 0; - for(map >::const_iterator mit = LoopConnections.begin(), mend=LoopConnections.end(); mit!=mend; mit++) - { - KeyFrame* pKF = mit->first; - const long unsigned int nIDi = pKF->mnId; - const set &spConnections = mit->second; - const g2o::Sim3 Siw = vScw[nIDi]; - const g2o::Sim3 Swi = Siw.inverse(); - - for(set::const_iterator sit=spConnections.begin(), send=spConnections.end(); sit!=send; sit++) - { - const long unsigned int nIDj = (*sit)->mnId; - if((nIDi!=pCurKF->mnId || nIDj!=pLoopKF->mnId) && pKF->GetWeight(*sit)setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); - e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); - e->setMeasurement(Sji); - - e->information() = matLambda; - - optimizer.addEdge(e); - count_loop++; - sInsertedEdges.insert(make_pair(min(nIDi,nIDj),max(nIDi,nIDj))); - } - } - - // Set normal edges - for(size_t i=0, iend=vpKFs.size(); imnId; - - g2o::Sim3 Swi; - - LoopClosing::KeyFrameAndPose::const_iterator iti = NonCorrectedSim3.find(pKF); - - if(iti!=NonCorrectedSim3.end()) - Swi = (iti->second).inverse(); - else - Swi = vScw[nIDi].inverse(); - - KeyFrame* pParentKF = pKF->GetParent(); - - // Spanning tree edge - if(pParentKF) - { - int nIDj = pParentKF->mnId; - - g2o::Sim3 Sjw; - - LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF); - - if(itj!=NonCorrectedSim3.end()) - Sjw = itj->second; - else - Sjw = vScw[nIDj]; - - g2o::Sim3 Sji = Sjw * Swi; - - g2o::EdgeSim3* e = new g2o::EdgeSim3(); - e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); - e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); - e->setMeasurement(Sji); - e->information() = matLambda; - optimizer.addEdge(e); - } - - // Loop edges - const set sLoopEdges = pKF->GetLoopEdges(); - for(set::const_iterator sit=sLoopEdges.begin(), send=sLoopEdges.end(); sit!=send; sit++) - { - KeyFrame* pLKF = *sit; - if(pLKF->mnIdmnId) - { - g2o::Sim3 Slw; - - LoopClosing::KeyFrameAndPose::const_iterator itl = NonCorrectedSim3.find(pLKF); - - if(itl!=NonCorrectedSim3.end()) - Slw = itl->second; - else - Slw = vScw[pLKF->mnId]; - - g2o::Sim3 Sli = Slw * Swi; - g2o::EdgeSim3* el = new g2o::EdgeSim3(); - el->setVertex(1, dynamic_cast(optimizer.vertex(pLKF->mnId))); - el->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); - el->setMeasurement(Sli); - el->information() = matLambda; - optimizer.addEdge(el); - } - } - - // Covisibility graph edges - const vector vpConnectedKFs = pKF->GetCovisiblesByWeight(minFeat); - for(vector::const_iterator vit=vpConnectedKFs.begin(); vit!=vpConnectedKFs.end(); vit++) - { - KeyFrame* pKFn = *vit; - if(pKFn && pKFn!=pParentKF && !pKF->hasChild(pKFn) /*&& !sLoopEdges.count(pKFn)*/) - { - if(!pKFn->isBad() && pKFn->mnIdmnId) - { - if(sInsertedEdges.count(make_pair(min(pKF->mnId,pKFn->mnId),max(pKF->mnId,pKFn->mnId)))) - continue; - - g2o::Sim3 Snw; - - LoopClosing::KeyFrameAndPose::const_iterator itn = NonCorrectedSim3.find(pKFn); - - if(itn!=NonCorrectedSim3.end()) - Snw = itn->second; - else - Snw = vScw[pKFn->mnId]; - - g2o::Sim3 Sni = Snw * Swi; - - g2o::EdgeSim3* en = new g2o::EdgeSim3(); - en->setVertex(1, dynamic_cast(optimizer.vertex(pKFn->mnId))); - en->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); - en->setMeasurement(Sni); - en->information() = matLambda; - optimizer.addEdge(en); - } - } - } - - // Inertial edges if inertial - if(pKF->bImu && pKF->mPrevKF) - { - g2o::Sim3 Spw; - LoopClosing::KeyFrameAndPose::const_iterator itp = NonCorrectedSim3.find(pKF->mPrevKF); - if(itp!=NonCorrectedSim3.end()) - Spw = itp->second; - else - Spw = vScw[pKF->mPrevKF->mnId]; - - g2o::Sim3 Spi = Spw * Swi; - g2o::EdgeSim3* ep = new g2o::EdgeSim3(); - ep->setVertex(1, dynamic_cast(optimizer.vertex(pKF->mPrevKF->mnId))); - ep->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); - ep->setMeasurement(Spi); - ep->information() = matLambda; - optimizer.addEdge(ep); - } - } - - - optimizer.initializeOptimization(); - optimizer.computeActiveErrors(); - optimizer.optimize(20); - optimizer.computeActiveErrors(); - unique_lock lock(pMap->mMutexMapUpdate); - - // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1] - for(size_t i=0;imnId; - - g2o::VertexSim3Expmap* VSim3 = static_cast(optimizer.vertex(nIDi)); - g2o::Sim3 CorrectedSiw = VSim3->estimate(); - vCorrectedSwc[nIDi]=CorrectedSiw.inverse(); - double s = CorrectedSiw.scale(); - - Sophus::SE3f Tiw(CorrectedSiw.rotation().cast(), CorrectedSiw.translation().cast() / s); - pKFi->SetPose(Tiw); - } - - // Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose - for(size_t i=0, iend=vpMPs.size(); iisBad()) - continue; - - int nIDr; - if(pMP->mnCorrectedByKF==pCurKF->mnId) - { - nIDr = pMP->mnCorrectedReference; - } - else - { - KeyFrame* pRefKF = pMP->GetReferenceKeyFrame(); - nIDr = pRefKF->mnId; - } - - - g2o::Sim3 Srw = vScw[nIDr]; - g2o::Sim3 correctedSwr = vCorrectedSwc[nIDr]; - - Eigen::Matrix eigP3Dw = pMP->GetWorldPos().cast(); - Eigen::Matrix eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw)); - pMP->SetWorldPos(eigCorrectedP3Dw.cast()); - - pMP->UpdateNormalAndDepth(); - } - - // TODO Check this changeindex - pMap->IncreaseChangeIndex(); -} - -void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF, vector &vpFixedKFs, vector &vpFixedCorrectedKFs, - vector &vpNonFixedKFs, vector &vpNonCorrectedMPs) +void Optimizer::LocalInertialBA( + KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int &num_fixedKF, int &num_OptKF, + int &num_MPs, int &num_edges, bool bLarge, bool bRecInit) { - Verbose::PrintMess("Opt_Essential: There are " + to_string(vpFixedKFs.size()) + " KFs fixed in the merged map", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("Opt_Essential: There are " + to_string(vpFixedCorrectedKFs.size()) + " KFs fixed in the old map", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("Opt_Essential: There are " + to_string(vpNonFixedKFs.size()) + " KFs non-fixed in the merged map", Verbose::VERBOSITY_DEBUG); - Verbose::PrintMess("Opt_Essential: There are " + to_string(vpNonCorrectedMPs.size()) + " MPs non-corrected in the merged map", Verbose::VERBOSITY_DEBUG); + Map *pCurrentMap = pKF->GetMap(); - g2o::SparseOptimizer optimizer; - optimizer.setVerbose(false); - g2o::BlockSolver_7_3::LinearSolverType * linearSolver = - new g2o::LinearSolverEigen(); - g2o::BlockSolver_7_3 * solver_ptr= new g2o::BlockSolver_7_3(linearSolver); - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); - - solver->setUserLambdaInit(1e-16); - optimizer.setAlgorithm(solver); - - Map* pMap = pCurKF->GetMap(); - const unsigned int nMaxKFid = pMap->GetMaxKFid(); - - vector > vScw(nMaxKFid+1); - vector > vCorrectedSwc(nMaxKFid+1); - vector vpVertices(nMaxKFid+1); - - vector vpGoodPose(nMaxKFid+1); - vector vpBadPose(nMaxKFid+1); - - const int minFeat = 100; - - for(KeyFrame* pKFi : vpFixedKFs) + int maxOpt = 10; + int opt_it = 10; + if (bLarge) { - if(pKFi->isBad()) - continue; - - g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap(); - - const int nIDi = pKFi->mnId; - - Sophus::SE3d Tcw = pKFi->GetPose().cast(); - g2o::Sim3 Siw(Tcw.unit_quaternion(),Tcw.translation(),1.0); - - vCorrectedSwc[nIDi]=Siw.inverse(); - VSim3->setEstimate(Siw); - - VSim3->setFixed(true); - - VSim3->setId(nIDi); - VSim3->setMarginalized(false); - VSim3->_fix_scale = true; - - optimizer.addVertex(VSim3); - - vpVertices[nIDi]=VSim3; - - vpGoodPose[nIDi] = true; - vpBadPose[nIDi] = false; + maxOpt = 25; + opt_it = 4; } - Verbose::PrintMess("Opt_Essential: vpFixedKFs loaded", Verbose::VERBOSITY_DEBUG); - - set sIdKF; - for(KeyFrame* pKFi : vpFixedCorrectedKFs) - { - if(pKFi->isBad()) - continue; - - g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap(); - - const int nIDi = pKFi->mnId; - - Sophus::SE3d Tcw = pKFi->GetPose().cast(); - g2o::Sim3 Siw(Tcw.unit_quaternion(),Tcw.translation(),1.0); - - vCorrectedSwc[nIDi]=Siw.inverse(); - VSim3->setEstimate(Siw); - - Sophus::SE3d Tcw_bef = pKFi->mTcwBefMerge.cast(); - vScw[nIDi] = g2o::Sim3(Tcw_bef.unit_quaternion(),Tcw_bef.translation(),1.0); - - VSim3->setFixed(true); - - VSim3->setId(nIDi); - VSim3->setMarginalized(false); - - optimizer.addVertex(VSim3); - - vpVertices[nIDi]=VSim3; - - sIdKF.insert(nIDi); - - vpGoodPose[nIDi] = true; - vpBadPose[nIDi] = true; - } - - for(KeyFrame* pKFi : vpNonFixedKFs) - { - if(pKFi->isBad()) - continue; - - const int nIDi = pKFi->mnId; - - if(sIdKF.count(nIDi)) // It has already added in the corrected merge KFs - continue; - - g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap(); - - Sophus::SE3d Tcw = pKFi->GetPose().cast(); - g2o::Sim3 Siw(Tcw.unit_quaternion(),Tcw.translation(),1.0); - - vScw[nIDi] = Siw; - VSim3->setEstimate(Siw); - - VSim3->setFixed(false); - - VSim3->setId(nIDi); - VSim3->setMarginalized(false); - - optimizer.addVertex(VSim3); - - vpVertices[nIDi]=VSim3; - - sIdKF.insert(nIDi); - - vpGoodPose[nIDi] = false; - vpBadPose[nIDi] = true; - } - - vector vpKFs; - vpKFs.reserve(vpFixedKFs.size() + vpFixedCorrectedKFs.size() + vpNonFixedKFs.size()); - vpKFs.insert(vpKFs.end(),vpFixedKFs.begin(),vpFixedKFs.end()); - vpKFs.insert(vpKFs.end(),vpFixedCorrectedKFs.begin(),vpFixedCorrectedKFs.end()); - vpKFs.insert(vpKFs.end(),vpNonFixedKFs.begin(),vpNonFixedKFs.end()); - set spKFs(vpKFs.begin(), vpKFs.end()); - - const Eigen::Matrix matLambda = Eigen::Matrix::Identity(); - - for(KeyFrame* pKFi : vpKFs) - { - int num_connections = 0; - const int nIDi = pKFi->mnId; - - g2o::Sim3 correctedSwi; - g2o::Sim3 Swi; - - if(vpGoodPose[nIDi]) - correctedSwi = vCorrectedSwc[nIDi]; - if(vpBadPose[nIDi]) - Swi = vScw[nIDi].inverse(); - - KeyFrame* pParentKFi = pKFi->GetParent(); - - // Spanning tree edge - if(pParentKFi && spKFs.find(pParentKFi) != spKFs.end()) - { - int nIDj = pParentKFi->mnId; - - g2o::Sim3 Sjw; - bool bHasRelation = false; - - if(vpGoodPose[nIDi] && vpGoodPose[nIDj]) - { - Sjw = vCorrectedSwc[nIDj].inverse(); - bHasRelation = true; - } - else if(vpBadPose[nIDi] && vpBadPose[nIDj]) - { - Sjw = vScw[nIDj]; - bHasRelation = true; - } - - if(bHasRelation) - { - g2o::Sim3 Sji = Sjw * Swi; - - g2o::EdgeSim3* e = new g2o::EdgeSim3(); - e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); - e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); - e->setMeasurement(Sji); - - e->information() = matLambda; - optimizer.addEdge(e); - num_connections++; - } - - } - - // Loop edges - const set sLoopEdges = pKFi->GetLoopEdges(); - for(set::const_iterator sit=sLoopEdges.begin(), send=sLoopEdges.end(); sit!=send; sit++) - { - KeyFrame* pLKF = *sit; - if(spKFs.find(pLKF) != spKFs.end() && pLKF->mnIdmnId) - { - g2o::Sim3 Slw; - bool bHasRelation = false; - - if(vpGoodPose[nIDi] && vpGoodPose[pLKF->mnId]) - { - Slw = vCorrectedSwc[pLKF->mnId].inverse(); - bHasRelation = true; - } - else if(vpBadPose[nIDi] && vpBadPose[pLKF->mnId]) - { - Slw = vScw[pLKF->mnId]; - bHasRelation = true; - } - - - if(bHasRelation) - { - g2o::Sim3 Sli = Slw * Swi; - g2o::EdgeSim3* el = new g2o::EdgeSim3(); - el->setVertex(1, dynamic_cast(optimizer.vertex(pLKF->mnId))); - el->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); - el->setMeasurement(Sli); - el->information() = matLambda; - optimizer.addEdge(el); - num_connections++; - } - } - } - - // Covisibility graph edges - const vector vpConnectedKFs = pKFi->GetCovisiblesByWeight(minFeat); - for(vector::const_iterator vit=vpConnectedKFs.begin(); vit!=vpConnectedKFs.end(); vit++) - { - KeyFrame* pKFn = *vit; - if(pKFn && pKFn!=pParentKFi && !pKFi->hasChild(pKFn) && !sLoopEdges.count(pKFn) && spKFs.find(pKFn) != spKFs.end()) - { - if(!pKFn->isBad() && pKFn->mnIdmnId) - { - - g2o::Sim3 Snw = vScw[pKFn->mnId]; - bool bHasRelation = false; - - if(vpGoodPose[nIDi] && vpGoodPose[pKFn->mnId]) - { - Snw = vCorrectedSwc[pKFn->mnId].inverse(); - bHasRelation = true; - } - else if(vpBadPose[nIDi] && vpBadPose[pKFn->mnId]) - { - Snw = vScw[pKFn->mnId]; - bHasRelation = true; - } - - if(bHasRelation) - { - g2o::Sim3 Sni = Snw * Swi; - - g2o::EdgeSim3* en = new g2o::EdgeSim3(); - en->setVertex(1, dynamic_cast(optimizer.vertex(pKFn->mnId))); - en->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); - en->setMeasurement(Sni); - en->information() = matLambda; - optimizer.addEdge(en); - num_connections++; - } - } - } - } - - if(num_connections == 0 ) - { - Verbose::PrintMess("Opt_Essential: KF " + to_string(pKFi->mnId) + " has 0 connections", Verbose::VERBOSITY_DEBUG); - } - } - - // Optimize! - optimizer.initializeOptimization(); - optimizer.optimize(20); - - unique_lock lock(pMap->mMutexMapUpdate); - - // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1] - for(KeyFrame* pKFi : vpNonFixedKFs) - { - if(pKFi->isBad()) - continue; - - const int nIDi = pKFi->mnId; - - g2o::VertexSim3Expmap* VSim3 = static_cast(optimizer.vertex(nIDi)); - g2o::Sim3 CorrectedSiw = VSim3->estimate(); - vCorrectedSwc[nIDi]=CorrectedSiw.inverse(); - double s = CorrectedSiw.scale(); - Sophus::SE3d Tiw(CorrectedSiw.rotation(),CorrectedSiw.translation() / s); - - pKFi->mTcwBefMerge = pKFi->GetPose(); - pKFi->mTwcBefMerge = pKFi->GetPoseInverse(); - pKFi->SetPose(Tiw.cast()); - } - - // Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose - for(MapPoint* pMPi : vpNonCorrectedMPs) - { - if(pMPi->isBad()) - continue; - - KeyFrame* pRefKF = pMPi->GetReferenceKeyFrame(); - while(pRefKF->isBad()) - { - if(!pRefKF) - { - Verbose::PrintMess("MP " + to_string(pMPi->mnId) + " without a valid reference KF", Verbose::VERBOSITY_DEBUG); - break; - } - - pMPi->EraseObservation(pRefKF); - pRefKF = pMPi->GetReferenceKeyFrame(); - } - - if(vpBadPose[pRefKF->mnId]) - { - Sophus::SE3f TNonCorrectedwr = pRefKF->mTwcBefMerge; - Sophus::SE3f Twr = pRefKF->GetPoseInverse(); - - Eigen::Vector3f eigCorrectedP3Dw = Twr * TNonCorrectedwr.inverse() * pMPi->GetWorldPos(); - pMPi->SetWorldPos(eigCorrectedP3Dw); - - pMPi->UpdateNormalAndDepth(); - } - else - { - cout << "ERROR: MapPoint has a reference KF from another map" << endl; - } - - } -} - -int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector &vpMatches1, g2o::Sim3 &g2oS12, const float th2, - const bool bFixScale, Eigen::Matrix &mAcumHessian, const bool bAllPoints) -{ - g2o::SparseOptimizer optimizer; - g2o::BlockSolverX::LinearSolverType * linearSolver; - - linearSolver = new g2o::LinearSolverDense(); - - g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); - - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); - optimizer.setAlgorithm(solver); - - // Camera poses - const Eigen::Matrix3f R1w = pKF1->GetRotation(); - const Eigen::Vector3f t1w = pKF1->GetTranslation(); - const Eigen::Matrix3f R2w = pKF2->GetRotation(); - const Eigen::Vector3f t2w = pKF2->GetTranslation(); - - // Set Sim3 vertex - ORB_SLAM3::VertexSim3Expmap * vSim3 = new ORB_SLAM3::VertexSim3Expmap(); - vSim3->_fix_scale=bFixScale; - vSim3->setEstimate(g2oS12); - vSim3->setId(0); - vSim3->setFixed(false); - vSim3->pCamera1 = pKF1->mpCamera; - vSim3->pCamera2 = pKF2->mpCamera; - optimizer.addVertex(vSim3); - - // Set MapPoint vertices - const int N = vpMatches1.size(); - const vector vpMapPoints1 = pKF1->GetMapPointMatches(); - vector vpEdges12; - vector vpEdges21; - vector vnIndexEdge; - vector vbIsInKF2; - - vnIndexEdge.reserve(2*N); - vpEdges12.reserve(2*N); - vpEdges21.reserve(2*N); - vbIsInKF2.reserve(2*N); - - const float deltaHuber = sqrt(th2); - - int nCorrespondences = 0; - int nBadMPs = 0; - int nInKF2 = 0; - int nOutKF2 = 0; - int nMatchWithoutMP = 0; - - vector vIdsOnlyInKF2; - - for(int i=0; i(pMP2->GetIndexInKeyFrame(pKF2)); - - Eigen::Vector3f P3D1c; - Eigen::Vector3f P3D2c; - - if(pMP1 && pMP2) - { - if(!pMP1->isBad() && !pMP2->isBad()) - { - g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ(); - Eigen::Vector3f P3D1w = pMP1->GetWorldPos(); - P3D1c = R1w*P3D1w + t1w; - vPoint1->setEstimate(P3D1c.cast()); - vPoint1->setId(id1); - vPoint1->setFixed(true); - optimizer.addVertex(vPoint1); - - g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ(); - Eigen::Vector3f P3D2w = pMP2->GetWorldPos(); - P3D2c = R2w*P3D2w + t2w; - vPoint2->setEstimate(P3D2c.cast()); - vPoint2->setId(id2); - vPoint2->setFixed(true); - optimizer.addVertex(vPoint2); - } - else - { - nBadMPs++; - continue; - } - } - else - { - nMatchWithoutMP++; - - //TODO The 3D position in KF1 doesn't exist - if(!pMP2->isBad()) - { - g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ(); - Eigen::Vector3f P3D2w = pMP2->GetWorldPos(); - P3D2c = R2w*P3D2w + t2w; - vPoint2->setEstimate(P3D2c.cast()); - vPoint2->setId(id2); - vPoint2->setFixed(true); - optimizer.addVertex(vPoint2); - - vIdsOnlyInKF2.push_back(id2); - } - continue; - } - - if(i2<0 && !bAllPoints) - { - Verbose::PrintMess(" Remove point -> i2: " + to_string(i2) + "; bAllPoints: " + to_string(bAllPoints), Verbose::VERBOSITY_DEBUG); - continue; - } - - if(P3D2c(2) < 0) - { - Verbose::PrintMess("Sim3: Z coordinate is negative", Verbose::VERBOSITY_DEBUG); - continue; - } - - nCorrespondences++; - - // Set edge x1 = S12*X2 - Eigen::Matrix obs1; - const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i]; - obs1 << kpUn1.pt.x, kpUn1.pt.y; - - ORB_SLAM3::EdgeSim3ProjectXYZ* e12 = new ORB_SLAM3::EdgeSim3ProjectXYZ(); - - e12->setVertex(0, dynamic_cast(optimizer.vertex(id2))); - e12->setVertex(1, dynamic_cast(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 obs2; - cv::KeyPoint kpUn2; - bool inKF2; - if(i2 >= 0) - { - kpUn2 = pKF2->mvKeysUn[i2]; - obs2 << kpUn2.pt.x, kpUn2.pt.y; - inKF2 = true; - - nInKF2++; - } - else - { - float invz = 1/P3D2c(2); - float x = P3D2c(0)*invz; - float y = P3D2c(1)*invz; - - obs2 << x, y; - kpUn2 = cv::KeyPoint(cv::Point2f(x, y), pMP2->mnTrackScaleLevel); - - inKF2 = false; - nOutKF2++; - } - - ORB_SLAM3::EdgeInverseSim3ProjectXYZ* e21 = new ORB_SLAM3::EdgeInverseSim3ProjectXYZ(); - - e21->setVertex(0, dynamic_cast(optimizer.vertex(id1))); - e21->setVertex(1, dynamic_cast(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); - - vbIsInKF2.push_back(inKF2); - } - - // Optimize! - optimizer.initializeOptimization(); - optimizer.optimize(5); - - // Check inliers - int nBad=0; - int nBadOutKF2 = 0; - for(size_t i=0; ichi2()>th2 || e21->chi2()>th2) - { - size_t idx = vnIndexEdge[i]; - vpMatches1[idx]=static_cast(NULL); - optimizer.removeEdge(e12); - optimizer.removeEdge(e21); - vpEdges12[i]=static_cast(NULL); - vpEdges21[i]=static_cast(NULL); - nBad++; - - if(!vbIsInKF2[i]) - { - nBadOutKF2++; - } - continue; - } - - //Check if remove the robust adjustment improve the result - e12->setRobustKernel(0); - e21->setRobustKernel(0); - } - - 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; - mAcumHessian = Eigen::MatrixXd::Zero(7, 7); - for(size_t i=0; icomputeError(); - e21->computeError(); - - if(e12->chi2()>th2 || e21->chi2()>th2){ - size_t idx = vnIndexEdge[i]; - vpMatches1[idx]=static_cast(NULL); - } - else{ - nIn++; - } - } - - // Recover optimized Sim3 - g2o::VertexSim3Expmap* vSim3_recov = static_cast(optimizer.vertex(0)); - g2oS12= vSim3_recov->estimate(); - - return nIn; -} - -void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int& num_fixedKF, int& num_OptKF, int& num_MPs, int& num_edges, bool bLarge, bool bRecInit) -{ - Map* pCurrentMap = pKF->GetMap(); - - int maxOpt=10; - int opt_it=10; - if(bLarge) - { - maxOpt=25; - opt_it=4; - } - const int Nd = std::min((int)pCurrentMap->KeyFramesInMap()-2,maxOpt); + const int Nd = std::min((int)pCurrentMap->KeyFramesInMap() - 2, maxOpt); const unsigned long maxKFid = pKF->mnId; - vector vpOptimizableKFs; - const vector vpNeighsKFs = pKF->GetVectorCovisibleKeyFrames(); - list lpOptVisKFs; + vector vpOptimizableKFs; + const vector vpNeighsKFs = pKF->GetVectorCovisibleKeyFrames(); + list lpOptVisKFs; vpOptimizableKFs.reserve(Nd); vpOptimizableKFs.push_back(pKF); pKF->mnBALocalForKF = pKF->mnId; - for(int i=1; imPrevKF) + if (vpOptimizableKFs.back()->mPrevKF) { vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mPrevKF); vpOptimizableKFs.back()->mnBALocalForKF = pKF->mnId; @@ -2415,63 +1661,63 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int& int N = vpOptimizableKFs.size(); // Optimizable points seen by temporal optimizable keyframes - list lLocalMapPoints; - for(int i=0; i lLocalMapPoints; + for (int i = 0; i < N; i++) { - vector vpMPs = vpOptimizableKFs[i]->GetMapPointMatches(); - for(vector::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++) + vector vpMPs = vpOptimizableKFs[i]->GetMapPointMatches(); + for (vector::iterator vit = vpMPs.begin(), vend = vpMPs.end(); vit != vend; vit++) { - MapPoint* pMP = *vit; - if(pMP) - if(!pMP->isBad()) - if(pMP->mnBALocalForKF!=pKF->mnId) + MapPoint *pMP = *vit; + if (pMP) + if (!pMP->isBad()) + if (pMP->mnBALocalForKF != pKF->mnId) { lLocalMapPoints.push_back(pMP); - pMP->mnBALocalForKF=pKF->mnId; + pMP->mnBALocalForKF = pKF->mnId; } } } // Fixed Keyframe: First frame previous KF to optimization window) - list lFixedKeyFrames; - if(vpOptimizableKFs.back()->mPrevKF) + list lFixedKeyFrames; + if (vpOptimizableKFs.back()->mPrevKF) { lFixedKeyFrames.push_back(vpOptimizableKFs.back()->mPrevKF); - vpOptimizableKFs.back()->mPrevKF->mnBAFixedForKF=pKF->mnId; + vpOptimizableKFs.back()->mPrevKF->mnBAFixedForKF = pKF->mnId; } else { - vpOptimizableKFs.back()->mnBALocalForKF=0; - vpOptimizableKFs.back()->mnBAFixedForKF=pKF->mnId; + vpOptimizableKFs.back()->mnBALocalForKF = 0; + vpOptimizableKFs.back()->mnBAFixedForKF = pKF->mnId; lFixedKeyFrames.push_back(vpOptimizableKFs.back()); vpOptimizableKFs.pop_back(); } // Optimizable visual KFs const int maxCovKF = 0; - for(int i=0, iend=vpNeighsKFs.size(); i= maxCovKF) + if (lpOptVisKFs.size() >= maxCovKF) break; - KeyFrame* pKFi = vpNeighsKFs[i]; - if(pKFi->mnBALocalForKF == pKF->mnId || pKFi->mnBAFixedForKF == pKF->mnId) + KeyFrame *pKFi = vpNeighsKFs[i]; + if (pKFi->mnBALocalForKF == pKF->mnId || pKFi->mnBAFixedForKF == pKF->mnId) continue; pKFi->mnBALocalForKF = pKF->mnId; - if(!pKFi->isBad() && pKFi->GetMap() == pCurrentMap) + if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap) { lpOptVisKFs.push_back(pKFi); - vector vpMPs = pKFi->GetMapPointMatches(); - for(vector::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++) + vector vpMPs = pKFi->GetMapPointMatches(); + for (vector::iterator vit = vpMPs.begin(), vend = vpMPs.end(); vit != vend; vit++) { - MapPoint* pMP = *vit; - if(pMP) - if(!pMP->isBad()) - if(pMP->mnBALocalForKF!=pKF->mnId) + MapPoint *pMP = *vit; + if (pMP) + if (!pMP->isBad()) + if (pMP->mnBALocalForKF != pKF->mnId) { lLocalMapPoints.push_back(pMP); - pMP->mnBALocalForKF=pKF->mnId; + pMP->mnBALocalForKF = pKF->mnId; } } } @@ -2480,24 +1726,24 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int& // Fixed KFs which are not covisible optimizable const int maxFixKF = 200; - for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) { - map> observations = (*lit)->GetObservations(); - for(map>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + map> observations = (*lit)->GetObservations(); + for (map>::iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) { - KeyFrame* pKFi = mit->first; + KeyFrame *pKFi = mit->first; - if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId) + if (pKFi->mnBALocalForKF != pKF->mnId && pKFi->mnBAFixedForKF != pKF->mnId) { - pKFi->mnBAFixedForKF=pKF->mnId; - if(!pKFi->isBad()) + pKFi->mnBAFixedForKF = pKF->mnId; + if (!pKFi->isBad()) { lFixedKeyFrames.push_back(pKFi); break; } } } - if(lFixedKeyFrames.size()>=maxFixKF) + if (lFixedKeyFrames.size() >= maxFixKF) break; } @@ -2505,156 +1751,155 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int& // Setup optimizer g2o::SparseOptimizer optimizer; - g2o::BlockSolverX::LinearSolverType * linearSolver; + g2o::BlockSolverX::LinearSolverType *linearSolver; linearSolver = new g2o::LinearSolverEigen(); - g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); - if(bLarge) + if (bLarge) { - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); solver->setUserLambdaInit(1e-2); // to avoid iterating for finding optimal lambda optimizer.setAlgorithm(solver); } else { - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); solver->setUserLambdaInit(1e0); optimizer.setAlgorithm(solver); } - // Set Local temporal KeyFrame vertices - N=vpOptimizableKFs.size(); - for(int i=0; isetId(pKFi->mnId); VP->setFixed(false); optimizer.addVertex(VP); - if(pKFi->bImu) + if (pKFi->bImu) { - VertexVelocity* VV = new VertexVelocity(pKFi); - VV->setId(maxKFid+3*(pKFi->mnId)+1); + VertexVelocity *VV = new VertexVelocity(pKFi); + VV->setId(maxKFid + 3 * (pKFi->mnId) + 1); VV->setFixed(false); optimizer.addVertex(VV); - VertexGyroBias* VG = new VertexGyroBias(pKFi); - VG->setId(maxKFid+3*(pKFi->mnId)+2); + VertexGyroBias *VG = new VertexGyroBias(pKFi); + VG->setId(maxKFid + 3 * (pKFi->mnId) + 2); VG->setFixed(false); optimizer.addVertex(VG); - VertexAccBias* VA = new VertexAccBias(pKFi); - VA->setId(maxKFid+3*(pKFi->mnId)+3); + VertexAccBias *VA = new VertexAccBias(pKFi); + VA->setId(maxKFid + 3 * (pKFi->mnId) + 3); VA->setFixed(false); optimizer.addVertex(VA); } } // Set Local visual KeyFrame vertices - for(list::iterator it=lpOptVisKFs.begin(), itEnd = lpOptVisKFs.end(); it!=itEnd; it++) + for (list::iterator it = lpOptVisKFs.begin(), itEnd = lpOptVisKFs.end(); it != itEnd; it++) { - KeyFrame* pKFi = *it; - VertexPose * VP = new VertexPose(pKFi); + KeyFrame *pKFi = *it; + VertexPose *VP = new VertexPose(pKFi); VP->setId(pKFi->mnId); VP->setFixed(false); optimizer.addVertex(VP); } // Set Fixed KeyFrame vertices - for(list::iterator lit=lFixedKeyFrames.begin(), lend=lFixedKeyFrames.end(); lit!=lend; lit++) + for (list::iterator lit = lFixedKeyFrames.begin(), lend = lFixedKeyFrames.end(); lit != lend; lit++) { - KeyFrame* pKFi = *lit; - VertexPose * VP = new VertexPose(pKFi); + KeyFrame *pKFi = *lit; + VertexPose *VP = new VertexPose(pKFi); VP->setId(pKFi->mnId); VP->setFixed(true); optimizer.addVertex(VP); - if(pKFi->bImu) // This should be done only for keyframe just before temporal window + if (pKFi->bImu) // This should be done only for keyframe just before temporal window { - VertexVelocity* VV = new VertexVelocity(pKFi); - VV->setId(maxKFid+3*(pKFi->mnId)+1); + VertexVelocity *VV = new VertexVelocity(pKFi); + VV->setId(maxKFid + 3 * (pKFi->mnId) + 1); VV->setFixed(true); optimizer.addVertex(VV); - VertexGyroBias* VG = new VertexGyroBias(pKFi); - VG->setId(maxKFid+3*(pKFi->mnId)+2); + VertexGyroBias *VG = new VertexGyroBias(pKFi); + VG->setId(maxKFid + 3 * (pKFi->mnId) + 2); VG->setFixed(true); optimizer.addVertex(VG); - VertexAccBias* VA = new VertexAccBias(pKFi); - VA->setId(maxKFid+3*(pKFi->mnId)+3); + VertexAccBias *VA = new VertexAccBias(pKFi); + VA->setId(maxKFid + 3 * (pKFi->mnId) + 3); VA->setFixed(true); optimizer.addVertex(VA); } } // Create intertial constraints - vector vei(N,(EdgeInertial*)NULL); - vector vegr(N,(EdgeGyroRW*)NULL); - vector vear(N,(EdgeAccRW*)NULL); + vector vei(N, (EdgeInertial *)NULL); + vector vegr(N, (EdgeGyroRW *)NULL); + vector vear(N, (EdgeAccRW *)NULL); - for(int i=0;imPrevKF) + if (!pKFi->mPrevKF) { cout << "NOT INERTIAL LINK TO PREVIOUS FRAME!!!!" << endl; continue; } - if(pKFi->bImu && pKFi->mPrevKF->bImu && pKFi->mpImuPreintegrated) + if (pKFi->bImu && pKFi->mPrevKF->bImu && pKFi->mpImuPreintegrated) { pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias()); - g2o::HyperGraph::Vertex* VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); - g2o::HyperGraph::Vertex* VV1 = optimizer.vertex(maxKFid+3*(pKFi->mPrevKF->mnId)+1); - g2o::HyperGraph::Vertex* VG1 = optimizer.vertex(maxKFid+3*(pKFi->mPrevKF->mnId)+2); - g2o::HyperGraph::Vertex* VA1 = optimizer.vertex(maxKFid+3*(pKFi->mPrevKF->mnId)+3); - g2o::HyperGraph::Vertex* VP2 = optimizer.vertex(pKFi->mnId); - g2o::HyperGraph::Vertex* VV2 = optimizer.vertex(maxKFid+3*(pKFi->mnId)+1); - g2o::HyperGraph::Vertex* VG2 = optimizer.vertex(maxKFid+3*(pKFi->mnId)+2); - g2o::HyperGraph::Vertex* VA2 = optimizer.vertex(maxKFid+3*(pKFi->mnId)+3); + g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); + g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 1); + g2o::HyperGraph::Vertex *VG1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 2); + g2o::HyperGraph::Vertex *VA1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 3); + g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId); + g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1); + g2o::HyperGraph::Vertex *VG2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2); + g2o::HyperGraph::Vertex *VA2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3); - if(!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2 || !VG2 || !VA2) + if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2 || !VG2 || !VA2) { - cerr << "Error " << VP1 << ", "<< VV1 << ", "<< VG1 << ", "<< VA1 << ", " << VP2 << ", " << VV2 << ", "<< VG2 << ", "<< VA2 <mpImuPreintegrated); - vei[i]->setVertex(0,dynamic_cast(VP1)); - vei[i]->setVertex(1,dynamic_cast(VV1)); - vei[i]->setVertex(2,dynamic_cast(VG1)); - vei[i]->setVertex(3,dynamic_cast(VA1)); - vei[i]->setVertex(4,dynamic_cast(VP2)); - vei[i]->setVertex(5,dynamic_cast(VV2)); + vei[i]->setVertex(0, dynamic_cast(VP1)); + vei[i]->setVertex(1, dynamic_cast(VV1)); + vei[i]->setVertex(2, dynamic_cast(VG1)); + vei[i]->setVertex(3, dynamic_cast(VA1)); + vei[i]->setVertex(4, dynamic_cast(VP2)); + vei[i]->setVertex(5, dynamic_cast(VV2)); - if(i==N-1 || bRecInit) + if (i == N - 1 || bRecInit) { // All inertial residuals are included without robust cost function, but not that one linking the // last optimizable keyframe inside of the local window and the first fixed keyframe out. The // information matrix for this measurement is also downweighted. This is done to avoid accumulating // error due to fixing variables. - g2o::RobustKernelHuber* rki = new g2o::RobustKernelHuber; + g2o::RobustKernelHuber *rki = new g2o::RobustKernelHuber; vei[i]->setRobustKernel(rki); - if(i==N-1) - vei[i]->setInformation(vei[i]->information()*1e-2); + if (i == N - 1) + vei[i]->setInformation(vei[i]->information() * 1e-2); rki->setDelta(sqrt(16.92)); } optimizer.addEdge(vei[i]); vegr[i] = new EdgeGyroRW(); - vegr[i]->setVertex(0,VG1); - vegr[i]->setVertex(1,VG2); - Eigen::Matrix3d InfoG = pKFi->mpImuPreintegrated->C.block<3,3>(9,9).cast().inverse(); + vegr[i]->setVertex(0, VG1); + vegr[i]->setVertex(1, VG2); + Eigen::Matrix3d InfoG = pKFi->mpImuPreintegrated->C.block<3, 3>(9, 9).cast().inverse(); vegr[i]->setInformation(InfoG); optimizer.addEdge(vegr[i]); vear[i] = new EdgeAccRW(); - vear[i]->setVertex(0,VA1); - vear[i]->setVertex(1,VA2); - Eigen::Matrix3d InfoA = pKFi->mpImuPreintegrated->C.block<3,3>(12,12).cast().inverse(); - vear[i]->setInformation(InfoA); + vear[i]->setVertex(0, VA1); + vear[i]->setVertex(1, VA2); + Eigen::Matrix3d InfoA = pKFi->mpImuPreintegrated->C.block<3, 3>(12, 12).cast().inverse(); + vear[i]->setInformation(InfoA); optimizer.addEdge(vear[i]); } @@ -2663,96 +1908,94 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int& } // Set MapPoint vertices - const int nExpectedSize = (N+lFixedKeyFrames.size())*lLocalMapPoints.size(); + const int nExpectedSize = (N + lFixedKeyFrames.size()) * lLocalMapPoints.size(); // Mono - vector vpEdgesMono; + vector vpEdgesMono; vpEdgesMono.reserve(nExpectedSize); - vector vpEdgeKFMono; + vector vpEdgeKFMono; vpEdgeKFMono.reserve(nExpectedSize); - vector vpMapPointEdgeMono; + vector vpMapPointEdgeMono; vpMapPointEdgeMono.reserve(nExpectedSize); // Stereo - vector vpEdgesStereo; + vector vpEdgesStereo; vpEdgesStereo.reserve(nExpectedSize); - vector vpEdgeKFStereo; + vector vpEdgeKFStereo; vpEdgeKFStereo.reserve(nExpectedSize); - vector vpMapPointEdgeStereo; + vector vpMapPointEdgeStereo; vpMapPointEdgeStereo.reserve(nExpectedSize); - - const float thHuberMono = sqrt(5.991); const float chi2Mono2 = 5.991; const float thHuberStereo = sqrt(7.815); const float chi2Stereo2 = 7.815; - const unsigned long iniMPid = maxKFid*5; + const unsigned long iniMPid = maxKFid * 5; - map mVisEdges; - for(int i=0;i mVisEdges; + for (int i = 0; i < N; i++) { - KeyFrame* pKFi = vpOptimizableKFs[i]; + KeyFrame *pKFi = vpOptimizableKFs[i]; mVisEdges[pKFi->mnId] = 0; } - for(list::iterator lit=lFixedKeyFrames.begin(), lend=lFixedKeyFrames.end(); lit!=lend; lit++) + for (list::iterator lit = lFixedKeyFrames.begin(), lend = lFixedKeyFrames.end(); lit != lend; lit++) { mVisEdges[(*lit)->mnId] = 0; } - for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) { - MapPoint* pMP = *lit; - g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ(); + MapPoint *pMP = *lit; + g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ(); vPoint->setEstimate(pMP->GetWorldPos().cast()); - unsigned long id = pMP->mnId+iniMPid+1; + unsigned long id = pMP->mnId + iniMPid + 1; vPoint->setId(id); vPoint->setMarginalized(true); optimizer.addVertex(vPoint); - const map> observations = pMP->GetObservations(); + const map> observations = pMP->GetObservations(); // Create visual constraints - for(map>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + for (map>::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) { - KeyFrame* pKFi = mit->first; + KeyFrame *pKFi = mit->first; - if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId) + if (pKFi->mnBALocalForKF != pKF->mnId && pKFi->mnBAFixedForKF != pKF->mnId) continue; - if(!pKFi->isBad() && pKFi->GetMap() == pCurrentMap) + if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap) { const int leftIndex = get<0>(mit->second); cv::KeyPoint kpUn; // Monocular left observation - if(leftIndex != -1 && pKFi->mvuRight[leftIndex]<0) + if (leftIndex != -1 && pKFi->mvuRight[leftIndex] < 0) { mVisEdges[pKFi->mnId]++; kpUn = pKFi->mvKeysUn[leftIndex]; - Eigen::Matrix obs; + Eigen::Matrix obs; obs << kpUn.pt.x, kpUn.pt.y; - EdgeMono* e = new EdgeMono(0); + EdgeMono *e = new EdgeMono(0); - e->setVertex(0, dynamic_cast(optimizer.vertex(id))); - e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); e->setMeasurement(obs); // Add here uncerteinty const float unc2 = pKFi->mpCamera->uncertainty2(obs); - const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]/unc2; - e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); + const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave] / unc2; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); rk->setDelta(thHuberMono); @@ -2762,28 +2005,28 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int& vpMapPointEdgeMono.push_back(pMP); } // Stereo-observation - else if(leftIndex != -1)// Stereo observation + else if (leftIndex != -1) // Stereo observation { kpUn = pKFi->mvKeysUn[leftIndex]; mVisEdges[pKFi->mnId]++; const float kp_ur = pKFi->mvuRight[leftIndex]; - Eigen::Matrix obs; + Eigen::Matrix obs; obs << kpUn.pt.x, kpUn.pt.y, kp_ur; - EdgeStereo* e = new EdgeStereo(0); + EdgeStereo *e = new EdgeStereo(0); - e->setVertex(0, dynamic_cast(optimizer.vertex(id))); - e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); e->setMeasurement(obs); // Add here uncerteinty const float unc2 = pKFi->mpCamera->uncertainty2(obs.head(2)); - const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]/unc2; - e->setInformation(Eigen::Matrix3d::Identity()*invSigma2); + const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave] / unc2; + e->setInformation(Eigen::Matrix3d::Identity() * invSigma2); - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); rk->setDelta(thHuberStereo); @@ -2794,30 +2037,32 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int& } // Monocular right observation - if(pKFi->mpCamera2){ + if (pKFi->mpCamera2) + { int rightIndex = get<1>(mit->second); - if(rightIndex != -1 ){ + if (rightIndex != -1) + { rightIndex -= pKFi->NLeft; mVisEdges[pKFi->mnId]++; - Eigen::Matrix obs; + Eigen::Matrix obs; cv::KeyPoint kp = pKFi->mvKeysRight[rightIndex]; obs << kp.pt.x, kp.pt.y; - EdgeMono* e = new EdgeMono(1); + EdgeMono *e = new EdgeMono(1); - e->setVertex(0, dynamic_cast(optimizer.vertex(id))); - e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); e->setMeasurement(obs); // Add here uncerteinty const float unc2 = pKFi->mpCamera->uncertainty2(obs); - const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]/unc2; - e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); + const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave] / unc2; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); rk->setDelta(thHuberMono); @@ -2831,11 +2076,11 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int& } } - //cout << "Total map points: " << lLocalMapPoints.size() << endl; - // TODO debug会报错先注释掉 - for(map::iterator mit=mVisEdges.begin(), mend=mVisEdges.end(); mit!=mend; mit++) + // cout << "Total map points: " << lLocalMapPoints.size() << endl; + // TODO debug会报错先注释掉 + for (map::iterator mit = mVisEdges.begin(), mend = mVisEdges.end(); mit != mend; mit++) { - assert(mit->second>=3); + assert(mit->second >= 3); } optimizer.initializeOptimization(); @@ -2843,114 +2088,109 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int& float err = optimizer.activeRobustChi2(); optimizer.optimize(opt_it); // Originally to 2 float err_end = optimizer.activeRobustChi2(); - if(pbStopFlag) + if (pbStopFlag) optimizer.setForceStopFlag(pbStopFlag); - vector > vToErase; - vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size()); + vector> vToErase; + vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size()); // Check inlier observations // Mono - for(size_t i=0, iend=vpEdgesMono.size(); imTrackDepth<10.f; + EdgeMono *e = vpEdgesMono[i]; + MapPoint *pMP = vpMapPointEdgeMono[i]; + bool bClose = pMP->mTrackDepth < 10.f; - if(pMP->isBad()) + if (pMP->isBad()) continue; - if((e->chi2()>chi2Mono2 && !bClose) || (e->chi2()>1.5f*chi2Mono2 && bClose) || !e->isDepthPositive()) + if ((e->chi2() > chi2Mono2 && !bClose) || (e->chi2() > 1.5f * chi2Mono2 && bClose) || !e->isDepthPositive()) { - KeyFrame* pKFi = vpEdgeKFMono[i]; - vToErase.push_back(make_pair(pKFi,pMP)); + KeyFrame *pKFi = vpEdgeKFMono[i]; + vToErase.push_back(make_pair(pKFi, pMP)); } } - // Stereo - for(size_t i=0, iend=vpEdgesStereo.size(); iisBad()) + if (pMP->isBad()) continue; - if(e->chi2()>chi2Stereo2) + if (e->chi2() > chi2Stereo2) { - KeyFrame* pKFi = vpEdgeKFStereo[i]; - vToErase.push_back(make_pair(pKFi,pMP)); + KeyFrame *pKFi = vpEdgeKFStereo[i]; + vToErase.push_back(make_pair(pKFi, pMP)); } } // Get Map Mutex and erase outliers unique_lock lock(pMap->mMutexMapUpdate); - // TODO: Some convergence problems have been detected here - if((2*err < err_end || isnan(err) || isnan(err_end)) && !bLarge) //bGN) + if ((2 * err < err_end || isnan(err) || isnan(err_end)) && !bLarge) // bGN) { cout << "FAIL LOCAL-INERTIAL BA!!!!" << endl; return; } - - - if(!vToErase.empty()) + if (!vToErase.empty()) { - for(size_t i=0;iEraseMapPointMatch(pMPi); pMPi->EraseObservation(pKFi); } } - for(list::iterator lit=lFixedKeyFrames.begin(), lend=lFixedKeyFrames.end(); lit!=lend; lit++) + for (list::iterator lit = lFixedKeyFrames.begin(), lend = lFixedKeyFrames.end(); lit != lend; lit++) (*lit)->mnBAFixedForKF = 0; // Recover optimized data // Local temporal Keyframes - N=vpOptimizableKFs.size(); - for(int i=0; i(optimizer.vertex(pKFi->mnId)); + VertexPose *VP = static_cast(optimizer.vertex(pKFi->mnId)); Sophus::SE3f Tcw(VP->estimate().Rcw[0].cast(), VP->estimate().tcw[0].cast()); pKFi->SetPose(Tcw); - pKFi->mnBALocalForKF=0; + pKFi->mnBALocalForKF = 0; - if(pKFi->bImu) + if (pKFi->bImu) { - VertexVelocity* VV = static_cast(optimizer.vertex(maxKFid+3*(pKFi->mnId)+1)); + VertexVelocity *VV = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1)); pKFi->SetVelocity(VV->estimate().cast()); - VertexGyroBias* VG = static_cast(optimizer.vertex(maxKFid+3*(pKFi->mnId)+2)); - VertexAccBias* VA = static_cast(optimizer.vertex(maxKFid+3*(pKFi->mnId)+3)); + VertexGyroBias *VG = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2)); + VertexAccBias *VA = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3)); Vector6d b; b << VG->estimate(), VA->estimate(); - pKFi->SetNewBias(IMU::Bias(b[3],b[4],b[5],b[0],b[1],b[2])); - + pKFi->SetNewBias(IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2])); } } // Local visual KeyFrame - for(list::iterator it=lpOptVisKFs.begin(), itEnd = lpOptVisKFs.end(); it!=itEnd; it++) + for (list::iterator it = lpOptVisKFs.begin(), itEnd = lpOptVisKFs.end(); it != itEnd; it++) { - KeyFrame* pKFi = *it; - VertexPose* VP = static_cast(optimizer.vertex(pKFi->mnId)); + KeyFrame *pKFi = *it; + VertexPose *VP = static_cast(optimizer.vertex(pKFi->mnId)); Sophus::SE3f Tcw(VP->estimate().Rcw[0].cast(), VP->estimate().tcw[0].cast()); pKFi->SetPose(Tcw); - pKFi->mnBALocalForKF=0; + pKFi->mnBALocalForKF = 0; } - //Points - for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) + // Points + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) { - MapPoint* pMP = *lit; - g2o::VertexSBAPointXYZ* vPoint = static_cast(optimizer.vertex(pMP->mnId+iniMPid+1)); + MapPoint *pMP = *lit; + g2o::VertexSBAPointXYZ *vPoint = static_cast(optimizer.vertex(pMP->mnId + iniMPid + 1)); pMP->SetWorldPos(vPoint->estimate().cast()); pMP->UpdateNormalAndDepth(); } @@ -2958,123 +2198,805 @@ void Optimizer::LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, int& pMap->IncreaseChangeIndex(); } -Eigen::MatrixXd Optimizer::Marginalize(const Eigen::MatrixXd &H, const int &start, const int &end) +/**************************************以下为全局优化**************************************************************/ + +void Optimizer::GlobalBundleAdjustemnt( + Map *pMap, int nIterations, bool *pbStopFlag, const unsigned long nLoopKF, const bool bRobust) { - // Goal - // a | ab | ac a* | 0 | ac* - // ba | b | bc --> 0 | 0 | 0 - // ca | cb | c ca* | 0 | c* - - // Size of block before block to marginalize - const int a = start; - // Size of block to marginalize - const int b = end-start+1; - // Size of block after block to marginalize - const int c = H.cols() - (end+1); - - // Reorder as follows: - // a | ab | ac a | ac | ab - // ba | b | bc --> ca | c | cb - // ca | cb | c ba | bc | b - - Eigen::MatrixXd Hn = Eigen::MatrixXd::Zero(H.rows(),H.cols()); - if(a>0) - { - Hn.block(0,0,a,a) = H.block(0,0,a,a); - Hn.block(0,a+c,a,b) = H.block(0,a,a,b); - Hn.block(a+c,0,b,a) = H.block(a,0,b,a); - } - if(a>0 && c>0) - { - Hn.block(0,a,a,c) = H.block(0,a+b,a,c); - Hn.block(a,0,c,a) = H.block(a+b,0,c,a); - } - if(c>0) - { - Hn.block(a,a,c,c) = H.block(a+b,a+b,c,c); - Hn.block(a,a+c,c,b) = H.block(a+b,a,c,b); - Hn.block(a+c,a,b,c) = H.block(a,a+b,b,c); - } - Hn.block(a+c,a+c,b,b) = H.block(a,a,b,b); - - // Perform marginalization (Schur complement) - Eigen::JacobiSVD svd(Hn.block(a+c,a+c,b,b),Eigen::ComputeThinU | Eigen::ComputeThinV); - Eigen::JacobiSVD::SingularValuesType singularValues_inv=svd.singularValues(); - for (int i=0; i1e-6) - singularValues_inv(i)=1.0/singularValues_inv(i); - else singularValues_inv(i)=0; - } - Eigen::MatrixXd invHb = svd.matrixV()*singularValues_inv.asDiagonal()*svd.matrixU().transpose(); - Hn.block(0,0,a+c,a+c) = Hn.block(0,0,a+c,a+c) - Hn.block(0,a+c,a+c,b)*invHb*Hn.block(a+c,0,b,a+c); - Hn.block(a+c,a+c,b,b) = Eigen::MatrixXd::Zero(b,b); - Hn.block(0,a+c,a+c,b) = Eigen::MatrixXd::Zero(a+c,b); - Hn.block(a+c,0,b,a+c) = Eigen::MatrixXd::Zero(b,a+c); - - // Inverse reorder - // a* | ac* | 0 a* | 0 | ac* - // ca* | c* | 0 --> 0 | 0 | 0 - // 0 | 0 | 0 ca* | 0 | c* - Eigen::MatrixXd res = Eigen::MatrixXd::Zero(H.rows(),H.cols()); - if(a>0) - { - res.block(0,0,a,a) = Hn.block(0,0,a,a); - res.block(0,a,a,b) = Hn.block(0,a+c,a,b); - res.block(a,0,b,a) = Hn.block(a+c,0,b,a); - } - if(a>0 && c>0) - { - res.block(0,a+b,a,c) = Hn.block(0,a,a,c); - res.block(a+b,0,c,a) = Hn.block(a,0,c,a); - } - if(c>0) - { - res.block(a+b,a+b,c,c) = Hn.block(a,a,c,c); - res.block(a+b,a,c,b) = Hn.block(a,a+c,c,b); - res.block(a,a+b,b,c) = Hn.block(a+c,a,b,c); - } - - res.block(a,a,b,b) = Hn.block(a+c,a+c,b,b); - - return res; + vector vpKFs = pMap->GetAllKeyFrames(); + vector vpMP = pMap->GetAllMapPoints(); + BundleAdjustment(vpKFs, vpMP, nIterations, pbStopFlag, nLoopKF, bRobust); } -void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale, Eigen::Vector3d &bg, Eigen::Vector3d &ba, bool bMono, Eigen::MatrixXd &covInertial, bool bFixedVel, bool bGauss, float priorG, float priorA) +void Optimizer::BundleAdjustment( + const vector &vpKFs, const vector &vpMP, + int nIterations, bool *pbStopFlag, const unsigned long nLoopKF, const bool bRobust) +{ + vector vbNotIncludedMP; + vbNotIncludedMP.resize(vpMP.size()); + + Map *pMap = vpKFs[0]->GetMap(); + + g2o::SparseOptimizer optimizer; + g2o::BlockSolver_6_3::LinearSolverType *linearSolver; + + linearSolver = new g2o::LinearSolverEigen(); + + g2o::BlockSolver_6_3 *solver_ptr = new g2o::BlockSolver_6_3(linearSolver); + + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + optimizer.setAlgorithm(solver); + optimizer.setVerbose(false); + + if (pbStopFlag) + optimizer.setForceStopFlag(pbStopFlag); + + long unsigned int maxKFid = 0; + + const int nExpectedSize = (vpKFs.size()) * vpMP.size(); + + vector vpEdgesMono; + vpEdgesMono.reserve(nExpectedSize); + + vector vpEdgesBody; + vpEdgesBody.reserve(nExpectedSize); + + vector vpEdgeKFMono; + vpEdgeKFMono.reserve(nExpectedSize); + + vector vpEdgeKFBody; + vpEdgeKFBody.reserve(nExpectedSize); + + vector vpMapPointEdgeMono; + vpMapPointEdgeMono.reserve(nExpectedSize); + + vector vpMapPointEdgeBody; + vpMapPointEdgeBody.reserve(nExpectedSize); + + vector vpEdgesStereo; + vpEdgesStereo.reserve(nExpectedSize); + + vector vpEdgeKFStereo; + vpEdgeKFStereo.reserve(nExpectedSize); + + vector vpMapPointEdgeStereo; + vpMapPointEdgeStereo.reserve(nExpectedSize); + + // Set KeyFrame vertices + + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKF = vpKFs[i]; + if (pKF->isBad()) + continue; + g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap(); + Sophus::SE3 Tcw = pKF->GetPose(); + vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(), Tcw.translation().cast())); + vSE3->setId(pKF->mnId); + vSE3->setFixed(pKF->mnId == pMap->GetInitKFid()); + 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(pMP->GetWorldPos().cast()); + const int id = pMP->mnId + maxKFid + 1; + vPoint->setId(id); + vPoint->setMarginalized(true); + optimizer.addVertex(vPoint); + + const map> observations = pMP->GetObservations(); + + int nEdges = 0; + // SET EDGES + for (map>::const_iterator mit = observations.begin(); mit != observations.end(); mit++) + { + KeyFrame *pKF = mit->first; + if (pKF->isBad() || pKF->mnId > maxKFid) + continue; + if (optimizer.vertex(id) == NULL || optimizer.vertex(pKF->mnId) == NULL) + continue; + nEdges++; + + const int leftIndex = get<0>(mit->second); + + if (leftIndex != -1 && pKF->mvuRight[get<0>(mit->second)] < 0) + { + const cv::KeyPoint &kpUn = pKF->mvKeysUn[leftIndex]; + + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y; + + ORB_SLAM3::EdgeSE3ProjectXYZ *e = new ORB_SLAM3::EdgeSE3ProjectXYZ(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(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->pCamera = pKF->mpCamera; + + optimizer.addEdge(e); + + vpEdgesMono.push_back(e); + vpEdgeKFMono.push_back(pKF); + vpMapPointEdgeMono.push_back(pMP); + } + else if (leftIndex != -1 && pKF->mvuRight[leftIndex] >= 0) // Stereo observation + { + const cv::KeyPoint &kpUn = pKF->mvKeysUn[leftIndex]; + + Eigen::Matrix obs; + const float kp_ur = pKF->mvuRight[get<0>(mit->second)]; + obs << kpUn.pt.x, kpUn.pt.y, kp_ur; + + g2o::EdgeStereoSE3ProjectXYZ *e = new g2o::EdgeStereoSE3ProjectXYZ(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(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); + + vpEdgesStereo.push_back(e); + vpEdgeKFStereo.push_back(pKF); + vpMapPointEdgeStereo.push_back(pMP); + } + + if (pKF->mpCamera2) + { + int rightIndex = get<1>(mit->second); + + if (rightIndex != -1 && rightIndex < pKF->mvKeysRight.size()) + { + rightIndex -= pKF->NLeft; + + Eigen::Matrix obs; + cv::KeyPoint kp = pKF->mvKeysRight[rightIndex]; + obs << kp.pt.x, kp.pt.y; + + ORB_SLAM3::EdgeSE3ProjectXYZToBody *e = new ORB_SLAM3::EdgeSE3ProjectXYZToBody(); + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKF->mnId))); + e->setMeasurement(obs); + const float &invSigma2 = pKF->mvInvLevelSigma2[kp.octave]; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuber2D); + + Sophus::SE3f Trl = pKF->GetRelativePoseTrl(); + e->mTrl = g2o::SE3Quat(Trl.unit_quaternion().cast(), Trl.translation().cast()); + + e->pCamera = pKF->mpCamera2; + + optimizer.addEdge(e); + vpEdgesBody.push_back(e); + vpEdgeKFBody.push_back(pKF); + vpMapPointEdgeBody.push_back(pMP); + } + } + } + + if (nEdges == 0) + { + optimizer.removeVertex(vPoint); + vbNotIncludedMP[i] = true; + } + else + { + vbNotIncludedMP[i] = false; + } + } + + // Optimize! + optimizer.setVerbose(false); + optimizer.initializeOptimization(); + optimizer.optimize(nIterations); + Verbose::PrintMess("BA: End of the optimization", Verbose::VERBOSITY_NORMAL); + + // 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(optimizer.vertex(pKF->mnId)); + + g2o::SE3Quat SE3quat = vSE3->estimate(); + if (nLoopKF == pMap->GetOriginKF()->mnId) + { + pKF->SetPose(Sophus::SE3f(SE3quat.rotation().cast(), SE3quat.translation().cast())); + } + else + { + pKF->mTcwGBA = Sophus::SE3d(SE3quat.rotation(), SE3quat.translation()).cast(); + pKF->mnBAGlobalForKF = nLoopKF; + + Sophus::SE3f mTwc = pKF->GetPoseInverse(); + Sophus::SE3f mTcGBA_c = pKF->mTcwGBA * mTwc; + Eigen::Vector3f vector_dist = mTcGBA_c.translation(); + double dist = vector_dist.norm(); + if (dist > 1) + { + int numMonoBadPoints = 0, numMonoOptPoints = 0; + int numStereoBadPoints = 0, numStereoOptPoints = 0; + vector vpMonoMPsOpt, vpStereoMPsOpt; + + for (size_t i2 = 0, iend = vpEdgesMono.size(); i2 < iend; i2++) + { + ORB_SLAM3::EdgeSE3ProjectXYZ *e = vpEdgesMono[i2]; + MapPoint *pMP = vpMapPointEdgeMono[i2]; + KeyFrame *pKFedge = vpEdgeKFMono[i2]; + + if (pKF != pKFedge) + { + continue; + } + + if (pMP->isBad()) + continue; + + if (e->chi2() > 5.991 || !e->isDepthPositive()) + { + numMonoBadPoints++; + } + else + { + numMonoOptPoints++; + vpMonoMPsOpt.push_back(pMP); + } + } + + for (size_t i2 = 0, iend = vpEdgesStereo.size(); i2 < iend; i2++) + { + g2o::EdgeStereoSE3ProjectXYZ *e = vpEdgesStereo[i2]; + MapPoint *pMP = vpMapPointEdgeStereo[i2]; + KeyFrame *pKFedge = vpEdgeKFMono[i2]; + + if (pKF != pKFedge) + { + continue; + } + + if (pMP->isBad()) + continue; + + if (e->chi2() > 7.815 || !e->isDepthPositive()) + { + numStereoBadPoints++; + } + else + { + numStereoOptPoints++; + vpStereoMPsOpt.push_back(pMP); + } + } + } + } + } + + // 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(optimizer.vertex(pMP->mnId + maxKFid + 1)); + + if (nLoopKF == pMap->GetOriginKF()->mnId) + { + pMP->SetWorldPos(vPoint->estimate().cast()); + pMP->UpdateNormalAndDepth(); + } + else + { + pMP->mPosGBA = vPoint->estimate().cast(); + pMP->mnBAGlobalForKF = nLoopKF; + } + } +} + +void Optimizer::FullInertialBA( + Map *pMap, int its, const bool bFixLocal, const long unsigned int nLoopId, bool *pbStopFlag, + bool bInit, float priorG, float priorA, Eigen::VectorXd *vSingVal, bool *bHess) +{ + long unsigned int maxKFid = pMap->GetMaxKFid(); + const vector vpKFs = pMap->GetAllKeyFrames(); + const vector vpMPs = pMap->GetAllMapPoints(); + + // Setup optimizer + g2o::SparseOptimizer optimizer; + g2o::BlockSolverX::LinearSolverType *linearSolver; + + linearSolver = new g2o::LinearSolverEigen(); + + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); + + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + solver->setUserLambdaInit(1e-5); + optimizer.setAlgorithm(solver); + optimizer.setVerbose(false); + + if (pbStopFlag) + optimizer.setForceStopFlag(pbStopFlag); + + int nNonFixed = 0; + + // Set KeyFrame vertices + KeyFrame *pIncKF; + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKFi = vpKFs[i]; + if (pKFi->mnId > maxKFid) + continue; + VertexPose *VP = new VertexPose(pKFi); + VP->setId(pKFi->mnId); + pIncKF = pKFi; + bool bFixed = false; + if (bFixLocal) + { + bFixed = (pKFi->mnBALocalForKF >= (maxKFid - 1)) || (pKFi->mnBAFixedForKF >= (maxKFid - 1)); + if (!bFixed) + nNonFixed++; + VP->setFixed(bFixed); + } + optimizer.addVertex(VP); + + if (pKFi->bImu) + { + VertexVelocity *VV = new VertexVelocity(pKFi); + VV->setId(maxKFid + 3 * (pKFi->mnId) + 1); + VV->setFixed(bFixed); + optimizer.addVertex(VV); + if (!bInit) + { + VertexGyroBias *VG = new VertexGyroBias(pKFi); + VG->setId(maxKFid + 3 * (pKFi->mnId) + 2); + VG->setFixed(bFixed); + optimizer.addVertex(VG); + VertexAccBias *VA = new VertexAccBias(pKFi); + VA->setId(maxKFid + 3 * (pKFi->mnId) + 3); + VA->setFixed(bFixed); + optimizer.addVertex(VA); + } + } + } + + if (bInit) + { + VertexGyroBias *VG = new VertexGyroBias(pIncKF); + VG->setId(4 * maxKFid + 2); + VG->setFixed(false); + optimizer.addVertex(VG); + VertexAccBias *VA = new VertexAccBias(pIncKF); + VA->setId(4 * maxKFid + 3); + VA->setFixed(false); + optimizer.addVertex(VA); + } + + if (bFixLocal) + { + if (nNonFixed < 3) + return; + } + + // IMU links + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKFi = vpKFs[i]; + + if (!pKFi->mPrevKF) + { + Verbose::PrintMess("NOT INERTIAL LINK TO PREVIOUS FRAME!", Verbose::VERBOSITY_NORMAL); + continue; + } + + if (pKFi->mPrevKF && pKFi->mnId <= maxKFid) + { + if (pKFi->isBad() || pKFi->mPrevKF->mnId > maxKFid) + continue; + if (pKFi->bImu && pKFi->mPrevKF->bImu) + { + pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias()); + g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); + g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 1); + + g2o::HyperGraph::Vertex *VG1; + g2o::HyperGraph::Vertex *VA1; + g2o::HyperGraph::Vertex *VG2; + g2o::HyperGraph::Vertex *VA2; + if (!bInit) + { + VG1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 2); + VA1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 3); + VG2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2); + VA2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3); + } + else + { + VG1 = optimizer.vertex(4 * maxKFid + 2); + VA1 = optimizer.vertex(4 * maxKFid + 3); + } + + g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId); + g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1); + + if (!bInit) + { + if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2 || !VG2 || !VA2) + { + cout << "Error" << VP1 << ", " << VV1 << ", " << VG1 << ", " << VA1 << ", " << VP2 << ", " << VV2 << ", " << VG2 << ", " << VA2 << endl; + continue; + } + } + else + { + if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2) + { + cout << "Error" << VP1 << ", " << VV1 << ", " << VG1 << ", " << VA1 << ", " << VP2 << ", " << VV2 << endl; + continue; + } + } + + EdgeInertial *ei = new EdgeInertial(pKFi->mpImuPreintegrated); + ei->setVertex(0, dynamic_cast(VP1)); + ei->setVertex(1, dynamic_cast(VV1)); + ei->setVertex(2, dynamic_cast(VG1)); + ei->setVertex(3, dynamic_cast(VA1)); + ei->setVertex(4, dynamic_cast(VP2)); + ei->setVertex(5, dynamic_cast(VV2)); + + g2o::RobustKernelHuber *rki = new g2o::RobustKernelHuber; + ei->setRobustKernel(rki); + rki->setDelta(sqrt(16.92)); + + optimizer.addEdge(ei); + + if (!bInit) + { + EdgeGyroRW *egr = new EdgeGyroRW(); + egr->setVertex(0, VG1); + egr->setVertex(1, VG2); + Eigen::Matrix3d InfoG = pKFi->mpImuPreintegrated->C.block<3, 3>(9, 9).cast().inverse(); + egr->setInformation(InfoG); + egr->computeError(); + optimizer.addEdge(egr); + + EdgeAccRW *ear = new EdgeAccRW(); + ear->setVertex(0, VA1); + ear->setVertex(1, VA2); + Eigen::Matrix3d InfoA = pKFi->mpImuPreintegrated->C.block<3, 3>(12, 12).cast().inverse(); + ear->setInformation(InfoA); + ear->computeError(); + optimizer.addEdge(ear); + } + } + else + cout << pKFi->mnId << " or " << pKFi->mPrevKF->mnId << " no imu" << endl; + } + } + + if (bInit) + { + g2o::HyperGraph::Vertex *VG = optimizer.vertex(4 * maxKFid + 2); + g2o::HyperGraph::Vertex *VA = optimizer.vertex(4 * maxKFid + 3); + + // Add prior to comon biases + Eigen::Vector3f bprior; + bprior.setZero(); + + EdgePriorAcc *epa = new EdgePriorAcc(bprior); + epa->setVertex(0, dynamic_cast(VA)); + double infoPriorA = priorA; // + epa->setInformation(infoPriorA * Eigen::Matrix3d::Identity()); + optimizer.addEdge(epa); + + EdgePriorGyro *epg = new EdgePriorGyro(bprior); + epg->setVertex(0, dynamic_cast(VG)); + double infoPriorG = priorG; // + epg->setInformation(infoPriorG * Eigen::Matrix3d::Identity()); + optimizer.addEdge(epg); + } + + const float thHuberMono = sqrt(5.991); + const float thHuberStereo = sqrt(7.815); + + const unsigned long iniMPid = maxKFid * 5; + + vector vbNotIncludedMP(vpMPs.size(), false); + + for (size_t i = 0; i < vpMPs.size(); i++) + { + MapPoint *pMP = vpMPs[i]; + g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ(); + vPoint->setEstimate(pMP->GetWorldPos().cast()); + unsigned long id = pMP->mnId + iniMPid + 1; + vPoint->setId(id); + vPoint->setMarginalized(true); + optimizer.addVertex(vPoint); + + const map> observations = pMP->GetObservations(); + + bool bAllFixed = true; + + // Set edges + for (map>::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) + { + KeyFrame *pKFi = mit->first; + + if (pKFi->mnId > maxKFid) + continue; + + if (!pKFi->isBad()) + { + const int leftIndex = get<0>(mit->second); + cv::KeyPoint kpUn; + + if (leftIndex != -1 && pKFi->mvuRight[get<0>(mit->second)] < 0) // Monocular observation + { + kpUn = pKFi->mvKeysUn[leftIndex]; + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y; + + EdgeMono *e = new EdgeMono(0); + + g2o::OptimizableGraph::Vertex *VP = dynamic_cast(optimizer.vertex(pKFi->mnId)); + if (bAllFixed) + if (!VP->fixed()) + bAllFixed = false; + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, VP); + e->setMeasurement(obs); + const float invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; + + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberMono); + + optimizer.addEdge(e); + } + else if (leftIndex != -1 && pKFi->mvuRight[leftIndex] >= 0) // stereo observation + { + kpUn = pKFi->mvKeysUn[leftIndex]; + const float kp_ur = pKFi->mvuRight[leftIndex]; + Eigen::Matrix obs; + obs << kpUn.pt.x, kpUn.pt.y, kp_ur; + + EdgeStereo *e = new EdgeStereo(0); + + g2o::OptimizableGraph::Vertex *VP = dynamic_cast(optimizer.vertex(pKFi->mnId)); + if (bAllFixed) + if (!VP->fixed()) + bAllFixed = false; + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, VP); + e->setMeasurement(obs); + const float invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; + + e->setInformation(Eigen::Matrix3d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberStereo); + + optimizer.addEdge(e); + } + + if (pKFi->mpCamera2) + { // Monocular right observation + int rightIndex = get<1>(mit->second); + + if (rightIndex != -1 && rightIndex < pKFi->mvKeysRight.size()) + { + rightIndex -= pKFi->NLeft; + + Eigen::Matrix obs; + kpUn = pKFi->mvKeysRight[rightIndex]; + obs << kpUn.pt.x, kpUn.pt.y; + + EdgeMono *e = new EdgeMono(1); + + g2o::OptimizableGraph::Vertex *VP = dynamic_cast(optimizer.vertex(pKFi->mnId)); + if (bAllFixed) + if (!VP->fixed()) + bAllFixed = false; + + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, VP); + e->setMeasurement(obs); + const float invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); + + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; + e->setRobustKernel(rk); + rk->setDelta(thHuberMono); + + optimizer.addEdge(e); + } + } + } + } + + if (bAllFixed) + { + optimizer.removeVertex(vPoint); + vbNotIncludedMP[i] = true; + } + } + + if (pbStopFlag) + if (*pbStopFlag) + return; + + optimizer.initializeOptimization(); + optimizer.optimize(its); + + // Recover optimized data + // Keyframes + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKFi = vpKFs[i]; + if (pKFi->mnId > maxKFid) + continue; + VertexPose *VP = static_cast(optimizer.vertex(pKFi->mnId)); + if (nLoopId == 0) + { + Sophus::SE3f Tcw(VP->estimate().Rcw[0].cast(), VP->estimate().tcw[0].cast()); + pKFi->SetPose(Tcw); + } + else + { + pKFi->mTcwGBA = Sophus::SE3f(VP->estimate().Rcw[0].cast(), VP->estimate().tcw[0].cast()); + pKFi->mnBAGlobalForKF = nLoopId; + } + if (pKFi->bImu) + { + VertexVelocity *VV = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1)); + if (nLoopId == 0) + { + pKFi->SetVelocity(VV->estimate().cast()); + } + else + { + pKFi->mVwbGBA = VV->estimate().cast(); + } + + VertexGyroBias *VG; + VertexAccBias *VA; + if (!bInit) + { + VG = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2)); + VA = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3)); + } + else + { + VG = static_cast(optimizer.vertex(4 * maxKFid + 2)); + VA = static_cast(optimizer.vertex(4 * maxKFid + 3)); + } + + Vector6d vb; + vb << VG->estimate(), VA->estimate(); + IMU::Bias b(vb[3], vb[4], vb[5], vb[0], vb[1], vb[2]); + if (nLoopId == 0) + { + pKFi->SetNewBias(b); + } + else + { + pKFi->mBiasGBA = b; + } + } + } + + // Points + for (size_t i = 0; i < vpMPs.size(); i++) + { + if (vbNotIncludedMP[i]) + continue; + + MapPoint *pMP = vpMPs[i]; + g2o::VertexSBAPointXYZ *vPoint = static_cast(optimizer.vertex(pMP->mnId + iniMPid + 1)); + + if (nLoopId == 0) + { + pMP->SetWorldPos(vPoint->estimate().cast()); + pMP->UpdateNormalAndDepth(); + } + else + { + pMP->mPosGBA = vPoint->estimate().cast(); + pMP->mnBAGlobalForKF = nLoopId; + } + } + + pMap->IncreaseChangeIndex(); +} + +/**************************************以下为尺度与重力优化**************************************************************/ + +void Optimizer::InertialOptimization( + Map *pMap, Eigen::Matrix3d &Rwg, double &scale, Eigen::Vector3d &bg, Eigen::Vector3d &ba, bool bMono, + Eigen::MatrixXd &covInertial, bool bFixedVel, bool bGauss, float priorG, float priorA) { Verbose::PrintMess("inertial optimization", Verbose::VERBOSITY_NORMAL); int its = 200; long unsigned int maxKFid = pMap->GetMaxKFid(); - const vector vpKFs = pMap->GetAllKeyFrames(); + const vector vpKFs = pMap->GetAllKeyFrames(); // Setup optimizer g2o::SparseOptimizer optimizer; - g2o::BlockSolverX::LinearSolverType * linearSolver; + g2o::BlockSolverX::LinearSolverType *linearSolver; linearSolver = new g2o::LinearSolverEigen(); - g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); - if (priorG!=0.f) + if (priorG != 0.f) solver->setUserLambdaInit(1e3); optimizer.setAlgorithm(solver); // Set KeyFrame vertices (fixed poses and optimizable velocities) - for(size_t i=0; imnId>maxKFid) + KeyFrame *pKFi = vpKFs[i]; + if (pKFi->mnId > maxKFid) continue; - VertexPose * VP = new VertexPose(pKFi); + VertexPose *VP = new VertexPose(pKFi); VP->setId(pKFi->mnId); VP->setFixed(true); optimizer.addVertex(VP); - VertexVelocity* VV = new VertexVelocity(pKFi); - VV->setId(maxKFid+(pKFi->mnId)+1); + VertexVelocity *VV = new VertexVelocity(pKFi); + VV->setId(maxKFid + (pKFi->mnId) + 1); if (bFixedVel) VV->setFixed(true); else @@ -3084,15 +3006,15 @@ void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &sc } // Biases - VertexGyroBias* VG = new VertexGyroBias(vpKFs.front()); - VG->setId(maxKFid*2+2); + VertexGyroBias *VG = new VertexGyroBias(vpKFs.front()); + VG->setId(maxKFid * 2 + 2); if (bFixedVel) VG->setFixed(true); else VG->setFixed(false); optimizer.addVertex(VG); - VertexAccBias* VA = new VertexAccBias(vpKFs.front()); - VA->setId(maxKFid*2+3); + VertexAccBias *VA = new VertexAccBias(vpKFs.front()); + VA->setId(maxKFid * 2 + 3); if (bFixedVel) VA->setFixed(true); else @@ -3103,81 +3025,80 @@ void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &sc Eigen::Vector3f bprior; bprior.setZero(); - EdgePriorAcc* epa = new EdgePriorAcc(bprior); - epa->setVertex(0,dynamic_cast(VA)); + EdgePriorAcc *epa = new EdgePriorAcc(bprior); + epa->setVertex(0, dynamic_cast(VA)); double infoPriorA = priorA; - epa->setInformation(infoPriorA*Eigen::Matrix3d::Identity()); + epa->setInformation(infoPriorA * Eigen::Matrix3d::Identity()); optimizer.addEdge(epa); - EdgePriorGyro* epg = new EdgePriorGyro(bprior); - epg->setVertex(0,dynamic_cast(VG)); + EdgePriorGyro *epg = new EdgePriorGyro(bprior); + epg->setVertex(0, dynamic_cast(VG)); double infoPriorG = priorG; - epg->setInformation(infoPriorG*Eigen::Matrix3d::Identity()); + epg->setInformation(infoPriorG * Eigen::Matrix3d::Identity()); optimizer.addEdge(epg); // Gravity and scale - VertexGDir* VGDir = new VertexGDir(Rwg); - VGDir->setId(maxKFid*2+4); + VertexGDir *VGDir = new VertexGDir(Rwg); + VGDir->setId(maxKFid * 2 + 4); VGDir->setFixed(false); optimizer.addVertex(VGDir); - VertexScale* VS = new VertexScale(scale); - VS->setId(maxKFid*2+5); + VertexScale *VS = new VertexScale(scale); + VS->setId(maxKFid * 2 + 5); VS->setFixed(!bMono); // Fixed for stereo case optimizer.addVertex(VS); // Graph edges // IMU links with gravity and scale - vector vpei; + vector vpei; vpei.reserve(vpKFs.size()); - vector > vppUsedKF; + vector> vppUsedKF; vppUsedKF.reserve(vpKFs.size()); - //std::cout << "build optimization graph" << std::endl; + // std::cout << "build optimization graph" << std::endl; - for(size_t i=0;imPrevKF && pKFi->mnId<=maxKFid) + if (pKFi->mPrevKF && pKFi->mnId <= maxKFid) { - if(pKFi->isBad() || pKFi->mPrevKF->mnId>maxKFid) + if (pKFi->isBad() || pKFi->mPrevKF->mnId > maxKFid) continue; - if(!pKFi->mpImuPreintegrated) + if (!pKFi->mpImuPreintegrated) std::cout << "Not preintegrated measurement" << std::endl; pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias()); - g2o::HyperGraph::Vertex* VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); - g2o::HyperGraph::Vertex* VV1 = optimizer.vertex(maxKFid+(pKFi->mPrevKF->mnId)+1); - g2o::HyperGraph::Vertex* VP2 = optimizer.vertex(pKFi->mnId); - g2o::HyperGraph::Vertex* VV2 = optimizer.vertex(maxKFid+(pKFi->mnId)+1); - g2o::HyperGraph::Vertex* VG = optimizer.vertex(maxKFid*2+2); - g2o::HyperGraph::Vertex* VA = optimizer.vertex(maxKFid*2+3); - g2o::HyperGraph::Vertex* VGDir = optimizer.vertex(maxKFid*2+4); - g2o::HyperGraph::Vertex* VS = optimizer.vertex(maxKFid*2+5); - if(!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS) + g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); + g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + (pKFi->mPrevKF->mnId) + 1); + g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId); + g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + (pKFi->mnId) + 1); + g2o::HyperGraph::Vertex *VG = optimizer.vertex(maxKFid * 2 + 2); + g2o::HyperGraph::Vertex *VA = optimizer.vertex(maxKFid * 2 + 3); + g2o::HyperGraph::Vertex *VGDir = optimizer.vertex(maxKFid * 2 + 4); + g2o::HyperGraph::Vertex *VS = optimizer.vertex(maxKFid * 2 + 5); + if (!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS) { - cout << "Error" << VP1 << ", "<< VV1 << ", "<< VG << ", "<< VA << ", " << VP2 << ", " << VV2 << ", "<< VGDir << ", "<< VS <mpImuPreintegrated); - ei->setVertex(0,dynamic_cast(VP1)); - ei->setVertex(1,dynamic_cast(VV1)); - ei->setVertex(2,dynamic_cast(VG)); - ei->setVertex(3,dynamic_cast(VA)); - ei->setVertex(4,dynamic_cast(VP2)); - ei->setVertex(5,dynamic_cast(VV2)); - ei->setVertex(6,dynamic_cast(VGDir)); - ei->setVertex(7,dynamic_cast(VS)); + EdgeInertialGS *ei = new EdgeInertialGS(pKFi->mpImuPreintegrated); + ei->setVertex(0, dynamic_cast(VP1)); + ei->setVertex(1, dynamic_cast(VV1)); + ei->setVertex(2, dynamic_cast(VG)); + ei->setVertex(3, dynamic_cast(VA)); + ei->setVertex(4, dynamic_cast(VP2)); + ei->setVertex(5, dynamic_cast(VV2)); + ei->setVertex(6, dynamic_cast(VGDir)); + ei->setVertex(7, dynamic_cast(VS)); vpei.push_back(ei); - vppUsedKF.push_back(make_pair(pKFi->mPrevKF,pKFi)); + vppUsedKF.push_back(make_pair(pKFi->mPrevKF, pKFi)); optimizer.addEdge(ei); - } } // Compute error for different scales - std::set setEdges = optimizer.edges(); + std::set setEdges = optimizer.edges(); optimizer.setVerbose(false); optimizer.initializeOptimization(); @@ -3187,27 +3108,26 @@ void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &sc // Recover optimized data // Biases - VG = static_cast(optimizer.vertex(maxKFid*2+2)); - VA = static_cast(optimizer.vertex(maxKFid*2+3)); + VG = static_cast(optimizer.vertex(maxKFid * 2 + 2)); + VA = static_cast(optimizer.vertex(maxKFid * 2 + 3)); Vector6d vb; vb << VG->estimate(), VA->estimate(); bg << VG->estimate(); ba << VA->estimate(); scale = VS->estimate(); - - IMU::Bias b (vb[3],vb[4],vb[5],vb[0],vb[1],vb[2]); + IMU::Bias b(vb[3], vb[4], vb[5], vb[0], vb[1], vb[2]); Rwg = VGDir->estimate().Rwg; - //Keyframes velocities and biases + // Keyframes velocities and biases const int N = vpKFs.size(); - for(size_t i=0; imnId>maxKFid) + KeyFrame *pKFi = vpKFs[i]; + if (pKFi->mnId > maxKFid) continue; - VertexVelocity* VV = static_cast(optimizer.vertex(maxKFid+(pKFi->mnId)+1)); + VertexVelocity *VV = static_cast(optimizer.vertex(maxKFid + (pKFi->mnId) + 1)); Eigen::Vector3d Vw = VV->estimate(); // Velocity is scaled after pKFi->SetVelocity(Vw.cast()); @@ -3219,57 +3139,54 @@ void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &sc } else pKFi->SetNewBias(b); - - } } - void Optimizer::InertialOptimization(Map *pMap, Eigen::Vector3d &bg, Eigen::Vector3d &ba, float priorG, float priorA) { int its = 200; // Check number of iterations long unsigned int maxKFid = pMap->GetMaxKFid(); - const vector vpKFs = pMap->GetAllKeyFrames(); + const vector vpKFs = pMap->GetAllKeyFrames(); // Setup optimizer g2o::SparseOptimizer optimizer; - g2o::BlockSolverX::LinearSolverType * linearSolver; + g2o::BlockSolverX::LinearSolverType *linearSolver; linearSolver = new g2o::LinearSolverEigen(); - g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); solver->setUserLambdaInit(1e3); optimizer.setAlgorithm(solver); // Set KeyFrame vertices (fixed poses and optimizable velocities) - for(size_t i=0; imnId>maxKFid) + KeyFrame *pKFi = vpKFs[i]; + if (pKFi->mnId > maxKFid) continue; - VertexPose * VP = new VertexPose(pKFi); + VertexPose *VP = new VertexPose(pKFi); VP->setId(pKFi->mnId); VP->setFixed(true); optimizer.addVertex(VP); - VertexVelocity* VV = new VertexVelocity(pKFi); - VV->setId(maxKFid+(pKFi->mnId)+1); + VertexVelocity *VV = new VertexVelocity(pKFi); + VV->setId(maxKFid + (pKFi->mnId) + 1); VV->setFixed(false); optimizer.addVertex(VV); } // Biases - VertexGyroBias* VG = new VertexGyroBias(vpKFs.front()); - VG->setId(maxKFid*2+2); + VertexGyroBias *VG = new VertexGyroBias(vpKFs.front()); + VG->setId(maxKFid * 2 + 2); VG->setFixed(false); optimizer.addVertex(VG); - VertexAccBias* VA = new VertexAccBias(vpKFs.front()); - VA->setId(maxKFid*2+3); + VertexAccBias *VA = new VertexAccBias(vpKFs.front()); + VA->setId(maxKFid * 2 + 3); VA->setFixed(false); optimizer.addVertex(VA); @@ -3277,73 +3194,72 @@ void Optimizer::InertialOptimization(Map *pMap, Eigen::Vector3d &bg, Eigen::Vect Eigen::Vector3f bprior; bprior.setZero(); - EdgePriorAcc* epa = new EdgePriorAcc(bprior); - epa->setVertex(0,dynamic_cast(VA)); + EdgePriorAcc *epa = new EdgePriorAcc(bprior); + epa->setVertex(0, dynamic_cast(VA)); double infoPriorA = priorA; - epa->setInformation(infoPriorA*Eigen::Matrix3d::Identity()); + epa->setInformation(infoPriorA * Eigen::Matrix3d::Identity()); optimizer.addEdge(epa); - EdgePriorGyro* epg = new EdgePriorGyro(bprior); - epg->setVertex(0,dynamic_cast(VG)); + EdgePriorGyro *epg = new EdgePriorGyro(bprior); + epg->setVertex(0, dynamic_cast(VG)); double infoPriorG = priorG; - epg->setInformation(infoPriorG*Eigen::Matrix3d::Identity()); + epg->setInformation(infoPriorG * Eigen::Matrix3d::Identity()); optimizer.addEdge(epg); // Gravity and scale - VertexGDir* VGDir = new VertexGDir(Eigen::Matrix3d::Identity()); - VGDir->setId(maxKFid*2+4); + VertexGDir *VGDir = new VertexGDir(Eigen::Matrix3d::Identity()); + VGDir->setId(maxKFid * 2 + 4); VGDir->setFixed(true); optimizer.addVertex(VGDir); - VertexScale* VS = new VertexScale(1.0); - VS->setId(maxKFid*2+5); + VertexScale *VS = new VertexScale(1.0); + VS->setId(maxKFid * 2 + 5); VS->setFixed(true); // Fixed since scale is obtained from already well initialized map optimizer.addVertex(VS); // Graph edges // IMU links with gravity and scale - vector vpei; + vector vpei; vpei.reserve(vpKFs.size()); - vector > vppUsedKF; + vector> vppUsedKF; vppUsedKF.reserve(vpKFs.size()); - for(size_t i=0;imPrevKF && pKFi->mnId<=maxKFid) + if (pKFi->mPrevKF && pKFi->mnId <= maxKFid) { - if(pKFi->isBad() || pKFi->mPrevKF->mnId>maxKFid) + if (pKFi->isBad() || pKFi->mPrevKF->mnId > maxKFid) continue; pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias()); - g2o::HyperGraph::Vertex* VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); - g2o::HyperGraph::Vertex* VV1 = optimizer.vertex(maxKFid+(pKFi->mPrevKF->mnId)+1); - g2o::HyperGraph::Vertex* VP2 = optimizer.vertex(pKFi->mnId); - g2o::HyperGraph::Vertex* VV2 = optimizer.vertex(maxKFid+(pKFi->mnId)+1); - g2o::HyperGraph::Vertex* VG = optimizer.vertex(maxKFid*2+2); - g2o::HyperGraph::Vertex* VA = optimizer.vertex(maxKFid*2+3); - g2o::HyperGraph::Vertex* VGDir = optimizer.vertex(maxKFid*2+4); - g2o::HyperGraph::Vertex* VS = optimizer.vertex(maxKFid*2+5); - if(!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS) + g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); + g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + (pKFi->mPrevKF->mnId) + 1); + g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId); + g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + (pKFi->mnId) + 1); + g2o::HyperGraph::Vertex *VG = optimizer.vertex(maxKFid * 2 + 2); + g2o::HyperGraph::Vertex *VA = optimizer.vertex(maxKFid * 2 + 3); + g2o::HyperGraph::Vertex *VGDir = optimizer.vertex(maxKFid * 2 + 4); + g2o::HyperGraph::Vertex *VS = optimizer.vertex(maxKFid * 2 + 5); + if (!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS) { - cout << "Error" << VP1 << ", "<< VV1 << ", "<< VG << ", "<< VA << ", " << VP2 << ", " << VV2 << ", "<< VGDir << ", "<< VS <mpImuPreintegrated); - ei->setVertex(0,dynamic_cast(VP1)); - ei->setVertex(1,dynamic_cast(VV1)); - ei->setVertex(2,dynamic_cast(VG)); - ei->setVertex(3,dynamic_cast(VA)); - ei->setVertex(4,dynamic_cast(VP2)); - ei->setVertex(5,dynamic_cast(VV2)); - ei->setVertex(6,dynamic_cast(VGDir)); - ei->setVertex(7,dynamic_cast(VS)); + EdgeInertialGS *ei = new EdgeInertialGS(pKFi->mpImuPreintegrated); + ei->setVertex(0, dynamic_cast(VP1)); + ei->setVertex(1, dynamic_cast(VV1)); + ei->setVertex(2, dynamic_cast(VG)); + ei->setVertex(3, dynamic_cast(VA)); + ei->setVertex(4, dynamic_cast(VP2)); + ei->setVertex(5, dynamic_cast(VV2)); + ei->setVertex(6, dynamic_cast(VGDir)); + ei->setVertex(7, dynamic_cast(VS)); vpei.push_back(ei); - vppUsedKF.push_back(make_pair(pKFi->mPrevKF,pKFi)); + vppUsedKF.push_back(make_pair(pKFi->mPrevKF, pKFi)); optimizer.addEdge(ei); - } } @@ -3352,27 +3268,26 @@ void Optimizer::InertialOptimization(Map *pMap, Eigen::Vector3d &bg, Eigen::Vect optimizer.initializeOptimization(); optimizer.optimize(its); - // Recover optimized data // Biases - VG = static_cast(optimizer.vertex(maxKFid*2+2)); - VA = static_cast(optimizer.vertex(maxKFid*2+3)); + VG = static_cast(optimizer.vertex(maxKFid * 2 + 2)); + VA = static_cast(optimizer.vertex(maxKFid * 2 + 3)); Vector6d vb; vb << VG->estimate(), VA->estimate(); bg << VG->estimate(); ba << VA->estimate(); - IMU::Bias b (vb[3],vb[4],vb[5],vb[0],vb[1],vb[2]); + IMU::Bias b(vb[3], vb[4], vb[5], vb[0], vb[1], vb[2]); - //Keyframes velocities and biases + // Keyframes velocities and biases const int N = vpKFs.size(); - for(size_t i=0; imnId>maxKFid) + KeyFrame *pKFi = vpKFs[i]; + if (pKFi->mnId > maxKFid) continue; - VertexVelocity* VV = static_cast(optimizer.vertex(maxKFid+(pKFi->mnId)+1)); + VertexVelocity *VV = static_cast(optimizer.vertex(maxKFid + (pKFi->mnId) + 1)); Eigen::Vector3d Vw = VV->estimate(); pKFi->SetVelocity(Vw.cast()); @@ -3391,92 +3306,92 @@ void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &sc { int its = 10; long unsigned int maxKFid = pMap->GetMaxKFid(); - const vector vpKFs = pMap->GetAllKeyFrames(); + const vector vpKFs = pMap->GetAllKeyFrames(); // Setup optimizer g2o::SparseOptimizer optimizer; - g2o::BlockSolverX::LinearSolverType * linearSolver; + g2o::BlockSolverX::LinearSolverType *linearSolver; linearSolver = new g2o::LinearSolverEigen(); - g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); - g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr); + g2o::OptimizationAlgorithmGaussNewton *solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr); optimizer.setAlgorithm(solver); // Set KeyFrame vertices (all variables are fixed) - for(size_t i=0; imnId>maxKFid) + KeyFrame *pKFi = vpKFs[i]; + if (pKFi->mnId > maxKFid) continue; - VertexPose * VP = new VertexPose(pKFi); + VertexPose *VP = new VertexPose(pKFi); VP->setId(pKFi->mnId); VP->setFixed(true); optimizer.addVertex(VP); - VertexVelocity* VV = new VertexVelocity(pKFi); - VV->setId(maxKFid+1+(pKFi->mnId)); + VertexVelocity *VV = new VertexVelocity(pKFi); + VV->setId(maxKFid + 1 + (pKFi->mnId)); VV->setFixed(true); optimizer.addVertex(VV); // Vertex of fixed biases - VertexGyroBias* VG = new VertexGyroBias(vpKFs.front()); - VG->setId(2*(maxKFid+1)+(pKFi->mnId)); + VertexGyroBias *VG = new VertexGyroBias(vpKFs.front()); + VG->setId(2 * (maxKFid + 1) + (pKFi->mnId)); VG->setFixed(true); optimizer.addVertex(VG); - VertexAccBias* VA = new VertexAccBias(vpKFs.front()); - VA->setId(3*(maxKFid+1)+(pKFi->mnId)); + VertexAccBias *VA = new VertexAccBias(vpKFs.front()); + VA->setId(3 * (maxKFid + 1) + (pKFi->mnId)); VA->setFixed(true); optimizer.addVertex(VA); } // Gravity and scale - VertexGDir* VGDir = new VertexGDir(Rwg); - VGDir->setId(4*(maxKFid+1)); + VertexGDir *VGDir = new VertexGDir(Rwg); + VGDir->setId(4 * (maxKFid + 1)); VGDir->setFixed(false); optimizer.addVertex(VGDir); - VertexScale* VS = new VertexScale(scale); - VS->setId(4*(maxKFid+1)+1); + VertexScale *VS = new VertexScale(scale); + VS->setId(4 * (maxKFid + 1) + 1); VS->setFixed(false); optimizer.addVertex(VS); // Graph edges int count_edges = 0; - for(size_t i=0;imPrevKF && pKFi->mnId<=maxKFid) + if (pKFi->mPrevKF && pKFi->mnId <= maxKFid) { - if(pKFi->isBad() || pKFi->mPrevKF->mnId>maxKFid) + if (pKFi->isBad() || pKFi->mPrevKF->mnId > maxKFid) continue; - - g2o::HyperGraph::Vertex* VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); - g2o::HyperGraph::Vertex* VV1 = optimizer.vertex((maxKFid+1)+pKFi->mPrevKF->mnId); - g2o::HyperGraph::Vertex* VP2 = optimizer.vertex(pKFi->mnId); - g2o::HyperGraph::Vertex* VV2 = optimizer.vertex((maxKFid+1)+pKFi->mnId); - g2o::HyperGraph::Vertex* VG = optimizer.vertex(2*(maxKFid+1)+pKFi->mPrevKF->mnId); - g2o::HyperGraph::Vertex* VA = optimizer.vertex(3*(maxKFid+1)+pKFi->mPrevKF->mnId); - g2o::HyperGraph::Vertex* VGDir = optimizer.vertex(4*(maxKFid+1)); - g2o::HyperGraph::Vertex* VS = optimizer.vertex(4*(maxKFid+1)+1); - if(!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS) + + g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); + g2o::HyperGraph::Vertex *VV1 = optimizer.vertex((maxKFid + 1) + pKFi->mPrevKF->mnId); + g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId); + g2o::HyperGraph::Vertex *VV2 = optimizer.vertex((maxKFid + 1) + pKFi->mnId); + g2o::HyperGraph::Vertex *VG = optimizer.vertex(2 * (maxKFid + 1) + pKFi->mPrevKF->mnId); + g2o::HyperGraph::Vertex *VA = optimizer.vertex(3 * (maxKFid + 1) + pKFi->mPrevKF->mnId); + g2o::HyperGraph::Vertex *VGDir = optimizer.vertex(4 * (maxKFid + 1)); + g2o::HyperGraph::Vertex *VS = optimizer.vertex(4 * (maxKFid + 1) + 1); + if (!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS) { - Verbose::PrintMess("Error" + to_string(VP1->id()) + ", " + to_string(VV1->id()) + ", " + to_string(VG->id()) + ", " + to_string(VA->id()) + ", " + to_string(VP2->id()) + ", " + to_string(VV2->id()) + ", " + to_string(VGDir->id()) + ", " + to_string(VS->id()), Verbose::VERBOSITY_NORMAL); + Verbose::PrintMess("Error" + to_string(VP1->id()) + ", " + to_string(VV1->id()) + ", " + to_string(VG->id()) + ", " + to_string(VA->id()) + ", " + to_string(VP2->id()) + ", " + to_string(VV2->id()) + ", " + to_string(VGDir->id()) + ", " + to_string(VS->id()), Verbose::VERBOSITY_NORMAL); continue; } count_edges++; - EdgeInertialGS* ei = new EdgeInertialGS(pKFi->mpImuPreintegrated); - ei->setVertex(0,dynamic_cast(VP1)); - ei->setVertex(1,dynamic_cast(VV1)); - ei->setVertex(2,dynamic_cast(VG)); - ei->setVertex(3,dynamic_cast(VA)); - ei->setVertex(4,dynamic_cast(VP2)); - ei->setVertex(5,dynamic_cast(VV2)); - ei->setVertex(6,dynamic_cast(VGDir)); - ei->setVertex(7,dynamic_cast(VS)); - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; + EdgeInertialGS *ei = new EdgeInertialGS(pKFi->mpImuPreintegrated); + ei->setVertex(0, dynamic_cast(VP1)); + ei->setVertex(1, dynamic_cast(VV1)); + ei->setVertex(2, dynamic_cast(VG)); + ei->setVertex(3, dynamic_cast(VA)); + ei->setVertex(4, dynamic_cast(VP2)); + ei->setVertex(5, dynamic_cast(VV2)); + ei->setVertex(6, dynamic_cast(VGDir)); + ei->setVertex(7, dynamic_cast(VS)); + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; ei->setRobustKernel(rk); rk->setDelta(1.f); optimizer.addEdge(ei); @@ -3496,37 +3411,898 @@ void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &sc Rwg = VGDir->estimate().Rwg; } -void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdjustKF, vector vpFixedKF, bool *pbStopFlag) +/**************************************以下为sim3优化**************************************************************/ + +int Optimizer::OptimizeSim3( + KeyFrame *pKF1, KeyFrame *pKF2, vector &vpMatches1, g2o::Sim3 &g2oS12, const float th2, + const bool bFixScale, Eigen::Matrix &mAcumHessian, const bool bAllPoints) +{ + g2o::SparseOptimizer optimizer; + g2o::BlockSolverX::LinearSolverType *linearSolver; + + linearSolver = new g2o::LinearSolverDense(); + + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); + + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + optimizer.setAlgorithm(solver); + + // Camera poses + const Eigen::Matrix3f R1w = pKF1->GetRotation(); + const Eigen::Vector3f t1w = pKF1->GetTranslation(); + const Eigen::Matrix3f R2w = pKF2->GetRotation(); + const Eigen::Vector3f t2w = pKF2->GetTranslation(); + + // Set Sim3 vertex + ORB_SLAM3::VertexSim3Expmap *vSim3 = new ORB_SLAM3::VertexSim3Expmap(); + vSim3->_fix_scale = bFixScale; + vSim3->setEstimate(g2oS12); + vSim3->setId(0); + vSim3->setFixed(false); + vSim3->pCamera1 = pKF1->mpCamera; + vSim3->pCamera2 = pKF2->mpCamera; + optimizer.addVertex(vSim3); + + // Set MapPoint vertices + const int N = vpMatches1.size(); + const vector vpMapPoints1 = pKF1->GetMapPointMatches(); + vector vpEdges12; + vector vpEdges21; + vector vnIndexEdge; + vector vbIsInKF2; + + vnIndexEdge.reserve(2 * N); + vpEdges12.reserve(2 * N); + vpEdges21.reserve(2 * N); + vbIsInKF2.reserve(2 * N); + + const float deltaHuber = sqrt(th2); + + int nCorrespondences = 0; + int nBadMPs = 0; + int nInKF2 = 0; + int nOutKF2 = 0; + int nMatchWithoutMP = 0; + + vector vIdsOnlyInKF2; + + 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 = get<0>(pMP2->GetIndexInKeyFrame(pKF2)); + + Eigen::Vector3f P3D1c; + Eigen::Vector3f P3D2c; + + if (pMP1 && pMP2) + { + if (!pMP1->isBad() && !pMP2->isBad()) + { + g2o::VertexSBAPointXYZ *vPoint1 = new g2o::VertexSBAPointXYZ(); + Eigen::Vector3f P3D1w = pMP1->GetWorldPos(); + P3D1c = R1w * P3D1w + t1w; + vPoint1->setEstimate(P3D1c.cast()); + vPoint1->setId(id1); + vPoint1->setFixed(true); + optimizer.addVertex(vPoint1); + + g2o::VertexSBAPointXYZ *vPoint2 = new g2o::VertexSBAPointXYZ(); + Eigen::Vector3f P3D2w = pMP2->GetWorldPos(); + P3D2c = R2w * P3D2w + t2w; + vPoint2->setEstimate(P3D2c.cast()); + vPoint2->setId(id2); + vPoint2->setFixed(true); + optimizer.addVertex(vPoint2); + } + else + { + nBadMPs++; + continue; + } + } + else + { + nMatchWithoutMP++; + + // TODO The 3D position in KF1 doesn't exist + if (!pMP2->isBad()) + { + g2o::VertexSBAPointXYZ *vPoint2 = new g2o::VertexSBAPointXYZ(); + Eigen::Vector3f P3D2w = pMP2->GetWorldPos(); + P3D2c = R2w * P3D2w + t2w; + vPoint2->setEstimate(P3D2c.cast()); + vPoint2->setId(id2); + vPoint2->setFixed(true); + optimizer.addVertex(vPoint2); + + vIdsOnlyInKF2.push_back(id2); + } + continue; + } + + if (i2 < 0 && !bAllPoints) + { + Verbose::PrintMess(" Remove point -> i2: " + to_string(i2) + "; bAllPoints: " + to_string(bAllPoints), Verbose::VERBOSITY_DEBUG); + continue; + } + + if (P3D2c(2) < 0) + { + Verbose::PrintMess("Sim3: Z coordinate is negative", Verbose::VERBOSITY_DEBUG); + continue; + } + + nCorrespondences++; + + // Set edge x1 = S12*X2 + Eigen::Matrix obs1; + const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i]; + obs1 << kpUn1.pt.x, kpUn1.pt.y; + + ORB_SLAM3::EdgeSim3ProjectXYZ *e12 = new ORB_SLAM3::EdgeSim3ProjectXYZ(); + + e12->setVertex(0, dynamic_cast(optimizer.vertex(id2))); + e12->setVertex(1, dynamic_cast(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 obs2; + cv::KeyPoint kpUn2; + bool inKF2; + if (i2 >= 0) + { + kpUn2 = pKF2->mvKeysUn[i2]; + obs2 << kpUn2.pt.x, kpUn2.pt.y; + inKF2 = true; + + nInKF2++; + } + else + { + float invz = 1 / P3D2c(2); + float x = P3D2c(0) * invz; + float y = P3D2c(1) * invz; + + obs2 << x, y; + kpUn2 = cv::KeyPoint(cv::Point2f(x, y), pMP2->mnTrackScaleLevel); + + inKF2 = false; + nOutKF2++; + } + + ORB_SLAM3::EdgeInverseSim3ProjectXYZ *e21 = new ORB_SLAM3::EdgeInverseSim3ProjectXYZ(); + + e21->setVertex(0, dynamic_cast(optimizer.vertex(id1))); + e21->setVertex(1, dynamic_cast(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); + + vbIsInKF2.push_back(inKF2); + } + + // Optimize! + optimizer.initializeOptimization(); + optimizer.optimize(5); + + // Check inliers + int nBad = 0; + int nBadOutKF2 = 0; + for (size_t i = 0; i < vpEdges12.size(); i++) + { + ORB_SLAM3::EdgeSim3ProjectXYZ *e12 = vpEdges12[i]; + ORB_SLAM3::EdgeInverseSim3ProjectXYZ *e21 = vpEdges21[i]; + if (!e12 || !e21) + continue; + + if (e12->chi2() > th2 || e21->chi2() > th2) + { + size_t idx = vnIndexEdge[i]; + vpMatches1[idx] = static_cast(NULL); + optimizer.removeEdge(e12); + optimizer.removeEdge(e21); + vpEdges12[i] = static_cast(NULL); + vpEdges21[i] = static_cast(NULL); + nBad++; + + if (!vbIsInKF2[i]) + { + nBadOutKF2++; + } + continue; + } + + // Check if remove the robust adjustment improve the result + e12->setRobustKernel(0); + e21->setRobustKernel(0); + } + + 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; + mAcumHessian = Eigen::MatrixXd::Zero(7, 7); + for (size_t i = 0; i < vpEdges12.size(); i++) + { + ORB_SLAM3::EdgeSim3ProjectXYZ *e12 = vpEdges12[i]; + ORB_SLAM3::EdgeInverseSim3ProjectXYZ *e21 = vpEdges21[i]; + if (!e12 || !e21) + continue; + + e12->computeError(); + e21->computeError(); + + if (e12->chi2() > th2 || e21->chi2() > th2) + { + size_t idx = vnIndexEdge[i]; + vpMatches1[idx] = static_cast(NULL); + } + else + { + nIn++; + } + } + + // Recover optimized Sim3 + g2o::VertexSim3Expmap *vSim3_recov = static_cast(optimizer.vertex(0)); + g2oS12 = vSim3_recov->estimate(); + + return nIn; +} + +/**************************************以下为地图回环优化**************************************************************/ + +void Optimizer::OptimizeEssentialGraph( + Map *pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF, + const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, + const LoopClosing::KeyFrameAndPose &CorrectedSim3, + const map> &LoopConnections, const bool &bFixScale) +{ + // Setup optimizer + g2o::SparseOptimizer optimizer; + optimizer.setVerbose(false); + g2o::BlockSolver_7_3::LinearSolverType *linearSolver = + new g2o::LinearSolverEigen(); + g2o::BlockSolver_7_3 *solver_ptr = new g2o::BlockSolver_7_3(linearSolver); + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + + solver->setUserLambdaInit(1e-16); + optimizer.setAlgorithm(solver); + + const vector vpKFs = pMap->GetAllKeyFrames(); + const vector vpMPs = pMap->GetAllMapPoints(); + + const unsigned int nMaxKFid = pMap->GetMaxKFid(); + + vector> vScw(nMaxKFid + 1); + vector> vCorrectedSwc(nMaxKFid + 1); + vector vpVertices(nMaxKFid + 1); + + vector vZvectors(nMaxKFid + 1); // For debugging + Eigen::Vector3d z_vec; + z_vec << 0.0, 0.0, 1.0; + + const int minFeat = 100; + + // Set KeyFrame vertices + for (size_t i = 0, iend = vpKFs.size(); i < iend; i++) + { + KeyFrame *pKF = vpKFs[i]; + if (pKF->isBad()) + continue; + g2o::VertexSim3Expmap *VSim3 = new g2o::VertexSim3Expmap(); + + const int nIDi = pKF->mnId; + + LoopClosing::KeyFrameAndPose::const_iterator it = CorrectedSim3.find(pKF); + + if (it != CorrectedSim3.end()) + { + vScw[nIDi] = it->second; + VSim3->setEstimate(it->second); + } + else + { + Sophus::SE3d Tcw = pKF->GetPose().cast(); + g2o::Sim3 Siw(Tcw.unit_quaternion(), Tcw.translation(), 1.0); + vScw[nIDi] = Siw; + VSim3->setEstimate(Siw); + } + + if (pKF->mnId == pMap->GetInitKFid()) + VSim3->setFixed(true); + + VSim3->setId(nIDi); + VSim3->setMarginalized(false); + VSim3->_fix_scale = bFixScale; + + optimizer.addVertex(VSim3); + vZvectors[nIDi] = vScw[nIDi].rotation() * z_vec; // For debugging + + vpVertices[nIDi] = VSim3; + } + + set> sInsertedEdges; + + const Eigen::Matrix matLambda = Eigen::Matrix::Identity(); + + // Set Loop edges + int count_loop = 0; + for (map>::const_iterator mit = LoopConnections.begin(), mend = LoopConnections.end(); mit != mend; mit++) + { + KeyFrame *pKF = mit->first; + const long unsigned int nIDi = pKF->mnId; + const set &spConnections = mit->second; + const g2o::Sim3 Siw = vScw[nIDi]; + const g2o::Sim3 Swi = Siw.inverse(); + + for (set::const_iterator sit = spConnections.begin(), send = spConnections.end(); sit != send; sit++) + { + const long unsigned int nIDj = (*sit)->mnId; + if ((nIDi != pCurKF->mnId || nIDj != pLoopKF->mnId) && pKF->GetWeight(*sit) < minFeat) + continue; + + const g2o::Sim3 Sjw = vScw[nIDj]; + const g2o::Sim3 Sji = Sjw * Swi; + + g2o::EdgeSim3 *e = new g2o::EdgeSim3(); + e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); + e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + e->setMeasurement(Sji); + + e->information() = matLambda; + + optimizer.addEdge(e); + count_loop++; + sInsertedEdges.insert(make_pair(min(nIDi, nIDj), max(nIDi, nIDj))); + } + } + + // Set normal edges + for (size_t i = 0, iend = vpKFs.size(); i < iend; i++) + { + KeyFrame *pKF = vpKFs[i]; + + const int nIDi = pKF->mnId; + + g2o::Sim3 Swi; + + LoopClosing::KeyFrameAndPose::const_iterator iti = NonCorrectedSim3.find(pKF); + + if (iti != NonCorrectedSim3.end()) + Swi = (iti->second).inverse(); + else + Swi = vScw[nIDi].inverse(); + + KeyFrame *pParentKF = pKF->GetParent(); + + // Spanning tree edge + if (pParentKF) + { + int nIDj = pParentKF->mnId; + + g2o::Sim3 Sjw; + + LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF); + + if (itj != NonCorrectedSim3.end()) + Sjw = itj->second; + else + Sjw = vScw[nIDj]; + + g2o::Sim3 Sji = Sjw * Swi; + + g2o::EdgeSim3 *e = new g2o::EdgeSim3(); + e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); + e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + e->setMeasurement(Sji); + e->information() = matLambda; + optimizer.addEdge(e); + } + + // Loop edges + const set sLoopEdges = pKF->GetLoopEdges(); + for (set::const_iterator sit = sLoopEdges.begin(), send = sLoopEdges.end(); sit != send; sit++) + { + KeyFrame *pLKF = *sit; + if (pLKF->mnId < pKF->mnId) + { + g2o::Sim3 Slw; + + LoopClosing::KeyFrameAndPose::const_iterator itl = NonCorrectedSim3.find(pLKF); + + if (itl != NonCorrectedSim3.end()) + Slw = itl->second; + else + Slw = vScw[pLKF->mnId]; + + g2o::Sim3 Sli = Slw * Swi; + g2o::EdgeSim3 *el = new g2o::EdgeSim3(); + el->setVertex(1, dynamic_cast(optimizer.vertex(pLKF->mnId))); + el->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + el->setMeasurement(Sli); + el->information() = matLambda; + optimizer.addEdge(el); + } + } + + // Covisibility graph edges + const vector vpConnectedKFs = pKF->GetCovisiblesByWeight(minFeat); + for (vector::const_iterator vit = vpConnectedKFs.begin(); vit != vpConnectedKFs.end(); vit++) + { + KeyFrame *pKFn = *vit; + if (pKFn && pKFn != pParentKF && !pKF->hasChild(pKFn) /*&& !sLoopEdges.count(pKFn)*/) + { + if (!pKFn->isBad() && pKFn->mnId < pKF->mnId) + { + if (sInsertedEdges.count(make_pair(min(pKF->mnId, pKFn->mnId), max(pKF->mnId, pKFn->mnId)))) + continue; + + g2o::Sim3 Snw; + + LoopClosing::KeyFrameAndPose::const_iterator itn = NonCorrectedSim3.find(pKFn); + + if (itn != NonCorrectedSim3.end()) + Snw = itn->second; + else + Snw = vScw[pKFn->mnId]; + + g2o::Sim3 Sni = Snw * Swi; + + g2o::EdgeSim3 *en = new g2o::EdgeSim3(); + en->setVertex(1, dynamic_cast(optimizer.vertex(pKFn->mnId))); + en->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + en->setMeasurement(Sni); + en->information() = matLambda; + optimizer.addEdge(en); + } + } + } + + // Inertial edges if inertial + if (pKF->bImu && pKF->mPrevKF) + { + g2o::Sim3 Spw; + LoopClosing::KeyFrameAndPose::const_iterator itp = NonCorrectedSim3.find(pKF->mPrevKF); + if (itp != NonCorrectedSim3.end()) + Spw = itp->second; + else + Spw = vScw[pKF->mPrevKF->mnId]; + + g2o::Sim3 Spi = Spw * Swi; + g2o::EdgeSim3 *ep = new g2o::EdgeSim3(); + ep->setVertex(1, dynamic_cast(optimizer.vertex(pKF->mPrevKF->mnId))); + ep->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + ep->setMeasurement(Spi); + ep->information() = matLambda; + optimizer.addEdge(ep); + } + } + + optimizer.initializeOptimization(); + optimizer.computeActiveErrors(); + optimizer.optimize(20); + optimizer.computeActiveErrors(); + unique_lock lock(pMap->mMutexMapUpdate); + + // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1] + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKFi = vpKFs[i]; + + const int nIDi = pKFi->mnId; + + g2o::VertexSim3Expmap *VSim3 = static_cast(optimizer.vertex(nIDi)); + g2o::Sim3 CorrectedSiw = VSim3->estimate(); + vCorrectedSwc[nIDi] = CorrectedSiw.inverse(); + double s = CorrectedSiw.scale(); + + Sophus::SE3f Tiw(CorrectedSiw.rotation().cast(), CorrectedSiw.translation().cast() / s); + pKFi->SetPose(Tiw); + } + + // Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose + for (size_t i = 0, iend = vpMPs.size(); i < iend; i++) + { + MapPoint *pMP = vpMPs[i]; + + if (pMP->isBad()) + continue; + + int nIDr; + if (pMP->mnCorrectedByKF == pCurKF->mnId) + { + nIDr = pMP->mnCorrectedReference; + } + else + { + KeyFrame *pRefKF = pMP->GetReferenceKeyFrame(); + nIDr = pRefKF->mnId; + } + + g2o::Sim3 Srw = vScw[nIDr]; + g2o::Sim3 correctedSwr = vCorrectedSwc[nIDr]; + + Eigen::Matrix eigP3Dw = pMP->GetWorldPos().cast(); + Eigen::Matrix eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw)); + pMP->SetWorldPos(eigCorrectedP3Dw.cast()); + + pMP->UpdateNormalAndDepth(); + } + + // TODO Check this changeindex + pMap->IncreaseChangeIndex(); +} + +void Optimizer::OptimizeEssentialGraph4DoF( + Map *pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF, + const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, + const LoopClosing::KeyFrameAndPose &CorrectedSim3, + const map> &LoopConnections) +{ + typedef g2o::BlockSolver> BlockSolver_4_4; + + // Setup optimizer + g2o::SparseOptimizer optimizer; + optimizer.setVerbose(false); + g2o::BlockSolverX::LinearSolverType *linearSolver = + new g2o::LinearSolverEigen(); + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); + + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + + optimizer.setAlgorithm(solver); + + const vector vpKFs = pMap->GetAllKeyFrames(); + const vector vpMPs = pMap->GetAllMapPoints(); + + const unsigned int nMaxKFid = pMap->GetMaxKFid(); + + vector> vScw(nMaxKFid + 1); + vector> vCorrectedSwc(nMaxKFid + 1); + + vector vpVertices(nMaxKFid + 1); + + const int minFeat = 100; + // Set KeyFrame vertices + for (size_t i = 0, iend = vpKFs.size(); i < iend; i++) + { + KeyFrame *pKF = vpKFs[i]; + if (pKF->isBad()) + continue; + + VertexPose4DoF *V4DoF; + + const int nIDi = pKF->mnId; + + LoopClosing::KeyFrameAndPose::const_iterator it = CorrectedSim3.find(pKF); + + if (it != CorrectedSim3.end()) + { + vScw[nIDi] = it->second; + const g2o::Sim3 Swc = it->second.inverse(); + Eigen::Matrix3d Rwc = Swc.rotation().toRotationMatrix(); + Eigen::Vector3d twc = Swc.translation(); + V4DoF = new VertexPose4DoF(Rwc, twc, pKF); + } + else + { + Sophus::SE3d Tcw = pKF->GetPose().cast(); + g2o::Sim3 Siw(Tcw.unit_quaternion(), Tcw.translation(), 1.0); + + vScw[nIDi] = Siw; + V4DoF = new VertexPose4DoF(pKF); + } + + if (pKF == pLoopKF) + V4DoF->setFixed(true); + + V4DoF->setId(nIDi); + V4DoF->setMarginalized(false); + + optimizer.addVertex(V4DoF); + vpVertices[nIDi] = V4DoF; + } + set> sInsertedEdges; + + // Edge used in posegraph has still 6Dof, even if updates of camera poses are just in 4DoF + Eigen::Matrix matLambda = Eigen::Matrix::Identity(); + matLambda(0, 0) = 1e3; + matLambda(1, 1) = 1e3; + matLambda(0, 0) = 1e3; + + // Set Loop edges + Edge4DoF *e_loop; + for (map>::const_iterator mit = LoopConnections.begin(), mend = LoopConnections.end(); mit != mend; mit++) + { + KeyFrame *pKF = mit->first; + const long unsigned int nIDi = pKF->mnId; + const set &spConnections = mit->second; + const g2o::Sim3 Siw = vScw[nIDi]; + + for (set::const_iterator sit = spConnections.begin(), send = spConnections.end(); sit != send; sit++) + { + const long unsigned int nIDj = (*sit)->mnId; + if ((nIDi != pCurKF->mnId || nIDj != pLoopKF->mnId) && pKF->GetWeight(*sit) < minFeat) + continue; + + const g2o::Sim3 Sjw = vScw[nIDj]; + const g2o::Sim3 Sij = Siw * Sjw.inverse(); + Eigen::Matrix4d Tij; + Tij.block<3, 3>(0, 0) = Sij.rotation().toRotationMatrix(); + Tij.block<3, 1>(0, 3) = Sij.translation(); + Tij(3, 3) = 1.; + + Edge4DoF *e = new Edge4DoF(Tij); + e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); + e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + + e->information() = matLambda; + e_loop = e; + optimizer.addEdge(e); + + sInsertedEdges.insert(make_pair(min(nIDi, nIDj), max(nIDi, nIDj))); + } + } + + // 1. Set normal edges + for (size_t i = 0, iend = vpKFs.size(); i < iend; i++) + { + KeyFrame *pKF = vpKFs[i]; + + const int nIDi = pKF->mnId; + + g2o::Sim3 Siw; + + // Use noncorrected poses for posegraph edges + LoopClosing::KeyFrameAndPose::const_iterator iti = NonCorrectedSim3.find(pKF); + + if (iti != NonCorrectedSim3.end()) + Siw = iti->second; + else + Siw = vScw[nIDi]; + + // 1.1.0 Spanning tree edge + KeyFrame *pParentKF = static_cast(NULL); + if (pParentKF) + { + int nIDj = pParentKF->mnId; + + g2o::Sim3 Swj; + + LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF); + + if (itj != NonCorrectedSim3.end()) + Swj = (itj->second).inverse(); + else + Swj = vScw[nIDj].inverse(); + + g2o::Sim3 Sij = Siw * Swj; + Eigen::Matrix4d Tij; + Tij.block<3, 3>(0, 0) = Sij.rotation().toRotationMatrix(); + Tij.block<3, 1>(0, 3) = Sij.translation(); + Tij(3, 3) = 1.; + + Edge4DoF *e = new Edge4DoF(Tij); + e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); + e->information() = matLambda; + optimizer.addEdge(e); + } + + // 1.1.1 Inertial edges + KeyFrame *prevKF = pKF->mPrevKF; + if (prevKF) + { + int nIDj = prevKF->mnId; + + g2o::Sim3 Swj; + + LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(prevKF); + + if (itj != NonCorrectedSim3.end()) + Swj = (itj->second).inverse(); + else + Swj = vScw[nIDj].inverse(); + + g2o::Sim3 Sij = Siw * Swj; + Eigen::Matrix4d Tij; + Tij.block<3, 3>(0, 0) = Sij.rotation().toRotationMatrix(); + Tij.block<3, 1>(0, 3) = Sij.translation(); + Tij(3, 3) = 1.; + + Edge4DoF *e = new Edge4DoF(Tij); + e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); + e->information() = matLambda; + optimizer.addEdge(e); + } + + // 1.2 Loop edges + const set sLoopEdges = pKF->GetLoopEdges(); + for (set::const_iterator sit = sLoopEdges.begin(), send = sLoopEdges.end(); sit != send; sit++) + { + KeyFrame *pLKF = *sit; + if (pLKF->mnId < pKF->mnId) + { + g2o::Sim3 Swl; + + LoopClosing::KeyFrameAndPose::const_iterator itl = NonCorrectedSim3.find(pLKF); + + if (itl != NonCorrectedSim3.end()) + Swl = itl->second.inverse(); + else + Swl = vScw[pLKF->mnId].inverse(); + + g2o::Sim3 Sil = Siw * Swl; + Eigen::Matrix4d Til; + Til.block<3, 3>(0, 0) = Sil.rotation().toRotationMatrix(); + Til.block<3, 1>(0, 3) = Sil.translation(); + Til(3, 3) = 1.; + + Edge4DoF *e = new Edge4DoF(Til); + e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pLKF->mnId))); + e->information() = matLambda; + optimizer.addEdge(e); + } + } + + // 1.3 Covisibility graph edges + const vector vpConnectedKFs = pKF->GetCovisiblesByWeight(minFeat); + for (vector::const_iterator vit = vpConnectedKFs.begin(); vit != vpConnectedKFs.end(); vit++) + { + KeyFrame *pKFn = *vit; + if (pKFn && pKFn != pParentKF && pKFn != prevKF && pKFn != pKF->mNextKF && !pKF->hasChild(pKFn) && !sLoopEdges.count(pKFn)) + { + if (!pKFn->isBad() && pKFn->mnId < pKF->mnId) + { + if (sInsertedEdges.count(make_pair(min(pKF->mnId, pKFn->mnId), max(pKF->mnId, pKFn->mnId)))) + continue; + + g2o::Sim3 Swn; + + LoopClosing::KeyFrameAndPose::const_iterator itn = NonCorrectedSim3.find(pKFn); + + if (itn != NonCorrectedSim3.end()) + Swn = itn->second.inverse(); + else + Swn = vScw[pKFn->mnId].inverse(); + + g2o::Sim3 Sin = Siw * Swn; + Eigen::Matrix4d Tin; + Tin.block<3, 3>(0, 0) = Sin.rotation().toRotationMatrix(); + Tin.block<3, 1>(0, 3) = Sin.translation(); + Tin(3, 3) = 1.; + Edge4DoF *e = new Edge4DoF(Tin); + e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFn->mnId))); + e->information() = matLambda; + optimizer.addEdge(e); + } + } + } + } + + optimizer.initializeOptimization(); + optimizer.computeActiveErrors(); + optimizer.optimize(20); + + unique_lock lock(pMap->mMutexMapUpdate); + + // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1] + for (size_t i = 0; i < vpKFs.size(); i++) + { + KeyFrame *pKFi = vpKFs[i]; + + const int nIDi = pKFi->mnId; + + VertexPose4DoF *Vi = static_cast(optimizer.vertex(nIDi)); + Eigen::Matrix3d Ri = Vi->estimate().Rcw[0]; + Eigen::Vector3d ti = Vi->estimate().tcw[0]; + + g2o::Sim3 CorrectedSiw = g2o::Sim3(Ri, ti, 1.); + vCorrectedSwc[nIDi] = CorrectedSiw.inverse(); + + Sophus::SE3d Tiw(CorrectedSiw.rotation(), CorrectedSiw.translation()); + pKFi->SetPose(Tiw.cast()); + } + + // Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose + for (size_t i = 0, iend = vpMPs.size(); i < iend; i++) + { + MapPoint *pMP = vpMPs[i]; + + if (pMP->isBad()) + continue; + + int nIDr; + + KeyFrame *pRefKF = pMP->GetReferenceKeyFrame(); + nIDr = pRefKF->mnId; + + g2o::Sim3 Srw = vScw[nIDr]; + g2o::Sim3 correctedSwr = vCorrectedSwc[nIDr]; + + Eigen::Matrix eigP3Dw = pMP->GetWorldPos().cast(); + Eigen::Matrix eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw)); + pMP->SetWorldPos(eigCorrectedP3Dw.cast()); + + pMP->UpdateNormalAndDepth(); + } + pMap->IncreaseChangeIndex(); +} + + + +/**************************************以下为地图融合优化**************************************************************/ + +void Optimizer::LocalBundleAdjustment( + KeyFrame *pMainKF, vector vpAdjustKF, vector vpFixedKF, bool *pbStopFlag) { bool bShowImages = false; - vector vpMPs; + vector vpMPs; g2o::SparseOptimizer optimizer; - g2o::BlockSolver_6_3::LinearSolverType * linearSolver; + g2o::BlockSolver_6_3::LinearSolverType *linearSolver; linearSolver = new g2o::LinearSolverEigen(); - g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver); + g2o::BlockSolver_6_3 *solver_ptr = new g2o::BlockSolver_6_3(linearSolver); - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); optimizer.setAlgorithm(solver); optimizer.setVerbose(false); - if(pbStopFlag) + if (pbStopFlag) optimizer.setForceStopFlag(pbStopFlag); long unsigned int maxKFid = 0; - set spKeyFrameBA; + set spKeyFrameBA; - Map* pCurrentMap = pMainKF->GetMap(); + Map *pCurrentMap = pMainKF->GetMap(); // Set fixed KeyFrame vertices int numInsertedPoints = 0; - for(KeyFrame* pKFi : vpFixedKF) + for (KeyFrame *pKFi : vpFixedKF) { - if(pKFi->isBad() || pKFi->GetMap() != pCurrentMap) + if (pKFi->isBad() || pKFi->GetMap() != pCurrentMap) { Verbose::PrintMess("ERROR LBA: KF is bad or is not in the current map", Verbose::VERBOSITY_NORMAL); continue; @@ -3534,25 +4310,25 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju pKFi->mnBALocalForMerge = pMainKF->mnId; - g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap(); + g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap(); Sophus::SE3 Tcw = pKFi->GetPose(); - vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(),Tcw.translation().cast())); + vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(), Tcw.translation().cast())); vSE3->setId(pKFi->mnId); vSE3->setFixed(true); optimizer.addVertex(vSE3); - if(pKFi->mnId>maxKFid) - maxKFid=pKFi->mnId; + if (pKFi->mnId > maxKFid) + maxKFid = pKFi->mnId; - set spViewMPs = pKFi->GetMapPoints(); - for(MapPoint* pMPi : spViewMPs) + set spViewMPs = pKFi->GetMapPoints(); + for (MapPoint *pMPi : spViewMPs) { - if(pMPi) - if(!pMPi->isBad() && pMPi->GetMap() == pCurrentMap) + if (pMPi) + if (!pMPi->isBad() && pMPi->GetMap() == pCurrentMap) - if(pMPi->mnBALocalForMerge!=pMainKF->mnId) + if (pMPi->mnBALocalForMerge != pMainKF->mnId) { vpMPs.push_back(pMPi); - pMPi->mnBALocalForMerge=pMainKF->mnId; + pMPi->mnBALocalForMerge = pMainKF->mnId; numInsertedPoints++; } } @@ -3561,31 +4337,31 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju } // Set non fixed Keyframe vertices - set spAdjustKF(vpAdjustKF.begin(), vpAdjustKF.end()); + set spAdjustKF(vpAdjustKF.begin(), vpAdjustKF.end()); numInsertedPoints = 0; - for(KeyFrame* pKFi : vpAdjustKF) + for (KeyFrame *pKFi : vpAdjustKF) { - if(pKFi->isBad() || pKFi->GetMap() != pCurrentMap) + if (pKFi->isBad() || pKFi->GetMap() != pCurrentMap) continue; pKFi->mnBALocalForMerge = pMainKF->mnId; - g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap(); + g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap(); Sophus::SE3 Tcw = pKFi->GetPose(); - vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(),Tcw.translation().cast())); + vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast(), Tcw.translation().cast())); vSE3->setId(pKFi->mnId); optimizer.addVertex(vSE3); - if(pKFi->mnId>maxKFid) - maxKFid=pKFi->mnId; + if (pKFi->mnId > maxKFid) + maxKFid = pKFi->mnId; - set spViewMPs = pKFi->GetMapPoints(); - for(MapPoint* pMPi : spViewMPs) + set spViewMPs = pKFi->GetMapPoints(); + for (MapPoint *pMPi : spViewMPs) { - if(pMPi) + if (pMPi) { - if(!pMPi->isBad() && pMPi->GetMap() == pCurrentMap) + if (!pMPi->isBad() && pMPi->GetMap() == pCurrentMap) { - if(pMPi->mnBALocalForMerge != pMainKF->mnId) + if (pMPi->mnBALocalForMerge != pMainKF->mnId) { vpMPs.push_back(pMPi); pMPi->mnBALocalForMerge = pMainKF->mnId; @@ -3598,75 +4374,74 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju spKeyFrameBA.insert(pKFi); } - const int nExpectedSize = (vpAdjustKF.size()+vpFixedKF.size())*vpMPs.size(); + const int nExpectedSize = (vpAdjustKF.size() + vpFixedKF.size()) * vpMPs.size(); - vector vpEdgesMono; + vector vpEdgesMono; vpEdgesMono.reserve(nExpectedSize); - vector vpEdgeKFMono; + vector vpEdgeKFMono; vpEdgeKFMono.reserve(nExpectedSize); - vector vpMapPointEdgeMono; + vector vpMapPointEdgeMono; vpMapPointEdgeMono.reserve(nExpectedSize); - vector vpEdgesStereo; + vector vpEdgesStereo; vpEdgesStereo.reserve(nExpectedSize); - vector vpEdgeKFStereo; + vector vpEdgeKFStereo; vpEdgeKFStereo.reserve(nExpectedSize); - vector vpMapPointEdgeStereo; + vector vpMapPointEdgeStereo; vpMapPointEdgeStereo.reserve(nExpectedSize); const float thHuber2D = sqrt(5.99); const float thHuber3D = sqrt(7.815); // Set MapPoint vertices - map mpObsKFs; - map mpObsFinalKFs; - map mpObsMPs; - for(unsigned int i=0; i < vpMPs.size(); ++i) + map mpObsKFs; + map mpObsFinalKFs; + map mpObsMPs; + for (unsigned int i = 0; i < vpMPs.size(); ++i) { - MapPoint* pMPi = vpMPs[i]; - if(pMPi->isBad()) + MapPoint *pMPi = vpMPs[i]; + if (pMPi->isBad()) continue; - g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ(); + g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ(); vPoint->setEstimate(pMPi->GetWorldPos().cast()); - const int id = pMPi->mnId+maxKFid+1; + const int id = pMPi->mnId + maxKFid + 1; vPoint->setId(id); vPoint->setMarginalized(true); optimizer.addVertex(vPoint); - - const map> observations = pMPi->GetObservations(); + const map> observations = pMPi->GetObservations(); int nEdges = 0; - //SET EDGES - for(map>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++) + // SET EDGES + for (map>::const_iterator mit = observations.begin(); mit != observations.end(); mit++) { - KeyFrame* pKF = mit->first; - if(pKF->isBad() || pKF->mnId>maxKFid || pKF->mnBALocalForMerge != pMainKF->mnId || !pKF->GetMapPoint(get<0>(mit->second))) + KeyFrame *pKF = mit->first; + if (pKF->isBad() || pKF->mnId > maxKFid || pKF->mnBALocalForMerge != pMainKF->mnId || !pKF->GetMapPoint(get<0>(mit->second))) continue; nEdges++; const cv::KeyPoint &kpUn = pKF->mvKeysUn[get<0>(mit->second)]; - if(pKF->mvuRight[get<0>(mit->second)]<0) //Monocular + if (pKF->mvuRight[get<0>(mit->second)] < 0) // Monocular { mpObsMPs[pMPi]++; - Eigen::Matrix obs; + Eigen::Matrix obs; obs << kpUn.pt.x, kpUn.pt.y; - ORB_SLAM3::EdgeSE3ProjectXYZ* e = new ORB_SLAM3::EdgeSE3ProjectXYZ(); + ORB_SLAM3::EdgeSE3ProjectXYZ *e = new ORB_SLAM3::EdgeSE3ProjectXYZ(); - e->setVertex(0, dynamic_cast(optimizer.vertex(id))); - e->setVertex(1, dynamic_cast(optimizer.vertex(pKF->mnId))); + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKF->mnId))); e->setMeasurement(obs); const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave]; - e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); rk->setDelta(thHuber2D); @@ -3682,21 +4457,21 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju } else // RGBD or Stereo { - mpObsMPs[pMPi]+=2; - Eigen::Matrix obs; + mpObsMPs[pMPi] += 2; + Eigen::Matrix obs; const float kp_ur = pKF->mvuRight[get<0>(mit->second)]; obs << kpUn.pt.x, kpUn.pt.y, kp_ur; - g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ(); + g2o::EdgeStereoSE3ProjectXYZ *e = new g2o::EdgeStereoSE3ProjectXYZ(); - e->setVertex(0, dynamic_cast(optimizer.vertex(id))); - e->setVertex(1, dynamic_cast(optimizer.vertex(pKF->mnId))); + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKF->mnId))); e->setMeasurement(obs); const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave]; - Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2; + Eigen::Matrix3d Info = Eigen::Matrix3d::Identity() * invSigma2; e->setInformation(Info); - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); rk->setDelta(thHuber3D); @@ -3717,33 +4492,33 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju } } - if(pbStopFlag) - if(*pbStopFlag) + if (pbStopFlag) + if (*pbStopFlag) return; optimizer.initializeOptimization(); optimizer.optimize(5); - bool bDoMore= true; + bool bDoMore = true; - if(pbStopFlag) - if(*pbStopFlag) + if (pbStopFlag) + if (*pbStopFlag) bDoMore = false; map mWrongObsKF; - if(bDoMore) + if (bDoMore) { // Check inlier observations int badMonoMP = 0, badStereoMP = 0; - for(size_t i=0, iend=vpEdgesMono.size(); iisBad()) + if (pMP->isBad()) continue; - if(e->chi2()>5.991 || !e->isDepthPositive()) + if (e->chi2() > 5.991 || !e->isDepthPositive()) { e->setLevel(1); badMonoMP++; @@ -3751,15 +4526,15 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju e->setRobustKernel(0); } - for(size_t i=0, iend=vpEdgesStereo.size(); iisBad()) + if (pMP->isBad()) continue; - if(e->chi2()>7.815 || !e->isDepthPositive()) + if (e->chi2() > 7.815 || !e->isDepthPositive()) { e->setLevel(1); badStereoMP++; @@ -3769,29 +4544,29 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju } Verbose::PrintMess("[BA]: First optimization(Huber), there are " + to_string(badMonoMP) + " monocular and " + to_string(badStereoMP) + " stereo bad edges", Verbose::VERBOSITY_DEBUG); - optimizer.initializeOptimization(0); - optimizer.optimize(10); + optimizer.initializeOptimization(0); + optimizer.optimize(10); } - vector > vToErase; - vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size()); - set spErasedMPs; - set spErasedKFs; + vector> vToErase; + vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size()); + set spErasedMPs; + set spErasedKFs; // Check inlier observations int badMonoMP = 0, badStereoMP = 0; - for(size_t i=0, iend=vpEdgesMono.size(); iisBad()) + if (pMP->isBad()) continue; - if(e->chi2()>5.991 || !e->isDepthPositive()) + if (e->chi2() > 5.991 || !e->isDepthPositive()) { - KeyFrame* pKFi = vpEdgeKFMono[i]; - vToErase.push_back(make_pair(pKFi,pMP)); + KeyFrame *pKFi = vpEdgeKFMono[i]; + vToErase.push_back(make_pair(pKFi, pMP)); mWrongObsKF[pKFi->mnId]++; badMonoMP++; @@ -3800,18 +4575,18 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju } } - for(size_t i=0, iend=vpEdgesStereo.size(); iisBad()) + if (pMP->isBad()) continue; - if(e->chi2()>7.815 || !e->isDepthPositive()) + if (e->chi2() > 7.815 || !e->isDepthPositive()) { - KeyFrame* pKFi = vpEdgeKFStereo[i]; - vToErase.push_back(make_pair(pKFi,pMP)); + KeyFrame *pKFi = vpEdgeKFStereo[i]; + vToErase.push_back(make_pair(pKFi, pMP)); mWrongObsKF[pKFi->mnId]++; badStereoMP++; @@ -3825,30 +4600,30 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju // Get Map Mutex unique_lock lock(pMainKF->GetMap()->mMutexMapUpdate); - if(!vToErase.empty()) + if (!vToErase.empty()) { - for(size_t i=0;iEraseMapPointMatch(pMPi); pMPi->EraseObservation(pKFi); } } - for(unsigned int i=0; i < vpMPs.size(); ++i) + for (unsigned int i = 0; i < vpMPs.size(); ++i) { - MapPoint* pMPi = vpMPs[i]; - if(pMPi->isBad()) + MapPoint *pMPi = vpMPs[i]; + if (pMPi->isBad()) continue; - const map> observations = pMPi->GetObservations(); - for(map>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++) + const map> observations = pMPi->GetObservations(); + for (map>::const_iterator mit = observations.begin(); mit != observations.end(); mit++) { - KeyFrame* pKF = mit->first; - if(pKF->isBad() || pKF->mnId>maxKFid || pKF->mnBALocalForKF != pMainKF->mnId || !pKF->GetMapPoint(get<0>(mit->second))) + KeyFrame *pKF = mit->first; + if (pKF->isBad() || pKF->mnId > maxKFid || pKF->mnBALocalForKF != pMainKF->mnId || !pKF->GetMapPoint(get<0>(mit->second))) continue; - if(pKF->mvuRight[get<0>(mit->second)]<0) //Monocular + if (pKF->mvuRight[get<0>(mit->second)] < 0) // Monocular { mpObsFinalKFs[pKF]++; } @@ -3861,63 +4636,61 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju // Recover optimized data // Keyframes - for(KeyFrame* pKFi : vpAdjustKF) + for (KeyFrame *pKFi : vpAdjustKF) { - if(pKFi->isBad()) + if (pKFi->isBad()) continue; - g2o::VertexSE3Expmap* vSE3 = static_cast(optimizer.vertex(pKFi->mnId)); + g2o::VertexSE3Expmap *vSE3 = static_cast(optimizer.vertex(pKFi->mnId)); g2o::SE3Quat SE3quat = vSE3->estimate(); Sophus::SE3f Tiw(SE3quat.rotation().cast(), SE3quat.translation().cast()); int numMonoBadPoints = 0, numMonoOptPoints = 0; int numStereoBadPoints = 0, numStereoOptPoints = 0; - vector vpMonoMPsOpt, vpStereoMPsOpt; - vector vpMonoMPsBad, vpStereoMPsBad; + vector vpMonoMPsOpt, vpStereoMPsOpt; + vector vpMonoMPsBad, vpStereoMPsBad; - for(size_t i=0, iend=vpEdgesMono.size(); iisBad()) + if (pMP->isBad()) continue; - if(e->chi2()>5.991 || !e->isDepthPositive()) + if (e->chi2() > 5.991 || !e->isDepthPositive()) { numMonoBadPoints++; vpMonoMPsBad.push_back(pMP); - } else { numMonoOptPoints++; vpMonoMPsOpt.push_back(pMP); } - } - for(size_t i=0, iend=vpEdgesStereo.size(); iisBad()) + if (pMP->isBad()) continue; - if(e->chi2()>7.815 || !e->isDepthPositive()) + if (e->chi2() > 7.815 || !e->isDepthPositive()) { numStereoBadPoints++; vpStereoMPsBad.push_back(pMP); @@ -3932,39 +4705,366 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,vector vpAdju pKFi->SetPose(Tiw); } - //Points - for(MapPoint* pMPi : vpMPs) + // Points + for (MapPoint *pMPi : vpMPs) { - if(pMPi->isBad()) + if (pMPi->isBad()) continue; - g2o::VertexSBAPointXYZ* vPoint = static_cast(optimizer.vertex(pMPi->mnId+maxKFid+1)); + g2o::VertexSBAPointXYZ *vPoint = static_cast(optimizer.vertex(pMPi->mnId + maxKFid + 1)); pMPi->SetWorldPos(vPoint->estimate().cast()); pMPi->UpdateNormalAndDepth(); - } } +void Optimizer::OptimizeEssentialGraph( + KeyFrame *pCurKF, vector &vpFixedKFs, vector &vpFixedCorrectedKFs, + vector &vpNonFixedKFs, vector &vpNonCorrectedMPs) +{ + Verbose::PrintMess("Opt_Essential: There are " + to_string(vpFixedKFs.size()) + " KFs fixed in the merged map", Verbose::VERBOSITY_DEBUG); + Verbose::PrintMess("Opt_Essential: There are " + to_string(vpFixedCorrectedKFs.size()) + " KFs fixed in the old map", Verbose::VERBOSITY_DEBUG); + Verbose::PrintMess("Opt_Essential: There are " + to_string(vpNonFixedKFs.size()) + " KFs non-fixed in the merged map", Verbose::VERBOSITY_DEBUG); + Verbose::PrintMess("Opt_Essential: There are " + to_string(vpNonCorrectedMPs.size()) + " MPs non-corrected in the merged map", Verbose::VERBOSITY_DEBUG); -void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbStopFlag, Map *pMap, LoopClosing::KeyFrameAndPose &corrPoses) + g2o::SparseOptimizer optimizer; + optimizer.setVerbose(false); + g2o::BlockSolver_7_3::LinearSolverType *linearSolver = + new g2o::LinearSolverEigen(); + g2o::BlockSolver_7_3 *solver_ptr = new g2o::BlockSolver_7_3(linearSolver); + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + + solver->setUserLambdaInit(1e-16); + optimizer.setAlgorithm(solver); + + Map *pMap = pCurKF->GetMap(); + const unsigned int nMaxKFid = pMap->GetMaxKFid(); + + vector> vScw(nMaxKFid + 1); + vector> vCorrectedSwc(nMaxKFid + 1); + vector vpVertices(nMaxKFid + 1); + + vector vpGoodPose(nMaxKFid + 1); + vector vpBadPose(nMaxKFid + 1); + + const int minFeat = 100; + + for (KeyFrame *pKFi : vpFixedKFs) + { + if (pKFi->isBad()) + continue; + + g2o::VertexSim3Expmap *VSim3 = new g2o::VertexSim3Expmap(); + + const int nIDi = pKFi->mnId; + + Sophus::SE3d Tcw = pKFi->GetPose().cast(); + g2o::Sim3 Siw(Tcw.unit_quaternion(), Tcw.translation(), 1.0); + + vCorrectedSwc[nIDi] = Siw.inverse(); + VSim3->setEstimate(Siw); + + VSim3->setFixed(true); + + VSim3->setId(nIDi); + VSim3->setMarginalized(false); + VSim3->_fix_scale = true; + + optimizer.addVertex(VSim3); + + vpVertices[nIDi] = VSim3; + + vpGoodPose[nIDi] = true; + vpBadPose[nIDi] = false; + } + Verbose::PrintMess("Opt_Essential: vpFixedKFs loaded", Verbose::VERBOSITY_DEBUG); + + set sIdKF; + for (KeyFrame *pKFi : vpFixedCorrectedKFs) + { + if (pKFi->isBad()) + continue; + + g2o::VertexSim3Expmap *VSim3 = new g2o::VertexSim3Expmap(); + + const int nIDi = pKFi->mnId; + + Sophus::SE3d Tcw = pKFi->GetPose().cast(); + g2o::Sim3 Siw(Tcw.unit_quaternion(), Tcw.translation(), 1.0); + + vCorrectedSwc[nIDi] = Siw.inverse(); + VSim3->setEstimate(Siw); + + Sophus::SE3d Tcw_bef = pKFi->mTcwBefMerge.cast(); + vScw[nIDi] = g2o::Sim3(Tcw_bef.unit_quaternion(), Tcw_bef.translation(), 1.0); + + VSim3->setFixed(true); + + VSim3->setId(nIDi); + VSim3->setMarginalized(false); + + optimizer.addVertex(VSim3); + + vpVertices[nIDi] = VSim3; + + sIdKF.insert(nIDi); + + vpGoodPose[nIDi] = true; + vpBadPose[nIDi] = true; + } + + for (KeyFrame *pKFi : vpNonFixedKFs) + { + if (pKFi->isBad()) + continue; + + const int nIDi = pKFi->mnId; + + if (sIdKF.count(nIDi)) // It has already added in the corrected merge KFs + continue; + + g2o::VertexSim3Expmap *VSim3 = new g2o::VertexSim3Expmap(); + + Sophus::SE3d Tcw = pKFi->GetPose().cast(); + g2o::Sim3 Siw(Tcw.unit_quaternion(), Tcw.translation(), 1.0); + + vScw[nIDi] = Siw; + VSim3->setEstimate(Siw); + + VSim3->setFixed(false); + + VSim3->setId(nIDi); + VSim3->setMarginalized(false); + + optimizer.addVertex(VSim3); + + vpVertices[nIDi] = VSim3; + + sIdKF.insert(nIDi); + + vpGoodPose[nIDi] = false; + vpBadPose[nIDi] = true; + } + + vector vpKFs; + vpKFs.reserve(vpFixedKFs.size() + vpFixedCorrectedKFs.size() + vpNonFixedKFs.size()); + vpKFs.insert(vpKFs.end(), vpFixedKFs.begin(), vpFixedKFs.end()); + vpKFs.insert(vpKFs.end(), vpFixedCorrectedKFs.begin(), vpFixedCorrectedKFs.end()); + vpKFs.insert(vpKFs.end(), vpNonFixedKFs.begin(), vpNonFixedKFs.end()); + set spKFs(vpKFs.begin(), vpKFs.end()); + + const Eigen::Matrix matLambda = Eigen::Matrix::Identity(); + + for (KeyFrame *pKFi : vpKFs) + { + int num_connections = 0; + const int nIDi = pKFi->mnId; + + g2o::Sim3 correctedSwi; + g2o::Sim3 Swi; + + if (vpGoodPose[nIDi]) + correctedSwi = vCorrectedSwc[nIDi]; + if (vpBadPose[nIDi]) + Swi = vScw[nIDi].inverse(); + + KeyFrame *pParentKFi = pKFi->GetParent(); + + // Spanning tree edge + if (pParentKFi && spKFs.find(pParentKFi) != spKFs.end()) + { + int nIDj = pParentKFi->mnId; + + g2o::Sim3 Sjw; + bool bHasRelation = false; + + if (vpGoodPose[nIDi] && vpGoodPose[nIDj]) + { + Sjw = vCorrectedSwc[nIDj].inverse(); + bHasRelation = true; + } + else if (vpBadPose[nIDi] && vpBadPose[nIDj]) + { + Sjw = vScw[nIDj]; + bHasRelation = true; + } + + if (bHasRelation) + { + g2o::Sim3 Sji = Sjw * Swi; + + g2o::EdgeSim3 *e = new g2o::EdgeSim3(); + e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); + e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + e->setMeasurement(Sji); + + e->information() = matLambda; + optimizer.addEdge(e); + num_connections++; + } + } + + // Loop edges + const set sLoopEdges = pKFi->GetLoopEdges(); + for (set::const_iterator sit = sLoopEdges.begin(), send = sLoopEdges.end(); sit != send; sit++) + { + KeyFrame *pLKF = *sit; + if (spKFs.find(pLKF) != spKFs.end() && pLKF->mnId < pKFi->mnId) + { + g2o::Sim3 Slw; + bool bHasRelation = false; + + if (vpGoodPose[nIDi] && vpGoodPose[pLKF->mnId]) + { + Slw = vCorrectedSwc[pLKF->mnId].inverse(); + bHasRelation = true; + } + else if (vpBadPose[nIDi] && vpBadPose[pLKF->mnId]) + { + Slw = vScw[pLKF->mnId]; + bHasRelation = true; + } + + if (bHasRelation) + { + g2o::Sim3 Sli = Slw * Swi; + g2o::EdgeSim3 *el = new g2o::EdgeSim3(); + el->setVertex(1, dynamic_cast(optimizer.vertex(pLKF->mnId))); + el->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + el->setMeasurement(Sli); + el->information() = matLambda; + optimizer.addEdge(el); + num_connections++; + } + } + } + + // Covisibility graph edges + const vector vpConnectedKFs = pKFi->GetCovisiblesByWeight(minFeat); + for (vector::const_iterator vit = vpConnectedKFs.begin(); vit != vpConnectedKFs.end(); vit++) + { + KeyFrame *pKFn = *vit; + if (pKFn && pKFn != pParentKFi && !pKFi->hasChild(pKFn) && !sLoopEdges.count(pKFn) && spKFs.find(pKFn) != spKFs.end()) + { + if (!pKFn->isBad() && pKFn->mnId < pKFi->mnId) + { + + g2o::Sim3 Snw = vScw[pKFn->mnId]; + bool bHasRelation = false; + + if (vpGoodPose[nIDi] && vpGoodPose[pKFn->mnId]) + { + Snw = vCorrectedSwc[pKFn->mnId].inverse(); + bHasRelation = true; + } + else if (vpBadPose[nIDi] && vpBadPose[pKFn->mnId]) + { + Snw = vScw[pKFn->mnId]; + bHasRelation = true; + } + + if (bHasRelation) + { + g2o::Sim3 Sni = Snw * Swi; + + g2o::EdgeSim3 *en = new g2o::EdgeSim3(); + en->setVertex(1, dynamic_cast(optimizer.vertex(pKFn->mnId))); + en->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); + en->setMeasurement(Sni); + en->information() = matLambda; + optimizer.addEdge(en); + num_connections++; + } + } + } + } + + if (num_connections == 0) + { + Verbose::PrintMess("Opt_Essential: KF " + to_string(pKFi->mnId) + " has 0 connections", Verbose::VERBOSITY_DEBUG); + } + } + + // Optimize! + optimizer.initializeOptimization(); + optimizer.optimize(20); + + unique_lock lock(pMap->mMutexMapUpdate); + + // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1] + for (KeyFrame *pKFi : vpNonFixedKFs) + { + if (pKFi->isBad()) + continue; + + const int nIDi = pKFi->mnId; + + g2o::VertexSim3Expmap *VSim3 = static_cast(optimizer.vertex(nIDi)); + g2o::Sim3 CorrectedSiw = VSim3->estimate(); + vCorrectedSwc[nIDi] = CorrectedSiw.inverse(); + double s = CorrectedSiw.scale(); + Sophus::SE3d Tiw(CorrectedSiw.rotation(), CorrectedSiw.translation() / s); + + pKFi->mTcwBefMerge = pKFi->GetPose(); + pKFi->mTwcBefMerge = pKFi->GetPoseInverse(); + pKFi->SetPose(Tiw.cast()); + } + + // Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose + for (MapPoint *pMPi : vpNonCorrectedMPs) + { + if (pMPi->isBad()) + continue; + + KeyFrame *pRefKF = pMPi->GetReferenceKeyFrame(); + while (pRefKF->isBad()) + { + if (!pRefKF) + { + Verbose::PrintMess("MP " + to_string(pMPi->mnId) + " without a valid reference KF", Verbose::VERBOSITY_DEBUG); + break; + } + + pMPi->EraseObservation(pRefKF); + pRefKF = pMPi->GetReferenceKeyFrame(); + } + + if (vpBadPose[pRefKF->mnId]) + { + Sophus::SE3f TNonCorrectedwr = pRefKF->mTwcBefMerge; + Sophus::SE3f Twr = pRefKF->GetPoseInverse(); + + Eigen::Vector3f eigCorrectedP3Dw = Twr * TNonCorrectedwr.inverse() * pMPi->GetWorldPos(); + pMPi->SetWorldPos(eigCorrectedP3Dw); + + pMPi->UpdateNormalAndDepth(); + } + else + { + cout << "ERROR: MapPoint has a reference KF from another map" << endl; + } + } +} + +void Optimizer::MergeInertialBA( + KeyFrame *pCurrKF, KeyFrame *pMergeKF, bool *pbStopFlag, Map *pMap, LoopClosing::KeyFrameAndPose &corrPoses) { const int Nd = 6; const unsigned long maxKFid = pCurrKF->mnId; - vector vpOptimizableKFs; - vpOptimizableKFs.reserve(2*Nd); + vector vpOptimizableKFs; + vpOptimizableKFs.reserve(2 * Nd); // For cov KFS, inertial parameters are not optimized const int maxCovKF = 30; - vector vpOptimizableCovKFs; + vector vpOptimizableCovKFs; vpOptimizableCovKFs.reserve(maxCovKF); // Add sliding window for current KF vpOptimizableKFs.push_back(pCurrKF); pCurrKF->mnBALocalForKF = pCurrKF->mnId; - for(int i=1; imPrevKF) + if (vpOptimizableKFs.back()->mPrevKF) { vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mPrevKF); vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId; @@ -3973,11 +5073,11 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbS break; } - list lFixedKeyFrames; - if(vpOptimizableKFs.back()->mPrevKF) + list lFixedKeyFrames; + if (vpOptimizableKFs.back()->mPrevKF) { vpOptimizableCovKFs.push_back(vpOptimizableKFs.back()->mPrevKF); - vpOptimizableKFs.back()->mPrevKF->mnBALocalForKF=pCurrKF->mnId; + vpOptimizableKFs.back()->mPrevKF->mnBALocalForKF = pCurrKF->mnId; } else { @@ -3990,9 +5090,9 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbS pMergeKF->mnBALocalForKF = pCurrKF->mnId; // Previous KFs - for(int i=1; i<(Nd/2); i++) + for (int i = 1; i < (Nd / 2); i++) { - if(vpOptimizableKFs.back()->mPrevKF) + if (vpOptimizableKFs.back()->mPrevKF) { vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mPrevKF); vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId; @@ -4002,29 +5102,29 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbS } // We fix just once the old map - if(vpOptimizableKFs.back()->mPrevKF) + if (vpOptimizableKFs.back()->mPrevKF) { lFixedKeyFrames.push_back(vpOptimizableKFs.back()->mPrevKF); - vpOptimizableKFs.back()->mPrevKF->mnBAFixedForKF=pCurrKF->mnId; + vpOptimizableKFs.back()->mPrevKF->mnBAFixedForKF = pCurrKF->mnId; } else { - vpOptimizableKFs.back()->mnBALocalForKF=0; - vpOptimizableKFs.back()->mnBAFixedForKF=pCurrKF->mnId; + vpOptimizableKFs.back()->mnBALocalForKF = 0; + vpOptimizableKFs.back()->mnBAFixedForKF = pCurrKF->mnId; lFixedKeyFrames.push_back(vpOptimizableKFs.back()); vpOptimizableKFs.pop_back(); } // Next KFs - if(pMergeKF->mNextKF) + if (pMergeKF->mNextKF) { vpOptimizableKFs.push_back(pMergeKF->mNextKF); vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId; } - while(vpOptimizableKFs.size()<(2*Nd)) + while (vpOptimizableKFs.size() < (2 * Nd)) { - if(vpOptimizableKFs.back()->mNextKF) + if (vpOptimizableKFs.back()->mNextKF) { vpOptimizableKFs.push_back(vpOptimizableKFs.back()->mNextKF); vpOptimizableKFs.back()->mnBALocalForKF = pCurrKF->mnId; @@ -4036,50 +5136,51 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbS int N = vpOptimizableKFs.size(); // Optimizable points seen by optimizable keyframes - list lLocalMapPoints; - map mLocalObs; - for(int i=0; i lLocalMapPoints; + map mLocalObs; + for (int i = 0; i < N; i++) { - vector vpMPs = vpOptimizableKFs[i]->GetMapPointMatches(); - for(vector::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++) + vector vpMPs = vpOptimizableKFs[i]->GetMapPointMatches(); + for (vector::iterator vit = vpMPs.begin(), vend = vpMPs.end(); vit != vend; vit++) { // Using mnBALocalForKF we avoid redundance here, one MP can not be added several times to lLocalMapPoints - MapPoint* pMP = *vit; - if(pMP) - if(!pMP->isBad()) - if(pMP->mnBALocalForKF!=pCurrKF->mnId) + MapPoint *pMP = *vit; + if (pMP) + if (!pMP->isBad()) + if (pMP->mnBALocalForKF != pCurrKF->mnId) { - mLocalObs[pMP]=1; + mLocalObs[pMP] = 1; lLocalMapPoints.push_back(pMP); - pMP->mnBALocalForKF=pCurrKF->mnId; + pMP->mnBALocalForKF = pCurrKF->mnId; } - else { + else + { mLocalObs[pMP]++; } } } - std::vector> pairs; + std::vector> pairs; pairs.reserve(mLocalObs.size()); for (auto itr = mLocalObs.begin(); itr != mLocalObs.end(); ++itr) pairs.push_back(*itr); - sort(pairs.begin(), pairs.end(),sortByVal); + sort(pairs.begin(), pairs.end(), sortByVal); // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes - int i=0; - for(vector>::iterator lit=pairs.begin(), lend=pairs.end(); lit!=lend; lit++, i++) + int i = 0; + for (vector>::iterator lit = pairs.begin(), lend = pairs.end(); lit != lend; lit++, i++) { - map> observations = lit->first->GetObservations(); - if(i>=maxCovKF) + map> observations = lit->first->GetObservations(); + if (i >= maxCovKF) break; - for(map>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + for (map>::iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) { - KeyFrame* pKFi = mit->first; + KeyFrame *pKFi = mit->first; - if(pKFi->mnBALocalForKF!=pCurrKF->mnId && pKFi->mnBAFixedForKF!=pCurrKF->mnId) // If optimizable or already included... + if (pKFi->mnBALocalForKF != pCurrKF->mnId && pKFi->mnBAFixedForKF != pCurrKF->mnId) // If optimizable or already included... { - pKFi->mnBALocalForKF=pCurrKF->mnId; - if(!pKFi->isBad()) + pKFi->mnBALocalForKF = pCurrKF->mnId; + if (!pKFi->isBad()) { vpOptimizableCovKFs.push_back(pKFi); break; @@ -4089,12 +5190,12 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbS } g2o::SparseOptimizer optimizer; - g2o::BlockSolverX::LinearSolverType * linearSolver; + g2o::BlockSolverX::LinearSolverType *linearSolver; linearSolver = new g2o::LinearSolverEigen(); - g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); + g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver); - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); solver->setUserLambdaInit(1e3); @@ -4102,145 +5203,145 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbS optimizer.setVerbose(false); // Set Local KeyFrame vertices - N=vpOptimizableKFs.size(); - for(int i=0; isetId(pKFi->mnId); VP->setFixed(false); optimizer.addVertex(VP); - if(pKFi->bImu) + if (pKFi->bImu) { - VertexVelocity* VV = new VertexVelocity(pKFi); - VV->setId(maxKFid+3*(pKFi->mnId)+1); + VertexVelocity *VV = new VertexVelocity(pKFi); + VV->setId(maxKFid + 3 * (pKFi->mnId) + 1); VV->setFixed(false); optimizer.addVertex(VV); - VertexGyroBias* VG = new VertexGyroBias(pKFi); - VG->setId(maxKFid+3*(pKFi->mnId)+2); + VertexGyroBias *VG = new VertexGyroBias(pKFi); + VG->setId(maxKFid + 3 * (pKFi->mnId) + 2); VG->setFixed(false); optimizer.addVertex(VG); - VertexAccBias* VA = new VertexAccBias(pKFi); - VA->setId(maxKFid+3*(pKFi->mnId)+3); + VertexAccBias *VA = new VertexAccBias(pKFi); + VA->setId(maxKFid + 3 * (pKFi->mnId) + 3); VA->setFixed(false); optimizer.addVertex(VA); } } // Set Local cov keyframes vertices - int Ncov=vpOptimizableCovKFs.size(); - for(int i=0; isetId(pKFi->mnId); VP->setFixed(false); optimizer.addVertex(VP); - if(pKFi->bImu) + if (pKFi->bImu) { - VertexVelocity* VV = new VertexVelocity(pKFi); - VV->setId(maxKFid+3*(pKFi->mnId)+1); + VertexVelocity *VV = new VertexVelocity(pKFi); + VV->setId(maxKFid + 3 * (pKFi->mnId) + 1); VV->setFixed(false); optimizer.addVertex(VV); - VertexGyroBias* VG = new VertexGyroBias(pKFi); - VG->setId(maxKFid+3*(pKFi->mnId)+2); + VertexGyroBias *VG = new VertexGyroBias(pKFi); + VG->setId(maxKFid + 3 * (pKFi->mnId) + 2); VG->setFixed(false); optimizer.addVertex(VG); - VertexAccBias* VA = new VertexAccBias(pKFi); - VA->setId(maxKFid+3*(pKFi->mnId)+3); + VertexAccBias *VA = new VertexAccBias(pKFi); + VA->setId(maxKFid + 3 * (pKFi->mnId) + 3); VA->setFixed(false); optimizer.addVertex(VA); } } // Set Fixed KeyFrame vertices - for(list::iterator lit=lFixedKeyFrames.begin(), lend=lFixedKeyFrames.end(); lit!=lend; lit++) + for (list::iterator lit = lFixedKeyFrames.begin(), lend = lFixedKeyFrames.end(); lit != lend; lit++) { - KeyFrame* pKFi = *lit; - VertexPose * VP = new VertexPose(pKFi); + KeyFrame *pKFi = *lit; + VertexPose *VP = new VertexPose(pKFi); VP->setId(pKFi->mnId); VP->setFixed(true); optimizer.addVertex(VP); - if(pKFi->bImu) + if (pKFi->bImu) { - VertexVelocity* VV = new VertexVelocity(pKFi); - VV->setId(maxKFid+3*(pKFi->mnId)+1); + VertexVelocity *VV = new VertexVelocity(pKFi); + VV->setId(maxKFid + 3 * (pKFi->mnId) + 1); VV->setFixed(true); optimizer.addVertex(VV); - VertexGyroBias* VG = new VertexGyroBias(pKFi); - VG->setId(maxKFid+3*(pKFi->mnId)+2); + VertexGyroBias *VG = new VertexGyroBias(pKFi); + VG->setId(maxKFid + 3 * (pKFi->mnId) + 2); VG->setFixed(true); optimizer.addVertex(VG); - VertexAccBias* VA = new VertexAccBias(pKFi); - VA->setId(maxKFid+3*(pKFi->mnId)+3); + VertexAccBias *VA = new VertexAccBias(pKFi); + VA->setId(maxKFid + 3 * (pKFi->mnId) + 3); VA->setFixed(true); optimizer.addVertex(VA); } } // Create intertial constraints - vector vei(N,(EdgeInertial*)NULL); - vector vegr(N,(EdgeGyroRW*)NULL); - vector vear(N,(EdgeAccRW*)NULL); - for(int i=0;i vei(N, (EdgeInertial *)NULL); + vector vegr(N, (EdgeGyroRW *)NULL); + vector vear(N, (EdgeAccRW *)NULL); + for (int i = 0; i < N; i++) { - //cout << "inserting inertial edge " << i << endl; - KeyFrame* pKFi = vpOptimizableKFs[i]; + // cout << "inserting inertial edge " << i << endl; + KeyFrame *pKFi = vpOptimizableKFs[i]; - if(!pKFi->mPrevKF) + if (!pKFi->mPrevKF) { Verbose::PrintMess("NOT INERTIAL LINK TO PREVIOUS FRAME!!!!", Verbose::VERBOSITY_NORMAL); continue; } - if(pKFi->bImu && pKFi->mPrevKF->bImu && pKFi->mpImuPreintegrated) + if (pKFi->bImu && pKFi->mPrevKF->bImu && pKFi->mpImuPreintegrated) { pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias()); - g2o::HyperGraph::Vertex* VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); - g2o::HyperGraph::Vertex* VV1 = optimizer.vertex(maxKFid+3*(pKFi->mPrevKF->mnId)+1); - g2o::HyperGraph::Vertex* VG1 = optimizer.vertex(maxKFid+3*(pKFi->mPrevKF->mnId)+2); - g2o::HyperGraph::Vertex* VA1 = optimizer.vertex(maxKFid+3*(pKFi->mPrevKF->mnId)+3); - g2o::HyperGraph::Vertex* VP2 = optimizer.vertex(pKFi->mnId); - g2o::HyperGraph::Vertex* VV2 = optimizer.vertex(maxKFid+3*(pKFi->mnId)+1); - g2o::HyperGraph::Vertex* VG2 = optimizer.vertex(maxKFid+3*(pKFi->mnId)+2); - g2o::HyperGraph::Vertex* VA2 = optimizer.vertex(maxKFid+3*(pKFi->mnId)+3); + g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId); + g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 1); + g2o::HyperGraph::Vertex *VG1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 2); + g2o::HyperGraph::Vertex *VA1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 3); + g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId); + g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1); + g2o::HyperGraph::Vertex *VG2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2); + g2o::HyperGraph::Vertex *VA2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3); - if(!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2 || !VG2 || !VA2) + if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2 || !VG2 || !VA2) { - cerr << "Error " << VP1 << ", "<< VV1 << ", "<< VG1 << ", "<< VA1 << ", " << VP2 << ", " << VV2 << ", "<< VG2 << ", "<< VA2 <mpImuPreintegrated); - vei[i]->setVertex(0,dynamic_cast(VP1)); - vei[i]->setVertex(1,dynamic_cast(VV1)); - vei[i]->setVertex(2,dynamic_cast(VG1)); - vei[i]->setVertex(3,dynamic_cast(VA1)); - vei[i]->setVertex(4,dynamic_cast(VP2)); - vei[i]->setVertex(5,dynamic_cast(VV2)); + vei[i]->setVertex(0, dynamic_cast(VP1)); + vei[i]->setVertex(1, dynamic_cast(VV1)); + vei[i]->setVertex(2, dynamic_cast(VG1)); + vei[i]->setVertex(3, dynamic_cast(VA1)); + vei[i]->setVertex(4, dynamic_cast(VP2)); + vei[i]->setVertex(5, dynamic_cast(VV2)); // TODO Uncomment - g2o::RobustKernelHuber* rki = new g2o::RobustKernelHuber; + g2o::RobustKernelHuber *rki = new g2o::RobustKernelHuber; vei[i]->setRobustKernel(rki); rki->setDelta(sqrt(16.92)); optimizer.addEdge(vei[i]); vegr[i] = new EdgeGyroRW(); - vegr[i]->setVertex(0,VG1); - vegr[i]->setVertex(1,VG2); - Eigen::Matrix3d InfoG = pKFi->mpImuPreintegrated->C.block<3,3>(9,9).cast().inverse(); + vegr[i]->setVertex(0, VG1); + vegr[i]->setVertex(1, VG2); + Eigen::Matrix3d InfoG = pKFi->mpImuPreintegrated->C.block<3, 3>(9, 9).cast().inverse(); vegr[i]->setInformation(InfoG); optimizer.addEdge(vegr[i]); vear[i] = new EdgeAccRW(); - vear[i]->setVertex(0,VA1); - vear[i]->setVertex(1,VA2); - Eigen::Matrix3d InfoA = pKFi->mpImuPreintegrated->C.block<3,3>(12,12).cast().inverse(); + vear[i]->setVertex(0, VA1); + vear[i]->setVertex(1, VA2); + Eigen::Matrix3d InfoA = pKFi->mpImuPreintegrated->C.block<3, 3>(12, 12).cast().inverse(); vear[i]->setInformation(InfoA); optimizer.addEdge(vear[i]); } @@ -4250,28 +5351,27 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbS Verbose::PrintMess("end inserting inertial edges", Verbose::VERBOSITY_NORMAL); - // Set MapPoint vertices - const int nExpectedSize = (N+Ncov+lFixedKeyFrames.size())*lLocalMapPoints.size(); + const int nExpectedSize = (N + Ncov + lFixedKeyFrames.size()) * lLocalMapPoints.size(); // Mono - vector vpEdgesMono; + vector vpEdgesMono; vpEdgesMono.reserve(nExpectedSize); - vector vpEdgeKFMono; + vector vpEdgeKFMono; vpEdgeKFMono.reserve(nExpectedSize); - vector vpMapPointEdgeMono; + vector vpMapPointEdgeMono; vpMapPointEdgeMono.reserve(nExpectedSize); // Stereo - vector vpEdgesStereo; + vector vpEdgesStereo; vpEdgesStereo.reserve(nExpectedSize); - vector vpEdgeKFStereo; + vector vpEdgeKFStereo; vpEdgeKFStereo.reserve(nExpectedSize); - vector vpMapPointEdgeStereo; + vector vpMapPointEdgeStereo; vpMapPointEdgeStereo.reserve(nExpectedSize); const float thHuberMono = sqrt(5.991); @@ -4279,60 +5379,60 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbS const float thHuberStereo = sqrt(7.815); const float chi2Stereo2 = 7.815; - const unsigned long iniMPid = maxKFid*5; + const unsigned long iniMPid = maxKFid * 5; - for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) { - MapPoint* pMP = *lit; + MapPoint *pMP = *lit; if (!pMP) continue; - g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ(); + g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ(); vPoint->setEstimate(pMP->GetWorldPos().cast()); - unsigned long id = pMP->mnId+iniMPid+1; + unsigned long id = pMP->mnId + iniMPid + 1; vPoint->setId(id); vPoint->setMarginalized(true); optimizer.addVertex(vPoint); - const map> observations = pMP->GetObservations(); + const map> observations = pMP->GetObservations(); // Create visual constraints - for(map>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + for (map>::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) { - KeyFrame* pKFi = mit->first; + KeyFrame *pKFi = mit->first; if (!pKFi) continue; - if ((pKFi->mnBALocalForKF!=pCurrKF->mnId) && (pKFi->mnBAFixedForKF!=pCurrKF->mnId)) + if ((pKFi->mnBALocalForKF != pCurrKF->mnId) && (pKFi->mnBAFixedForKF != pCurrKF->mnId)) continue; - if (pKFi->mnId>maxKFid){ + if (pKFi->mnId > maxKFid) + { continue; } - - if(optimizer.vertex(id)==NULL || optimizer.vertex(pKFi->mnId)==NULL) + if (optimizer.vertex(id) == NULL || optimizer.vertex(pKFi->mnId) == NULL) continue; - if(!pKFi->isBad()) + if (!pKFi->isBad()) { const cv::KeyPoint &kpUn = pKFi->mvKeysUn[get<0>(mit->second)]; - if(pKFi->mvuRight[get<0>(mit->second)]<0) // Monocular observation + if (pKFi->mvuRight[get<0>(mit->second)] < 0) // Monocular observation { - Eigen::Matrix obs; + Eigen::Matrix obs; obs << kpUn.pt.x, kpUn.pt.y; - EdgeMono* e = new EdgeMono(); - e->setVertex(0, dynamic_cast(optimizer.vertex(id))); - e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); + EdgeMono *e = new EdgeMono(); + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); e->setMeasurement(obs); const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; - e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); rk->setDelta(thHuberMono); optimizer.addEdge(e); @@ -4343,18 +5443,18 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbS else // stereo observation { const float kp_ur = pKFi->mvuRight[get<0>(mit->second)]; - Eigen::Matrix obs; + Eigen::Matrix obs; obs << kpUn.pt.x, kpUn.pt.y, kp_ur; - EdgeStereo* e = new EdgeStereo(); + EdgeStereo *e = new EdgeStereo(); - e->setVertex(0, dynamic_cast(optimizer.vertex(id))); - e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); + e->setVertex(0, dynamic_cast(optimizer.vertex(id))); + e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); e->setMeasurement(obs); const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; - e->setInformation(Eigen::Matrix3d::Identity()*invSigma2); + e->setInformation(Eigen::Matrix3d::Identity() * invSigma2); - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; + g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); rk->setDelta(thHuberStereo); @@ -4367,121 +5467,120 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbS } } - if(pbStopFlag) + if (pbStopFlag) optimizer.setForceStopFlag(pbStopFlag); - if(pbStopFlag) - if(*pbStopFlag) + if (pbStopFlag) + if (*pbStopFlag) return; optimizer.initializeOptimization(); optimizer.optimize(8); - vector > vToErase; - vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size()); + vector> vToErase; + vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size()); // Check inlier observations // Mono - for(size_t i=0, iend=vpEdgesMono.size(); iisBad()) + if (pMP->isBad()) continue; - if(e->chi2()>chi2Mono2) + if (e->chi2() > chi2Mono2) { - KeyFrame* pKFi = vpEdgeKFMono[i]; - vToErase.push_back(make_pair(pKFi,pMP)); + KeyFrame *pKFi = vpEdgeKFMono[i]; + vToErase.push_back(make_pair(pKFi, pMP)); } } // Stereo - for(size_t i=0, iend=vpEdgesStereo.size(); iisBad()) + if (pMP->isBad()) continue; - if(e->chi2()>chi2Stereo2) + if (e->chi2() > chi2Stereo2) { - KeyFrame* pKFi = vpEdgeKFStereo[i]; - vToErase.push_back(make_pair(pKFi,pMP)); + KeyFrame *pKFi = vpEdgeKFStereo[i]; + vToErase.push_back(make_pair(pKFi, pMP)); } } // Get Map Mutex and erase outliers unique_lock lock(pMap->mMutexMapUpdate); - if(!vToErase.empty()) + if (!vToErase.empty()) { - for(size_t i=0;iEraseMapPointMatch(pMPi); pMPi->EraseObservation(pKFi); } } - // Recover optimized data - //Keyframes - for(int i=0; i(optimizer.vertex(pKFi->mnId)); + VertexPose *VP = static_cast(optimizer.vertex(pKFi->mnId)); Sophus::SE3f Tcw(VP->estimate().Rcw[0].cast(), VP->estimate().tcw[0].cast()); pKFi->SetPose(Tcw); Sophus::SE3d Tiw = pKFi->GetPose().cast(); - g2o::Sim3 g2oSiw(Tiw.unit_quaternion(),Tiw.translation(),1.0); + g2o::Sim3 g2oSiw(Tiw.unit_quaternion(), Tiw.translation(), 1.0); corrPoses[pKFi] = g2oSiw; - if(pKFi->bImu) + if (pKFi->bImu) { - VertexVelocity* VV = static_cast(optimizer.vertex(maxKFid+3*(pKFi->mnId)+1)); + VertexVelocity *VV = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1)); pKFi->SetVelocity(VV->estimate().cast()); - VertexGyroBias* VG = static_cast(optimizer.vertex(maxKFid+3*(pKFi->mnId)+2)); - VertexAccBias* VA = static_cast(optimizer.vertex(maxKFid+3*(pKFi->mnId)+3)); + VertexGyroBias *VG = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2)); + VertexAccBias *VA = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3)); Vector6d b; b << VG->estimate(), VA->estimate(); - pKFi->SetNewBias(IMU::Bias(b[3],b[4],b[5],b[0],b[1],b[2])); + pKFi->SetNewBias(IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2])); } } - for(int i=0; i(optimizer.vertex(pKFi->mnId)); + VertexPose *VP = static_cast(optimizer.vertex(pKFi->mnId)); Sophus::SE3f Tcw(VP->estimate().Rcw[0].cast(), VP->estimate().tcw[0].cast()); pKFi->SetPose(Tcw); Sophus::SE3d Tiw = pKFi->GetPose().cast(); - g2o::Sim3 g2oSiw(Tiw.unit_quaternion(),Tiw.translation(),1.0); + g2o::Sim3 g2oSiw(Tiw.unit_quaternion(), Tiw.translation(), 1.0); corrPoses[pKFi] = g2oSiw; - if(pKFi->bImu) + if (pKFi->bImu) { - VertexVelocity* VV = static_cast(optimizer.vertex(maxKFid+3*(pKFi->mnId)+1)); + VertexVelocity *VV = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1)); pKFi->SetVelocity(VV->estimate().cast()); - VertexGyroBias* VG = static_cast(optimizer.vertex(maxKFid+3*(pKFi->mnId)+2)); - VertexAccBias* VA = static_cast(optimizer.vertex(maxKFid+3*(pKFi->mnId)+3)); + VertexGyroBias *VG = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2)); + VertexAccBias *VA = static_cast(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3)); Vector6d b; b << VG->estimate(), VA->estimate(); - pKFi->SetNewBias(IMU::Bias(b[3],b[4],b[5],b[0],b[1],b[2])); + pKFi->SetNewBias(IMU::Bias(b[3], b[4], b[5], b[0], b[1], b[2])); } } - //Points - for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) + // Points + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) { - MapPoint* pMP = *lit; - g2o::VertexSBAPointXYZ* vPoint = static_cast(optimizer.vertex(pMP->mnId+iniMPid+1)); + MapPoint *pMP = *lit; + g2o::VertexSBAPointXYZ *vPoint = static_cast(optimizer.vertex(pMP->mnId + iniMPid + 1)); pMP->SetWorldPos(vPoint->estimate().cast()); pMP->UpdateNormalAndDepth(); } @@ -4489,1107 +5588,4 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbS pMap->IncreaseChangeIndex(); } -int Optimizer::PoseInertialOptimizationLastKeyFrame(Frame *pFrame, bool bRecInit) -{ - g2o::SparseOptimizer optimizer; - g2o::BlockSolverX::LinearSolverType * linearSolver; - - linearSolver = new g2o::LinearSolverDense(); - - g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); - - g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr); - optimizer.setVerbose(false); - optimizer.setAlgorithm(solver); - - int nInitialMonoCorrespondences=0; - int nInitialStereoCorrespondences=0; - int nInitialCorrespondences=0; - - // Set Frame vertex - VertexPose* VP = new VertexPose(pFrame); - VP->setId(0); - VP->setFixed(false); - optimizer.addVertex(VP); - VertexVelocity* VV = new VertexVelocity(pFrame); - VV->setId(1); - VV->setFixed(false); - optimizer.addVertex(VV); - VertexGyroBias* VG = new VertexGyroBias(pFrame); - VG->setId(2); - VG->setFixed(false); - optimizer.addVertex(VG); - VertexAccBias* VA = new VertexAccBias(pFrame); - VA->setId(3); - VA->setFixed(false); - optimizer.addVertex(VA); - - // Set MapPoint vertices - const int N = pFrame->N; - const int Nleft = pFrame->Nleft; - const bool bRight = (Nleft!=-1); - - vector vpEdgesMono; - vector vpEdgesStereo; - vector vnIndexEdgeMono; - vector vnIndexEdgeStereo; - vpEdgesMono.reserve(N); - vpEdgesStereo.reserve(N); - vnIndexEdgeMono.reserve(N); - vnIndexEdgeStereo.reserve(N); - - const float thHuberMono = sqrt(5.991); - const float thHuberStereo = sqrt(7.815); - - { - unique_lock lock(MapPoint::mGlobalMutex); - - for(int i=0; imvpMapPoints[i]; - if(pMP) - { - cv::KeyPoint kpUn; - - // Left monocular observation - // 这里说的Left monocular包含两种情况：1.单目情况 2.两个相机情况下的相机1 - if((!bRight && pFrame->mvuRight[i]<0) || i < Nleft) - { - //如果是两个相机情况下的相机1 - if(i < Nleft) // pair left-right - kpUn = pFrame->mvKeys[i]; - else - kpUn = pFrame->mvKeysUn[i]; - - nInitialMonoCorrespondences++; - pFrame->mvbOutlier[i] = false; - - Eigen::Matrix obs; - obs << kpUn.pt.x, kpUn.pt.y; - - EdgeMonoOnlyPose* e = new EdgeMonoOnlyPose(pMP->GetWorldPos(),0); - - e->setVertex(0,VP); - e->setMeasurement(obs); - - // Add here uncerteinty - const float unc2 = pFrame->mpCamera->uncertainty2(obs); - - const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]/unc2; - e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); - - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; - e->setRobustKernel(rk); - rk->setDelta(thHuberMono); - - optimizer.addEdge(e); - - vpEdgesMono.push_back(e); - vnIndexEdgeMono.push_back(i); - } - // Stereo observation - else if(!bRight) - { - nInitialStereoCorrespondences++; - pFrame->mvbOutlier[i] = false; - - kpUn = pFrame->mvKeysUn[i]; - const float kp_ur = pFrame->mvuRight[i]; - Eigen::Matrix obs; - obs << kpUn.pt.x, kpUn.pt.y, kp_ur; - - EdgeStereoOnlyPose* e = new EdgeStereoOnlyPose(pMP->GetWorldPos()); - - e->setVertex(0, VP); - e->setMeasurement(obs); - - // Add here uncerteinty - const float unc2 = pFrame->mpCamera->uncertainty2(obs.head(2)); - - const float &invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]/unc2; - e->setInformation(Eigen::Matrix3d::Identity()*invSigma2); - - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; - e->setRobustKernel(rk); - rk->setDelta(thHuberStereo); - - optimizer.addEdge(e); - - vpEdgesStereo.push_back(e); - vnIndexEdgeStereo.push_back(i); - } - - // Right monocular observation - if(bRight && i >= Nleft) - { - nInitialMonoCorrespondences++; - pFrame->mvbOutlier[i] = false; - - kpUn = pFrame->mvKeysRight[i - Nleft]; - Eigen::Matrix obs; - obs << kpUn.pt.x, kpUn.pt.y; - - EdgeMonoOnlyPose* e = new EdgeMonoOnlyPose(pMP->GetWorldPos(),1); - - e->setVertex(0,VP); - e->setMeasurement(obs); - - // Add here uncerteinty - const float unc2 = pFrame->mpCamera->uncertainty2(obs); - - const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]/unc2; - e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); - - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; - e->setRobustKernel(rk); - rk->setDelta(thHuberMono); - - optimizer.addEdge(e); - - vpEdgesMono.push_back(e); - vnIndexEdgeMono.push_back(i); - } - } - } - } - nInitialCorrespondences = nInitialMonoCorrespondences + nInitialStereoCorrespondences; - - KeyFrame* pKF = pFrame->mpLastKeyFrame; - VertexPose* VPk = new VertexPose(pKF); - VPk->setId(4); - VPk->setFixed(true); - optimizer.addVertex(VPk); - VertexVelocity* VVk = new VertexVelocity(pKF); - VVk->setId(5); - VVk->setFixed(true); - optimizer.addVertex(VVk); - VertexGyroBias* VGk = new VertexGyroBias(pKF); - VGk->setId(6); - VGk->setFixed(true); - optimizer.addVertex(VGk); - VertexAccBias* VAk = new VertexAccBias(pKF); - VAk->setId(7); - VAk->setFixed(true); - optimizer.addVertex(VAk); - - EdgeInertial* ei = new EdgeInertial(pFrame->mpImuPreintegrated); - - ei->setVertex(0, VPk); - ei->setVertex(1, VVk); - ei->setVertex(2, VGk); - ei->setVertex(3, VAk); - ei->setVertex(4, VP); - ei->setVertex(5, VV); - optimizer.addEdge(ei); - - EdgeGyroRW* egr = new EdgeGyroRW(); - egr->setVertex(0,VGk); - egr->setVertex(1,VG); - Eigen::Matrix3d InfoG = pFrame->mpImuPreintegrated->C.block<3,3>(9,9).cast().inverse(); - egr->setInformation(InfoG); - optimizer.addEdge(egr); - - EdgeAccRW* ear = new EdgeAccRW(); - ear->setVertex(0,VAk); - ear->setVertex(1,VA); - Eigen::Matrix3d InfoA = pFrame->mpImuPreintegrated->C.block<3,3>(12,12).cast().inverse(); - ear->setInformation(InfoA); - optimizer.addEdge(ear); - - // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier - // At the next optimization, outliers are not included, but at the end they can be classified as inliers again. - float chi2Mono[4]={12,7.5,5.991,5.991}; - float chi2Stereo[4]={15.6,9.8,7.815,7.815}; - - int its[4]={10,10,10,10}; - - int nBad = 0; - int nBadMono = 0; - int nBadStereo = 0; - int nInliersMono = 0; - int nInliersStereo = 0; - int nInliers = 0; - for(size_t it=0; it<4; it++) - { - optimizer.initializeOptimization(0); - optimizer.optimize(its[it]); - - nBad = 0; - nBadMono = 0; - nBadStereo = 0; - nInliers = 0; - nInliersMono = 0; - nInliersStereo = 0; - float chi2close = 1.5*chi2Mono[it]; - - // For monocular observations - for(size_t i=0, iend=vpEdgesMono.size(); imvbOutlier[idx]) - { - e->computeError(); - } - - const float chi2 = e->chi2(); - bool bClose = pFrame->mvpMapPoints[idx]->mTrackDepth<10.f; - - if((chi2>chi2Mono[it]&&!bClose)||(bClose && chi2>chi2close)||!e->isDepthPositive()) - { - pFrame->mvbOutlier[idx]=true; - e->setLevel(1); - nBadMono++; - } - else - { - pFrame->mvbOutlier[idx]=false; - e->setLevel(0); - nInliersMono++; - } - - if (it==2) - e->setRobustKernel(0); - } - - // For stereo observations - for(size_t i=0, iend=vpEdgesStereo.size(); imvbOutlier[idx]) - { - e->computeError(); - } - - const float chi2 = e->chi2(); - - if(chi2>chi2Stereo[it]) - { - pFrame->mvbOutlier[idx]=true; - e->setLevel(1); // not included in next optimization - nBadStereo++; - } - else - { - pFrame->mvbOutlier[idx]=false; - e->setLevel(0); - nInliersStereo++; - } - - if(it==2) - e->setRobustKernel(0); - } - - nInliers = nInliersMono + nInliersStereo; - nBad = nBadMono + nBadStereo; - - if(optimizer.edges().size()<10) - { - break; - } - - } - - // If not too much tracks, recover not too bad points - if ((nInliers<30) && !bRecInit) - { - nBad=0; - const float chi2MonoOut = 18.f; - const float chi2StereoOut = 24.f; - EdgeMonoOnlyPose* e1; - EdgeStereoOnlyPose* e2; - for(size_t i=0, iend=vnIndexEdgeMono.size(); icomputeError(); - if (e1->chi2()mvbOutlier[idx]=false; - else - nBad++; - } - for(size_t i=0, iend=vnIndexEdgeStereo.size(); icomputeError(); - if (e2->chi2()mvbOutlier[idx]=false; - else - nBad++; - } - } - - // Recover optimized pose, velocity and biases - pFrame->SetImuPoseVelocity(VP->estimate().Rwb.cast(), VP->estimate().twb.cast(), VV->estimate().cast()); - Vector6d b; - b << VG->estimate(), VA->estimate(); - pFrame->mImuBias = IMU::Bias(b[3],b[4],b[5],b[0],b[1],b[2]); - - // Recover Hessian, marginalize keyFframe states and generate new prior for frame - Eigen::Matrix H; - H.setZero(); - - H.block<9,9>(0,0)+= ei->GetHessian2(); - H.block<3,3>(9,9) += egr->GetHessian2(); - H.block<3,3>(12,12) += ear->GetHessian2(); - - int tot_in = 0, tot_out = 0; - for(size_t i=0, iend=vpEdgesMono.size(); imvbOutlier[idx]) - { - H.block<6,6>(0,0) += e->GetHessian(); - tot_in++; - } - else - tot_out++; - } - - for(size_t i=0, iend=vpEdgesStereo.size(); imvbOutlier[idx]) - { - H.block<6,6>(0,0) += e->GetHessian(); - tot_in++; - } - else - tot_out++; - } - - pFrame->mpcpi = new ConstraintPoseImu(VP->estimate().Rwb,VP->estimate().twb,VV->estimate(),VG->estimate(),VA->estimate(),H); - - return nInitialCorrespondences-nBad; -} - -int Optimizer::PoseInertialOptimizationLastFrame(Frame *pFrame, bool bRecInit) -{ - g2o::SparseOptimizer optimizer; - g2o::BlockSolverX::LinearSolverType * linearSolver; - - linearSolver = new g2o::LinearSolverDense(); - - g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); - - g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr); - optimizer.setAlgorithm(solver); - optimizer.setVerbose(false); - - int nInitialMonoCorrespondences=0; - int nInitialStereoCorrespondences=0; - int nInitialCorrespondences=0; - - // Set Current Frame vertex - VertexPose* VP = new VertexPose(pFrame); - VP->setId(0); - VP->setFixed(false); - optimizer.addVertex(VP); - VertexVelocity* VV = new VertexVelocity(pFrame); - VV->setId(1); - VV->setFixed(false); - optimizer.addVertex(VV); - VertexGyroBias* VG = new VertexGyroBias(pFrame); - VG->setId(2); - VG->setFixed(false); - optimizer.addVertex(VG); - VertexAccBias* VA = new VertexAccBias(pFrame); - VA->setId(3); - VA->setFixed(false); - optimizer.addVertex(VA); - - // Set MapPoint vertices - const int N = pFrame->N; - const int Nleft = pFrame->Nleft; - const bool bRight = (Nleft!=-1); - - vector vpEdgesMono; - vector vpEdgesStereo; - vector vnIndexEdgeMono; - vector vnIndexEdgeStereo; - vpEdgesMono.reserve(N); - vpEdgesStereo.reserve(N); - vnIndexEdgeMono.reserve(N); - vnIndexEdgeStereo.reserve(N); - - const float thHuberMono = sqrt(5.991); - const float thHuberStereo = sqrt(7.815); - - { - unique_lock lock(MapPoint::mGlobalMutex); - - for(int i=0; imvpMapPoints[i]; - if(pMP) - { - cv::KeyPoint kpUn; - // Left monocular observation - // 这里说的Left monocular包含两种情况：1.单目情况 2.两个相机情况下的相机1 - if((!bRight && pFrame->mvuRight[i]<0) || i < Nleft) - { - //如果是两个相机情况下的相机1 - if(i < Nleft) // pair left-right - kpUn = pFrame->mvKeys[i]; - else - kpUn = pFrame->mvKeysUn[i]; - - nInitialMonoCorrespondences++; - pFrame->mvbOutlier[i] = false; - - Eigen::Matrix obs; - obs << kpUn.pt.x, kpUn.pt.y; - - EdgeMonoOnlyPose* e = new EdgeMonoOnlyPose(pMP->GetWorldPos(),0); - - e->setVertex(0,VP); - e->setMeasurement(obs); - - // Add here uncerteinty - const float unc2 = pFrame->mpCamera->uncertainty2(obs); - - const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]/unc2; - e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); - - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; - e->setRobustKernel(rk); - rk->setDelta(thHuberMono); - - optimizer.addEdge(e); - - vpEdgesMono.push_back(e); - vnIndexEdgeMono.push_back(i); - } - // Stereo observation - else if(!bRight) - { - nInitialStereoCorrespondences++; - pFrame->mvbOutlier[i] = false; - - kpUn = pFrame->mvKeysUn[i]; - const float kp_ur = pFrame->mvuRight[i]; - Eigen::Matrix obs; - obs << kpUn.pt.x, kpUn.pt.y, kp_ur; - - EdgeStereoOnlyPose* e = new EdgeStereoOnlyPose(pMP->GetWorldPos()); - - e->setVertex(0, VP); - e->setMeasurement(obs); - - // Add here uncerteinty - const float unc2 = pFrame->mpCamera->uncertainty2(obs.head(2)); - - const float &invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]/unc2; - e->setInformation(Eigen::Matrix3d::Identity()*invSigma2); - - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; - e->setRobustKernel(rk); - rk->setDelta(thHuberStereo); - - optimizer.addEdge(e); - - vpEdgesStereo.push_back(e); - vnIndexEdgeStereo.push_back(i); - } - - // Right monocular observation - if(bRight && i >= Nleft) - { - nInitialMonoCorrespondences++; - pFrame->mvbOutlier[i] = false; - - kpUn = pFrame->mvKeysRight[i - Nleft]; - Eigen::Matrix obs; - obs << kpUn.pt.x, kpUn.pt.y; - - EdgeMonoOnlyPose* e = new EdgeMonoOnlyPose(pMP->GetWorldPos(),1); - - e->setVertex(0,VP); - e->setMeasurement(obs); - - // Add here uncerteinty - const float unc2 = pFrame->mpCamera->uncertainty2(obs); - - const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]/unc2; - e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); - - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; - e->setRobustKernel(rk); - rk->setDelta(thHuberMono); - - optimizer.addEdge(e); - - vpEdgesMono.push_back(e); - vnIndexEdgeMono.push_back(i); - } - } - } - } - - nInitialCorrespondences = nInitialMonoCorrespondences + nInitialStereoCorrespondences; - - // Set Previous Frame Vertex - Frame* pFp = pFrame->mpPrevFrame; - - VertexPose* VPk = new VertexPose(pFp); - VPk->setId(4); - VPk->setFixed(false); - optimizer.addVertex(VPk); - VertexVelocity* VVk = new VertexVelocity(pFp); - VVk->setId(5); - VVk->setFixed(false); - optimizer.addVertex(VVk); - VertexGyroBias* VGk = new VertexGyroBias(pFp); - VGk->setId(6); - VGk->setFixed(false); - optimizer.addVertex(VGk); - VertexAccBias* VAk = new VertexAccBias(pFp); - VAk->setId(7); - VAk->setFixed(false); - optimizer.addVertex(VAk); - - EdgeInertial* ei = new EdgeInertial(pFrame->mpImuPreintegratedFrame); - - ei->setVertex(0, VPk); - ei->setVertex(1, VVk); - ei->setVertex(2, VGk); - ei->setVertex(3, VAk); - ei->setVertex(4, VP); - ei->setVertex(5, VV); - optimizer.addEdge(ei); - - EdgeGyroRW* egr = new EdgeGyroRW(); - egr->setVertex(0,VGk); - egr->setVertex(1,VG); - Eigen::Matrix3d InfoG = pFrame->mpImuPreintegrated->C.block<3,3>(9,9).cast().inverse(); - egr->setInformation(InfoG); - optimizer.addEdge(egr); - - EdgeAccRW* ear = new EdgeAccRW(); - ear->setVertex(0,VAk); - ear->setVertex(1,VA); - Eigen::Matrix3d InfoA = pFrame->mpImuPreintegrated->C.block<3,3>(12,12).cast().inverse(); - ear->setInformation(InfoA); - optimizer.addEdge(ear); - - if (!pFp->mpcpi) - Verbose::PrintMess("pFp->mpcpi does not exist!!!\nPrevious Frame " + to_string(pFp->mnId), Verbose::VERBOSITY_NORMAL); - - EdgePriorPoseImu* ep = new EdgePriorPoseImu(pFp->mpcpi); - - ep->setVertex(0,VPk); - ep->setVertex(1,VVk); - ep->setVertex(2,VGk); - ep->setVertex(3,VAk); - g2o::RobustKernelHuber* rkp = new g2o::RobustKernelHuber; - ep->setRobustKernel(rkp); - rkp->setDelta(5); - optimizer.addEdge(ep); - - // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier - // At the next optimization, outliers are not included, but at the end they can be classified as inliers again. - const float chi2Mono[4]={5.991,5.991,5.991,5.991}; - const float chi2Stereo[4]={15.6f,9.8f,7.815f,7.815f}; - const int its[4]={10,10,10,10}; - - int nBad=0; - int nBadMono = 0; - int nBadStereo = 0; - int nInliersMono = 0; - int nInliersStereo = 0; - int nInliers=0; - for(size_t it=0; it<4; it++) - { - optimizer.initializeOptimization(0); - optimizer.optimize(its[it]); - - nBad=0; - nBadMono = 0; - nBadStereo = 0; - nInliers=0; - nInliersMono=0; - nInliersStereo=0; - float chi2close = 1.5*chi2Mono[it]; - - for(size_t i=0, iend=vpEdgesMono.size(); imvpMapPoints[idx]->mTrackDepth<10.f; - - if(pFrame->mvbOutlier[idx]) - { - e->computeError(); - } - - const float chi2 = e->chi2(); - - if((chi2>chi2Mono[it]&&!bClose)||(bClose && chi2>chi2close)||!e->isDepthPositive()) - { - pFrame->mvbOutlier[idx]=true; - e->setLevel(1); - nBadMono++; - } - else - { - pFrame->mvbOutlier[idx]=false; - e->setLevel(0); - nInliersMono++; - } - - if (it==2) - e->setRobustKernel(0); - - } - - for(size_t i=0, iend=vpEdgesStereo.size(); imvbOutlier[idx]) - { - e->computeError(); - } - - const float chi2 = e->chi2(); - - if(chi2>chi2Stereo[it]) - { - pFrame->mvbOutlier[idx]=true; - e->setLevel(1); - nBadStereo++; - } - else - { - pFrame->mvbOutlier[idx]=false; - e->setLevel(0); - nInliersStereo++; - } - - if(it==2) - e->setRobustKernel(0); - } - - nInliers = nInliersMono + nInliersStereo; - nBad = nBadMono + nBadStereo; - - if(optimizer.edges().size()<10) - { - break; - } - } - - - if ((nInliers<30) && !bRecInit) - { - nBad=0; - const float chi2MonoOut = 18.f; - const float chi2StereoOut = 24.f; - EdgeMonoOnlyPose* e1; - EdgeStereoOnlyPose* e2; - for(size_t i=0, iend=vnIndexEdgeMono.size(); icomputeError(); - if (e1->chi2()mvbOutlier[idx]=false; - else - nBad++; - - } - for(size_t i=0, iend=vnIndexEdgeStereo.size(); icomputeError(); - if (e2->chi2()mvbOutlier[idx]=false; - else - nBad++; - } - } - - nInliers = nInliersMono + nInliersStereo; - - - // Recover optimized pose, velocity and biases - pFrame->SetImuPoseVelocity(VP->estimate().Rwb.cast(), VP->estimate().twb.cast(), VV->estimate().cast()); - Vector6d b; - b << VG->estimate(), VA->estimate(); - pFrame->mImuBias = IMU::Bias(b[3],b[4],b[5],b[0],b[1],b[2]); - - // Recover Hessian, marginalize previous frame states and generate new prior for frame - Eigen::Matrix H; - H.setZero(); - - H.block<24,24>(0,0)+= ei->GetHessian(); - - Eigen::Matrix Hgr = egr->GetHessian(); - H.block<3,3>(9,9) += Hgr.block<3,3>(0,0); - H.block<3,3>(9,24) += Hgr.block<3,3>(0,3); - H.block<3,3>(24,9) += Hgr.block<3,3>(3,0); - H.block<3,3>(24,24) += Hgr.block<3,3>(3,3); - - Eigen::Matrix Har = ear->GetHessian(); - H.block<3,3>(12,12) += Har.block<3,3>(0,0); - H.block<3,3>(12,27) += Har.block<3,3>(0,3); - H.block<3,3>(27,12) += Har.block<3,3>(3,0); - H.block<3,3>(27,27) += Har.block<3,3>(3,3); - - H.block<15,15>(0,0) += ep->GetHessian(); - - int tot_in = 0, tot_out = 0; - for(size_t i=0, iend=vpEdgesMono.size(); imvbOutlier[idx]) - { - H.block<6,6>(15,15) += e->GetHessian(); - tot_in++; - } - else - tot_out++; - } - - for(size_t i=0, iend=vpEdgesStereo.size(); imvbOutlier[idx]) - { - H.block<6,6>(15,15) += e->GetHessian(); - tot_in++; - } - else - tot_out++; - } - - H = Marginalize(H,0,14); - - pFrame->mpcpi = new ConstraintPoseImu(VP->estimate().Rwb,VP->estimate().twb,VV->estimate(),VG->estimate(),VA->estimate(),H.block<15,15>(15,15)); - delete pFp->mpcpi; - pFp->mpcpi = NULL; - - return nInitialCorrespondences-nBad; -} - -void Optimizer::OptimizeEssentialGraph4DoF(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, - const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, - const LoopClosing::KeyFrameAndPose &CorrectedSim3, - const map > &LoopConnections) -{ - typedef g2o::BlockSolver< g2o::BlockSolverTraits<4, 4> > BlockSolver_4_4; - - // Setup optimizer - g2o::SparseOptimizer optimizer; - optimizer.setVerbose(false); - g2o::BlockSolverX::LinearSolverType * linearSolver = - new g2o::LinearSolverEigen(); - g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver); - - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); - - optimizer.setAlgorithm(solver); - - const vector vpKFs = pMap->GetAllKeyFrames(); - const vector vpMPs = pMap->GetAllMapPoints(); - - const unsigned int nMaxKFid = pMap->GetMaxKFid(); - - vector > vScw(nMaxKFid+1); - vector > vCorrectedSwc(nMaxKFid+1); - - vector vpVertices(nMaxKFid+1); - - const int minFeat = 100; - // Set KeyFrame vertices - for(size_t i=0, iend=vpKFs.size(); iisBad()) - continue; - - VertexPose4DoF* V4DoF; - - const int nIDi = pKF->mnId; - - LoopClosing::KeyFrameAndPose::const_iterator it = CorrectedSim3.find(pKF); - - if(it!=CorrectedSim3.end()) - { - vScw[nIDi] = it->second; - const g2o::Sim3 Swc = it->second.inverse(); - Eigen::Matrix3d Rwc = Swc.rotation().toRotationMatrix(); - Eigen::Vector3d twc = Swc.translation(); - V4DoF = new VertexPose4DoF(Rwc, twc, pKF); - } - else - { - Sophus::SE3d Tcw = pKF->GetPose().cast(); - g2o::Sim3 Siw(Tcw.unit_quaternion(),Tcw.translation(),1.0); - - vScw[nIDi] = Siw; - V4DoF = new VertexPose4DoF(pKF); - } - - if(pKF==pLoopKF) - V4DoF->setFixed(true); - - V4DoF->setId(nIDi); - V4DoF->setMarginalized(false); - - optimizer.addVertex(V4DoF); - vpVertices[nIDi]=V4DoF; - } - set > sInsertedEdges; - - // Edge used in posegraph has still 6Dof, even if updates of camera poses are just in 4DoF - Eigen::Matrix matLambda = Eigen::Matrix::Identity(); - matLambda(0,0) = 1e3; - matLambda(1,1) = 1e3; - matLambda(0,0) = 1e3; - - // Set Loop edges - Edge4DoF* e_loop; - for(map >::const_iterator mit = LoopConnections.begin(), mend=LoopConnections.end(); mit!=mend; mit++) - { - KeyFrame* pKF = mit->first; - const long unsigned int nIDi = pKF->mnId; - const set &spConnections = mit->second; - const g2o::Sim3 Siw = vScw[nIDi]; - - for(set::const_iterator sit=spConnections.begin(), send=spConnections.end(); sit!=send; sit++) - { - const long unsigned int nIDj = (*sit)->mnId; - if((nIDi!=pCurKF->mnId || nIDj!=pLoopKF->mnId) && pKF->GetWeight(*sit)(0,0) = Sij.rotation().toRotationMatrix(); - Tij.block<3,1>(0,3) = Sij.translation(); - Tij(3,3) = 1.; - - Edge4DoF* e = new Edge4DoF(Tij); - e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); - e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); - - e->information() = matLambda; - e_loop = e; - optimizer.addEdge(e); - - sInsertedEdges.insert(make_pair(min(nIDi,nIDj),max(nIDi,nIDj))); - } - } - - // 1. Set normal edges - for(size_t i=0, iend=vpKFs.size(); imnId; - - g2o::Sim3 Siw; - - // Use noncorrected poses for posegraph edges - LoopClosing::KeyFrameAndPose::const_iterator iti = NonCorrectedSim3.find(pKF); - - if(iti!=NonCorrectedSim3.end()) - Siw = iti->second; - else - Siw = vScw[nIDi]; - - // 1.1.0 Spanning tree edge - KeyFrame* pParentKF = static_cast(NULL); - if(pParentKF) - { - int nIDj = pParentKF->mnId; - - g2o::Sim3 Swj; - - LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF); - - if(itj!=NonCorrectedSim3.end()) - Swj = (itj->second).inverse(); - else - Swj = vScw[nIDj].inverse(); - - g2o::Sim3 Sij = Siw * Swj; - Eigen::Matrix4d Tij; - Tij.block<3,3>(0,0) = Sij.rotation().toRotationMatrix(); - Tij.block<3,1>(0,3) = Sij.translation(); - Tij(3,3)=1.; - - Edge4DoF* e = new Edge4DoF(Tij); - e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); - e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); - e->information() = matLambda; - optimizer.addEdge(e); - } - - // 1.1.1 Inertial edges - KeyFrame* prevKF = pKF->mPrevKF; - if(prevKF) - { - int nIDj = prevKF->mnId; - - g2o::Sim3 Swj; - - LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(prevKF); - - if(itj!=NonCorrectedSim3.end()) - Swj = (itj->second).inverse(); - else - Swj = vScw[nIDj].inverse(); - - g2o::Sim3 Sij = Siw * Swj; - Eigen::Matrix4d Tij; - Tij.block<3,3>(0,0) = Sij.rotation().toRotationMatrix(); - Tij.block<3,1>(0,3) = Sij.translation(); - Tij(3,3)=1.; - - Edge4DoF* e = new Edge4DoF(Tij); - e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); - e->setVertex(1, dynamic_cast(optimizer.vertex(nIDj))); - e->information() = matLambda; - optimizer.addEdge(e); - } - - // 1.2 Loop edges - const set sLoopEdges = pKF->GetLoopEdges(); - for(set::const_iterator sit=sLoopEdges.begin(), send=sLoopEdges.end(); sit!=send; sit++) - { - KeyFrame* pLKF = *sit; - if(pLKF->mnIdmnId) - { - g2o::Sim3 Swl; - - LoopClosing::KeyFrameAndPose::const_iterator itl = NonCorrectedSim3.find(pLKF); - - if(itl!=NonCorrectedSim3.end()) - Swl = itl->second.inverse(); - else - Swl = vScw[pLKF->mnId].inverse(); - - g2o::Sim3 Sil = Siw * Swl; - Eigen::Matrix4d Til; - Til.block<3,3>(0,0) = Sil.rotation().toRotationMatrix(); - Til.block<3,1>(0,3) = Sil.translation(); - Til(3,3) = 1.; - - Edge4DoF* e = new Edge4DoF(Til); - e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); - e->setVertex(1, dynamic_cast(optimizer.vertex(pLKF->mnId))); - e->information() = matLambda; - optimizer.addEdge(e); - } - } - - // 1.3 Covisibility graph edges - const vector vpConnectedKFs = pKF->GetCovisiblesByWeight(minFeat); - for(vector::const_iterator vit=vpConnectedKFs.begin(); vit!=vpConnectedKFs.end(); vit++) - { - KeyFrame* pKFn = *vit; - if(pKFn && pKFn!=pParentKF && pKFn!=prevKF && pKFn!=pKF->mNextKF && !pKF->hasChild(pKFn) && !sLoopEdges.count(pKFn)) - { - if(!pKFn->isBad() && pKFn->mnIdmnId) - { - if(sInsertedEdges.count(make_pair(min(pKF->mnId,pKFn->mnId),max(pKF->mnId,pKFn->mnId)))) - continue; - - g2o::Sim3 Swn; - - LoopClosing::KeyFrameAndPose::const_iterator itn = NonCorrectedSim3.find(pKFn); - - if(itn!=NonCorrectedSim3.end()) - Swn = itn->second.inverse(); - else - Swn = vScw[pKFn->mnId].inverse(); - - g2o::Sim3 Sin = Siw * Swn; - Eigen::Matrix4d Tin; - Tin.block<3,3>(0,0) = Sin.rotation().toRotationMatrix(); - Tin.block<3,1>(0,3) = Sin.translation(); - Tin(3,3) = 1.; - Edge4DoF* e = new Edge4DoF(Tin); - e->setVertex(0, dynamic_cast(optimizer.vertex(nIDi))); - e->setVertex(1, dynamic_cast(optimizer.vertex(pKFn->mnId))); - e->information() = matLambda; - optimizer.addEdge(e); - } - } - } - } - - optimizer.initializeOptimization(); - optimizer.computeActiveErrors(); - optimizer.optimize(20); - - unique_lock lock(pMap->mMutexMapUpdate); - - // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1] - for(size_t i=0;imnId; - - VertexPose4DoF* Vi = static_cast(optimizer.vertex(nIDi)); - Eigen::Matrix3d Ri = Vi->estimate().Rcw[0]; - Eigen::Vector3d ti = Vi->estimate().tcw[0]; - - g2o::Sim3 CorrectedSiw = g2o::Sim3(Ri,ti,1.); - vCorrectedSwc[nIDi]=CorrectedSiw.inverse(); - - Sophus::SE3d Tiw(CorrectedSiw.rotation(),CorrectedSiw.translation()); - pKFi->SetPose(Tiw.cast()); - } - - // Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose - for(size_t i=0, iend=vpMPs.size(); iisBad()) - continue; - - int nIDr; - - KeyFrame* pRefKF = pMP->GetReferenceKeyFrame(); - nIDr = pRefKF->mnId; - - g2o::Sim3 Srw = vScw[nIDr]; - g2o::Sim3 correctedSwr = vCorrectedSwc[nIDr]; - - Eigen::Matrix eigP3Dw = pMP->GetWorldPos().cast(); - Eigen::Matrix eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw)); - pMP->SetWorldPos(eigCorrectedP3Dw.cast()); - - pMP->UpdateNormalAndDepth(); - } - pMap->IncreaseChangeIndex(); -} - -} //namespace ORB_SLAM +} // namespace ORB_SLAM