Skip to content

Commit

Permalink
fix some warnings
Browse files Browse the repository at this point in the history
  • Loading branch information
@DavidPetkovsek
DavidPetkovsek committed Jul 20, 2022
1 parent 71c627f commit c8cd905
Show file tree
Hide file tree
Showing 6 changed files with 86 additions and 105 deletions.
104 changes: 52 additions & 52 deletions include/Settings.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ class Settings {
/*
* Getter methods
*/
CameraType cameraType() { return cameraType_; }
CameraType cameraType() const { return cameraType_; }
GeometricCamera* camera1() { return calibration1_; }
GeometricCamera* camera2() { return calibration2_; }
cv::Mat camera1DistortionCoef() {
Expand All @@ -74,59 +74,59 @@ class Settings {
}
cv::Mat camera2DistortionCoef() {
return cv::Mat(vPinHoleDistorsion2_.size(), 1, CV_32F,
vPinHoleDistorsion1_.data());
vPinHoleDistorsion2_.data());
}

Sophus::SE3f Tlr() { return Tlr_; }
float bf() { return bf_; }
float b() { return b_; }
float thDepth() { return thDepth_; }

bool needToUndistort() { return bNeedToUndistort_; }

cv::Size newImSize() { return newImSize_; }
float fps() { return fps_; }
bool rgb() { return bRGB_; }
bool needToResize() { return bNeedToResize1_; }
bool needToRectify() { return bNeedToRectify_; }

float noiseGyro() { return noiseGyro_; }
float noiseAcc() { return noiseAcc_; }
float gyroWalk() { return gyroWalk_; }
float accWalk() { return accWalk_; }
float imuFrequency() { return imuFrequency_; }
Sophus::SE3f Tbc() { return Tbc_; }
bool insertKFsWhenLost() { return insertKFsWhenLost_; }

float depthMapFactor() { return depthMapFactor_; }

int nFeatures() { return nFeatures_; }
int nLevels() { return nLevels_; }
float initThFAST() { return initThFAST_; }
float minThFAST() { return minThFAST_; }
float scaleFactor() { return scaleFactor_; }

float keyFrameSize() { return keyFrameSize_; }
float keyFrameLineWidth() { return keyFrameLineWidth_; }
float graphLineWidth() { return graphLineWidth_; }
float pointSize() { return pointSize_; }
float cameraSize() { return cameraSize_; }
float cameraLineWidth() { return cameraLineWidth_; }
float viewPointX() { return viewPointX_; }
float viewPointY() { return viewPointY_; }
float viewPointZ() { return viewPointZ_; }
float viewPointF() { return viewPointF_; }
float imageViewerScale() { return imageViewerScale_; }

std::string atlasLoadFile() { return sLoadFrom_; }
std::string atlasSaveFile() { return sSaveto_; }

float thFarPoints() { return thFarPoints_; }

cv::Mat M1l() { return M1l_; }
cv::Mat M2l() { return M2l_; }
cv::Mat M1r() { return M1r_; }
cv::Mat M2r() { return M2r_; }
const Sophus::SE3f &Tlr() const { return Tlr_; }
float bf() const { return bf_; }
float b() const { return b_; }
float thDepth() const { return thDepth_; }

bool needToUndistort() const { return bNeedToUndistort_; }

const cv::Size &newImSize() const { return newImSize_; }
float fps() const { return fps_; }
bool rgb() const { return bRGB_; }
bool needToResize() const { return bNeedToResize1_; }
bool needToRectify() const { return bNeedToRectify_; }

float noiseGyro() const { return noiseGyro_; }
float noiseAcc() const { return noiseAcc_; }
float gyroWalk() const { return gyroWalk_; }
float accWalk() const { return accWalk_; }
float imuFrequency() const { return imuFrequency_; }
const Sophus::SE3f &Tbc() const { return Tbc_; }
bool insertKFsWhenLost() const { return insertKFsWhenLost_; }

float depthMapFactor() const { return depthMapFactor_; }

int nFeatures() const { return nFeatures_; }
int nLevels() const { return nLevels_; }
float initThFAST() const { return initThFAST_; }
float minThFAST() const { return minThFAST_; }
float scaleFactor() const { return scaleFactor_; }

float keyFrameSize() const { return keyFrameSize_; }
float keyFrameLineWidth() const { return keyFrameLineWidth_; }
float graphLineWidth() const { return graphLineWidth_; }
float pointSize() const { return pointSize_; }
float cameraSize() const { return cameraSize_; }
float cameraLineWidth() const { return cameraLineWidth_; }
float viewPointX() const { return viewPointX_; }
float viewPointY() const { return viewPointY_; }
float viewPointZ() const { return viewPointZ_; }
float viewPointF() const { return viewPointF_; }
float imageViewerScale() const { return imageViewerScale_; }

const std::string &atlasLoadFile() const { return sLoadFrom_; }
const std::string &atlasSaveFile() const { return sSaveto_; }

float thFarPoints() const { return thFarPoints_; }

const cv::Mat &M1l() const { return M1l_; }
const cv::Mat &M2l() const { return M2l_; }
const cv::Mat &M1r() const { return M1r_; }
const cv::Mat &M2r() const { return M2r_; }

private:
template <typename T>
Expand Down
8 changes: 1 addition & 7 deletions include/System.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,7 @@
*/


#ifndef SYSTEM_H
#define SYSTEM_H
#pragma once


#include <unistd.h>
Expand Down Expand Up @@ -226,9 +225,6 @@ class System
bool mbActivateLocalizationMode;
bool mbDeactivateLocalizationMode;

// Shutdown flag
bool mbShutDown;

// Tracking state
int mTrackingState;
std::vector<MapPoint*> mTrackedMapPoints;
Expand All @@ -245,5 +241,3 @@ class System
};

}// namespace ORB_SLAM

#endif // SYSTEM_H
2 changes: 1 addition & 1 deletion src/Atlas.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -206,7 +206,7 @@ Map* Atlas::GetCurrentMap(System* sys) {
unique_lock<mutex> lock(mMutexAtlas);
if (!mpCurrentMap) CreateNewMap();
while (mpCurrentMap->IsBad()) {
if (sys != nullptr && sys->isShutDown()) return nullptr;
if (sys != nullptr) return nullptr;
}

return mpCurrentMap;
Expand Down
50 changes: 23 additions & 27 deletions src/LoopClosing.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,9 +35,9 @@ namespace ORB_SLAM3

LoopClosing::LoopClosing(Atlas *pAtlas, KeyFrameDatabase *pDB, ORBVocabulary *pVoc, const bool bFixScale, const bool bActiveLC):
mbResetRequested(false), mbResetActiveMapRequested(false), mbFinishRequested(false), mbFinished(true), mpAtlas(pAtlas),
mpKeyFrameDB(pDB), mpORBVocabulary(pVoc), mpMatchedKF(NULL), mbRunningGBA(false), mbFinishedGBA(true),
mbStopGBA(false), mpThreadGBA(NULL), mbFixScale(bFixScale), mnFullBAIdx(0), mnLoopNumCoincidences(0), mnMergeNumCoincidences(0),
mbLoopDetected(false), mbMergeDetected(false), mnLoopNumNotFound(0), mnMergeNumNotFound(0), mbActiveLC(bActiveLC)
mpKeyFrameDB(pDB), mpORBVocabulary(pVoc), mpMatchedKF(NULL), mbLoopDetected(false), mnLoopNumCoincidences(0), mnLoopNumNotFound(0),
mbMergeDetected(false), mnMergeNumCoincidences(0), mnMergeNumNotFound(0), mbRunningGBA(false), mbFinishedGBA(true),
mbStopGBA(false), mpThreadGBA(NULL), mbFixScale(bFixScale), mnFullBAIdx(0), mbActiveLC(bActiveLC)
{
mnCovisibilityConsistencyTh = 3;
mpLastCurrentKF = static_cast<KeyFrame*>(NULL);
Expand Down Expand Up @@ -134,7 +134,7 @@ void LoopClosing::Run()
Sophus::SE3d mTcw = mpCurrentKF->GetPose().cast<double>();
g2o::Sim3 gScw1(mTcw.unit_quaternion(), mTcw.translation(), 1.0);
g2o::Sim3 gSw2c = mg2oMergeSlw.inverse();
g2o::Sim3 gSw1m = mg2oMergeSlw;
// g2o::Sim3 gSw1m = mg2oMergeSlw; // UNUSED

mSold_new = (gSw2c * gScw1);

Expand Down Expand Up @@ -365,14 +365,14 @@ bool LoopClosing::NewDetectCommonRegions()
//Check the last candidates with geometric validation
// Loop candidates
bool bLoopDetectedInKF = false;
bool bCheckSpatial = false;
// bool bCheckSpatial = false; // UNUSED

#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_StartEstSim3_1 = std::chrono::steady_clock::now();
#endif
if(mnLoopNumCoincidences > 0)
{
bCheckSpatial = true;
// bCheckSpatial = true; // UNUSED
// Find from the last KF candidates
Sophus::SE3d mTcl = (mpCurrentKF->GetPose() * mpLoopLastCurrentKF->GetPoseInverse()).cast<double>();
g2o::Sim3 gScl(mTcl.unit_quaternion(),mTcl.translation(),1.0);
Expand Down Expand Up @@ -625,7 +625,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,


bool bAbortByNearKF = false;
for(int j=0; j<vpCovKFi.size(); ++j)
for(size_t j=0; j<vpCovKFi.size(); ++j)
{
if(spConnectedKeyFrames.find(vpCovKFi[j]) != spConnectedKeyFrames.end())
{
Expand Down Expand Up @@ -653,7 +653,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
std::vector<KeyFrame*> vpKeyFrameMatchedMP = std::vector<KeyFrame*>(mpCurrentKF->GetMapPointMatches().size(), static_cast<KeyFrame*>(NULL));

int nIndexMostBoWMatchesKF=0;
for(int j=0; j<vpCovKFi.size(); ++j)
for(size_t j=0; j<vpCovKFi.size(); ++j)
{
if(!vpCovKFi[j] || vpCovKFi[j]->isBad())
continue;
Expand All @@ -666,9 +666,9 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
}
}

for(int j=0; j<vpCovKFi.size(); ++j)
for(size_t j=0; j<vpCovKFi.size(); ++j)
{
for(int k=0; k < vvpMatchedMPs[j].size(); ++k)
for(size_t k=0; k < vvpMatchedMPs[j].size(); ++k)
{
MapPoint* pMPi_j = vvpMatchedMPs[j][k];
if(!pMPi_j || pMPi_j->isBad())
Expand Down Expand Up @@ -759,9 +759,9 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
// Optimize Sim3 transformation with every matches
Eigen::Matrix<double, 7, 7> mHessian7x7;

bool bFixedScale = mbFixScale;
if(mpTracker->mSensor==CameraType::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2())
bFixedScale=false;
// bool bFixedScale = mbFixScale; // UNUSED
// if(mpTracker->mSensor==CameraType::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2())
// bFixedScale=false; // UNUSED

int numOptMatches = Optimizer::OptimizeSim3(mpCurrentKF, pKFi, vpMatchedMP, gScm, 10, mbFixScale, mHessian7x7, true);

Expand Down Expand Up @@ -817,8 +817,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
// Check the Sim3 transformation with the current KeyFrame covisibles
vector<KeyFrame*> vpCurrentCovKFs = mpCurrentKF->GetBestCovisibilityKeyFrames(nNumCovisibles);

int j = 0;
while(nNumKFs < 3 && j<vpCurrentCovKFs.size())
for(size_t j = 0; nNumKFs < 3 && j<vpCurrentCovKFs.size(); ++j)
{
KeyFrame* pKFj = vpCurrentCovKFs[j];
Sophus::SE3d mTjc = (pKFj->GetPose() * mpCurrentKF->GetPoseInverse()).cast<double>();
Expand All @@ -830,13 +829,12 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,

if(bValid)
{
Sophus::SE3f Tc_w = mpCurrentKF->GetPose();
Sophus::SE3f Tw_cj = pKFj->GetPoseInverse();
Sophus::SE3f Tc_cj = Tc_w * Tw_cj;
Eigen::Vector3f vector_dist = Tc_cj.translation();
// Sophus::SE3f Tc_w = mpCurrentKF->GetPose();
// Sophus::SE3f Tw_cj = pKFj->GetPoseInverse();
// Sophus::SE3f Tc_cj = Tc_w * Tw_cj;
// Eigen::Vector3f vector_dist = Tc_cj.translation();
nNumKFs++;
}
j++;
}

if(nNumKFs < 3)
Expand Down Expand Up @@ -881,13 +879,13 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
else
{
int maxStage = -1;
int maxMatched;
for(int i=0; i<vnStage.size(); ++i)
// int maxMatched; // UNUSED
for(size_t i=0; i<vnStage.size(); ++i)
{
if(vnStage[i] > maxStage)
{
maxStage = vnStage[i];
maxMatched = vnMatchesStage[i];
// maxMatched = vnMatchesStage[i]; // UNUSED
}
}
}
Expand Down Expand Up @@ -925,15 +923,13 @@ int LoopClosing::FindMatchesByProjection(KeyFrame* pCurrentKF, KeyFrame* pMatche
{
vector<KeyFrame*> vpKFs = vpCovKFm[i]->GetBestCovisibilityKeyFrames(nNumCovisibles);
int nInserted = 0;
int j = 0;
while(j < vpKFs.size() && nInserted < nNumCovisibles)
for(size_t j = 0; j < vpKFs.size() && nInserted < nNumCovisibles; ++j)
{
if(spCheckKFs.find(vpKFs[j]) == spCheckKFs.end() && spCurrentCovisbles.find(vpKFs[j]) == spCurrentCovisbles.end())
{
spCheckKFs.insert(vpKFs[j]);
++nInserted;
}
++j;
}
vpCovKFm.insert(vpCovKFm.end(), vpKFs.begin(), vpKFs.end());
}
Expand Down Expand Up @@ -1306,7 +1302,7 @@ void LoopClosing::MergeLocal()
spLocalWindowKFs.insert(mpCurrentKF);
const int nMaxTries = 5;
int nNumTries = 0;
while(spLocalWindowKFs.size() < numTemporalKFs && nNumTries < nMaxTries)
while(static_cast<int>(spLocalWindowKFs.size()) < numTemporalKFs && nNumTries < nMaxTries)
{
vector<KeyFrame*> vpNewCovKFs;
vpNewCovKFs.empty();
Expand Down
18 changes: 5 additions & 13 deletions src/System.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,8 +52,7 @@ System::System(const string& strVocFile, const string& strSettingsFile,
mbReset(false),
mbResetActiveMap(false),
mbActivateLocalizationMode(false),
mbDeactivateLocalizationMode(false),
mbShutDown(false) {
mbDeactivateLocalizationMode(false) {
// Output welcome message
cout << endl
<< "ORB-SLAM3 Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, "
Expand Down Expand Up @@ -389,10 +388,10 @@ Sophus::SE3f System::TrackRGBD(const cv::Mat& im, const cv::Mat& depthmap,
Sophus::SE3f System::TrackMonocular(const cv::Mat& im, const double& timestamp,
const vector<IMU::Point>& vImuMeas,
string filename) {
{
unique_lock<mutex> lock(mMutexReset);
if (mbShutDown) return Sophus::SE3f();
}
// {
// unique_lock<mutex> lock(mMutexReset);
// if (mbShutDown) return Sophus::SE3f();
// }

if (mSensor != CameraType::MONOCULAR && mSensor != CameraType::IMU_MONOCULAR) {
cerr << "ERROR: you called TrackMonocular but input sensor was not set to "
Expand Down Expand Up @@ -525,13 +524,6 @@ System::~System() {
#ifdef REGISTER_TIMES
mpTracker->PrintTimeStats();
#endif

mbShutDown = true;
}

bool System::isShutDown() {
unique_lock<mutex> lock(mMutexReset);
return mbShutDown;
}

void System::SaveTrajectoryTUM(const string& filename) {
Expand Down
9 changes: 4 additions & 5 deletions src/Tracking.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,8 +39,7 @@ using namespace std;

namespace ORB_SLAM3 {

Tracking::Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer,
MapDrawer* pMapDrawer, Atlas* pAtlas,
Tracking::Tracking(System* pSys, ORBVocabulary* pVoc, Atlas* pAtlas,
KeyFrameDatabase* pKFDB, const string& strSettingPath,
const int sensor, Settings* settings, const string& _nameSeq)
: mState(Tracker::NO_IMAGES_YET),
Expand Down Expand Up @@ -1705,9 +1704,9 @@ void Tracking::Track() {

Map* pCurrentMap = mpAtlas->GetCurrentMap(mpSystem);
if (!pCurrentMap) {
if (!mpSystem->isShutDown()){
cout << "ERROR: There is not an active map in the atlas" << endl;
}
// if (!mpSystem->isShutDown()){
// cout << "ERROR: There is not an active map in the atlas" << endl;
// }
return;
}

Expand Down

0 comments on commit c8cd905

Please sign in to comment.