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