ORBSLAM和LearnVIORB的代码差别
在Converter.cc中，后者在前者的基础上添加了
void Converter::updateNS(NavState& ns, const IMUPreintegrator& imupreint, const Vector3d& gw)
{}
cv::Mat Converter::toCvMatInverse(const cv::Mat &Tcw)
{}在Frame.cc中，后者在前者的第一个Frame::Frame(const Frame &frame)析构函数中添加了
mvIMUDataSinceLastFrame = frame.mvIMUDataSinceLastFrame;
mNavState = frame.GetNavState();
mMargCovInv = frame.mMargCovInv;
mNavStatePrior = frame.mNavStatePrior;
除此之外，还在开头添加了下列函数
void Frame::ComputeIMUPreIntSinceLastFrame(const Frame* pLastF, IMUPreintegrator& IMUPreInt) const
{}
void Frame::UpdatePoseFromNS(const cv::Mat &Tbc)
{}
void Frame::UpdateNavState(const IMUPreintegrator& imupreint, const Vector3d& gw)
{}
const NavState& Frame::GetNavState(void) const
{}
void Frame::SetInitialNavStateAndBias(const NavState& ns)
{}
void Frame::SetNavStateBiasGyr(const Vector3d &bg)
{}
void Frame::SetNavStateBiasAcc(const Vector3d &ba)
{}
void Frame::SetNavState(const NavState& ns)
{}
Frame::Frame(const cv::Mat &imGray, const double &timeStamp, const std::vector &vimu, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, KeyFrame*pLastKF):mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast(NULL)),mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth)
{} FrameDrawer.cc中，总体改动不大，后者只添加了几个时间值 mStartTime，mCurTime
Initializer.cc没做修改
KeyFlame.cc中，后者修改了void KeyFrame::SetBadFlag() ,void KeyFrame::UpdateConnections()(
mapMapPointObs/*map*/ observations = pMP->GetObservations();
for(mapMapPointObs/*map*/::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
原：
map observations = pMP->GetObservations();
for(map::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) 将map
cerr<<"shouldn't call this KeyFrame()"<)还添加了以下函数
void KeyFrame::UpdateNavStatePVRFromTcw(const cv::Mat &Tcw,const cv::Mat &Tbc)
{}
void KeyFrame::SetInitialNavStateAndBias(const NavState& ns)
{}
KeyFrame* KeyFrame::GetPrevKeyFrame(void)
{}
KeyFrame* KeyFrame::GetNextKeyFrame(void)
{}
void KeyFrame::SetPrevKeyFrame(KeyFrame* pKF)
{}
void KeyFrame::SetNextKeyFrame(KeyFrame* pKF)
{}
std::vector KeyFrame::GetVectorIMUData(void)
{}
void KeyFrame::AppendIMUDataToFront(KeyFrame* pPrevKF)
{}
void KeyFrame::UpdatePoseFromNS(const cv::Mat &Tbc)
{}
void KeyFrame::UpdateNavState(const IMUPreintegrator& imupreint, const Vector3d& gw)
{}
void KeyFrame::SetNavState(const NavState& ns)
{}
const NavState& KeyFrame::GetNavState(void)
{}
void KeyFrame::SetNavStateBiasGyr(const Vector3d &bg)
{}
void KeyFrame::SetNavStateBiasAcc(const Vector3d &ba)
{}
void KeyFrame::SetNavStateVel(const Vector3d &vel)
{}
void KeyFrame::SetNavStatePos(const Vector3d &pos)
{}
void KeyFrame::SetNavStateRot(const Matrix3d &rot)
{}
void KeyFrame::SetNavStateRot(const Sophus::SO3 &rot)
{}
void KeyFrame::SetNavStateDeltaBg(const Vector3d &dbg)
{}
void KeyFrame::SetNavStateDeltaBa(const Vector3d &dba)
{}
const IMUPreintegrator & KeyFrame::GetIMUPreInt(void)
{}
void KeyFrame::ComputePreInt(void)
{} LocalMapping.cc，后者修改了void LocalMapping::ResetIfRequested()
添加了
mlLocalKeyFrames.clear();
// Add resetting init flags
mbVINSInited = false;
mbFirstTry = true;void LocalMapping::KeyFrameCulling()中添加了
KeyFrame* pOldestLocalKF = mlLocalKeyFrames.front();
KeyFrame* pPrevLocalKF = pOldestLocalKF->GetPrevKeyFrame();
KeyFrame* pNewestLocalKF = mlLocalKeyFrames.back();
// Test log
if(pOldestLocalKF->isBad()) cerr<<"pOldestLocalKF is bad, check 1. id: "<mnId<isBad()) cerr<<"pPrevLocalKF is bad, check 1. id: "<mnId<isBad()) cerr<<"pNewestLocalKF is bad, check 1. id: "<mnId<GetPrevKeyFrame();
KeyFrame* pNextKF = pKF->GetNextKeyFrame();
if(pPrevKF && pNextKF && !GetVINSInited())
{
if(fabs(pNextKF->mTimeStamp - pPrevKF->mTimeStamp) > /*0.2*/0.5)
continue;
}
// Don't drop the KF before current KF
if(pKF->GetNextKeyFrame() == mpCurrentKeyFrame)
continue;
if(pKF->mTimeStamp >= mpCurrentKeyFrame->mTimeStamp - 0.11)
continue;
if(pPrevKF && pNextKF)
{
double timegap=0.51;
if(GetVINSInited() && pKF->mTimeStamp < mpCurrentKeyFrame->mTimeStamp - 4.0)
timegap = 3.01;
if(fabs(pNextKF->mTimeStamp - pPrevKF->mTimeStamp) > timegap)
continue;
} void LocalMapping::ProcessNewKeyFrame()中添加了
// Delete bad KF in LocalWindow
DeleteBadInLocalWindow();
// Add Keyframe to LocalWindow
AddToLocalWindow(mpCurrentKeyFrame);class KeyFrameInit
{};
bool LocalMapping::GetUpdatingInitPoses(void)
{}
void LocalMapping::SetUpdatingInitPoses(bool flag)
{}
KeyFrame* LocalMapping::GetMapUpdateKF()
{}
bool LocalMapping::GetMapUpdateFlagForTracking()
{}
void LocalMapping::SetMapUpdateFlagInTracking(bool bflag)
{}
bool LocalMapping::GetVINSInited(void)
{}
void LocalMapping::SetVINSInited(bool flag)
{}
bool LocalMapping::GetFirstVINSInited(void)
{}
void LocalMapping::SetFirstVINSInited(bool flag)
{}
cv::Mat LocalMapping::GetGravityVec()
{}
cv::Mat LocalMapping::GetRwiInit()
{}
void LocalMapping::VINSInitThread()
{}
bool LocalMapping::TryInitVIO(void)
{}
void LocalMapping::AddToLocalWindow(KeyFrame* pKF)
{}
void LocalMapping::DeleteBadInLocalWindow(void)
{}在LocalMapping::LocalMapping这析构函数里添加了一些变量
mpParams = pParams;
mnLocalWindowSize = ConfigParam::GetLocalWindowSize();
cout<<"mnLocalWindowSize:"<Map.cc,后者删除了
void Map::InformNewBigChange()
{
unique_lock lock(mMutexMap);
mnBigChangeIdx++;
}
int Map::GetLastBigChangeIdx()
{
unique_lock lock(mMutexMap);
return mnBigChangeIdx;
} 增加了
bool KFIdComapre::operator ()(const KeyFrame* kfleft,const KeyFrame* kfright) const
{}
void Map::UpdateScale(const double &scale)
{}MapPoint.cc，后者添加了
bool cmpKeyFrameId::operator() (const KeyFrame* a, const KeyFrame* b) const {
}
void MapPoint::UpdateScale(float scale){
}
并对void MapPoint::EraseObservation(KeyFrame* pKF)添加了
KeyFrame* pKFrefnew = NULL;
for(auto ob : mObservations)
{
KeyFrame* pkfi = ob.first;
if(!pkfi->isBad())
{
pKFrefnew = pkfi;
break;
}
}Optimizer.cc后者很明显的添加了以下的头文件，说明IMU信息就是用来做优化
#include "IMU/configparam.h"
#include "IMU/g2otypes.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_cholmod.h"KeyFrameDatabase.cc没做修改
MapDrawer.cc没做修改
ORBextractor.cc没做修改
ORBmatcher.cc没做修改
PnPsolver.cc没做修改
Sim3Solver.cc没做修改