Skip to content

Commit 9edd50c

Browse files
DavidPetkovsekDavidPetkovsek
committed
Fix stereo_euroc example
1 parent be5bab7 commit 9edd50c

4 files changed

Lines changed: 120 additions & 115 deletions

File tree

‎Examples/Stereo/stereo_euroc.cc‎

Lines changed: 15 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -22,9 +22,10 @@
2222
#include<iomanip>
2323
#include<chrono>
2424

25-
#include<opencv2/core/core.hpp>
25+
#include<opencv2/opencv.hpp>
2626

2727
#include<System.h>
28+
#include<Viewer.h>
2829

2930
using namespace std;
3031

@@ -88,17 +89,20 @@ int main(int argc, char **argv)
8889
cout.precision(17);
8990

9091
// Create SLAM system. It initializes all system threads and gets ready to process frames.
91-
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::CameraType::STEREO, true);
92+
ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::STEREO);
93+
ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
9294

9395
cv::Mat imLeft, imRight;
9496
for (seq = 0; seq<num_seq; seq++)
9597
{
9698

9799
// Seq loop
100+
#ifdef REGISTER_TIMES
98101
double t_resize = 0;
99102
double t_rect = 0;
100103
double t_track = 0;
101104
int num_rect = 0;
105+
#endif
102106
int proccIm = 0;
103107
for(int ni=0; ni<nImages[seq]; ni++, proccIm++)
104108
{
@@ -125,10 +129,12 @@ int main(int argc, char **argv)
125129
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
126130

127131
// Pass the images to the SLAM system
128-
SLAM.TrackStereo(imLeft,imRight,tframe, vector<ORB_SLAM3::IMU::Point>(), vstrImageLeft[seq][ni]);
132+
auto pos = SLAM->TrackStereo(imLeft,imRight,tframe, vector<ORB_SLAM3::IMU::Point>(), vstrImageLeft[seq][ni]);
129133

130134
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
131135

136+
viewer.update(pos);
137+
132138
#ifdef REGISTER_TIMES
133139
t_track = t_resize + t_rect + std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t2 - t1).count();
134140
SLAM.InsertTrackTime(t_track);
@@ -153,25 +159,25 @@ int main(int argc, char **argv)
153159
{
154160
cout << "Changing the dataset" << endl;
155161

156-
SLAM.ChangeDataset();
162+
SLAM->ChangeDataset();
157163
}
158164

159165
}
160166
// Stop all threads
161-
SLAM.Shutdown();
167+
// SLAM.Shutdown();
162168

163169
// Save camera trajectory
164170
if (bFileName)
165171
{
166172
const string kf_file = "kf_" + string(argv[argc-1]) + ".txt";
167173
const string f_file = "f_" + string(argv[argc-1]) + ".txt";
168-
SLAM.SaveTrajectoryEuRoC(f_file);
169-
SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);
174+
SLAM->SaveTrajectoryEuRoC(f_file);
175+
SLAM->SaveKeyFrameTrajectoryEuRoC(kf_file);
170176
}
171177
else
172178
{
173-
SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");
174-
SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
179+
SLAM->SaveTrajectoryEuRoC("CameraTrajectory.txt");
180+
SLAM->SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
175181
}
176182

177183
return 0;

‎include/LoopClosing.h‎

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -17,9 +17,9 @@
1717
*/
1818

1919

20-
#ifndef LOOPCLOSING_H
21-
#define LOOPCLOSING_H
20+
#pragma once
2221

22+
#include "ImprovedTypes.hpp"
2323
#include "KeyFrame.h"
2424
#include "LocalMapping.h"
2525
#include "Atlas.h"
@@ -52,7 +52,7 @@ class LoopClosing
5252

5353
public:
5454

55-
LoopClosing(Atlas* pAtlas, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale, const bool bActiveLC);
55+
LoopClosing(const Atlas_ptr &pAtlas, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale, const bool bActiveLC);
5656

5757
void SetTracker(Tracking* pTracker);
5858

@@ -154,7 +154,7 @@ class LoopClosing
154154
bool mbFinished;
155155
std::mutex mMutexFinish;
156156

157-
Atlas* mpAtlas;
157+
Atlas_ptr mpAtlas;
158158
Tracking* mpTracker;
159159

160160
KeyFrameDatabase* mpKeyFrameDB;
@@ -243,4 +243,3 @@ class LoopClosing
243243

244244
} //namespace ORB_SLAM
245245

246-
#endif // LOOPCLOSING_H

‎src/LoopClosing.cc‎

Lines changed: 27 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@
2424
#include <mutex>
2525
#include <thread>
2626

27+
#include "ImprovedTypes.hpp"
2728
#include "Converter.h"
2829
#include "G2oTypes.h"
2930
#include "ORBmatcher.h"
@@ -32,7 +33,7 @@
3233

3334
namespace ORB_SLAM3 {
3435

35-
LoopClosing::LoopClosing(Atlas* pAtlas, KeyFrameDatabase* pDB,
36+
LoopClosing::LoopClosing(const Atlas_ptr &pAtlas, KeyFrameDatabase* pDB,
3637
ORBVocabulary* pVoc, const bool bFixScale,
3738
const bool bActiveLC)
3839
:
@@ -108,9 +109,9 @@ void LoopClosing::Run() {
108109
#endif
109110
if (bFindedRegion) {
110111
if (mbMergeDetected) {
111-
if ((mpTracker->mSensor == System::IMU_MONOCULAR ||
112-
mpTracker->mSensor == System::IMU_STEREO ||
113-
mpTracker->mSensor == System::IMU_RGBD) &&
112+
if ((mpTracker->mSensor == CameraType::IMU_MONOCULAR ||
113+
mpTracker->mSensor == CameraType::IMU_STEREO ||
114+
mpTracker->mSensor == CameraType::IMU_RGBD) &&
114115
(!mpCurrentKF->GetMap()->isImuInitialized())) {
115116
cout << "IMU is not initilized, merge is aborted" << endl;
116117
} else {
@@ -139,9 +140,9 @@ void LoopClosing::Run() {
139140
continue;
140141
}
141142
// If inertial, force only yaw
142-
if ((mpTracker->mSensor == System::IMU_MONOCULAR ||
143-
mpTracker->mSensor == System::IMU_STEREO ||
144-
mpTracker->mSensor == System::IMU_RGBD) &&
143+
if ((mpTracker->mSensor == CameraType::IMU_MONOCULAR ||
144+
mpTracker->mSensor == CameraType::IMU_STEREO ||
145+
mpTracker->mSensor == CameraType::IMU_RGBD) &&
145146
mpCurrentKF->GetMap()->GetIniertialBA1()) {
146147
Eigen::Vector3d phi =
147148
LogSO3(mSold_new.rotation().toRotationMatrix());
@@ -167,9 +168,9 @@ void LoopClosing::Run() {
167168
nMerges += 1;
168169
#endif
169170
// TODO UNCOMMENT
170-
if (mpTracker->mSensor == System::IMU_MONOCULAR ||
171-
mpTracker->mSensor == System::IMU_STEREO ||
172-
mpTracker->mSensor == System::IMU_RGBD)
171+
if (mpTracker->mSensor == CameraType::IMU_MONOCULAR ||
172+
mpTracker->mSensor == CameraType::IMU_STEREO ||
173+
mpTracker->mSensor == CameraType::IMU_RGBD)
173174
MergeLocal2();
174175
else
175176
MergeLocal();
@@ -235,9 +236,9 @@ void LoopClosing::Run() {
235236
fabs(phi(2)) < 0.349f) {
236237
if (mpCurrentKF->GetMap()->IsInertial()) {
237238
// If inertial, force only yaw
238-
if ((mpTracker->mSensor == System::IMU_MONOCULAR ||
239-
mpTracker->mSensor == System::IMU_STEREO ||
240-
mpTracker->mSensor == System::IMU_RGBD) &&
239+
if ((mpTracker->mSensor == CameraType::IMU_MONOCULAR ||
240+
mpTracker->mSensor == CameraType::IMU_STEREO ||
241+
mpTracker->mSensor == CameraType::IMU_RGBD) &&
241242
mpCurrentKF->GetMap()->GetIniertialBA2()) {
242243
phi(0) = 0;
243244
phi(1) = 0;
@@ -337,7 +338,7 @@ bool LoopClosing::NewDetectCommonRegions() {
337338
return false;
338339
}
339340

340-
if (mpTracker->mSensor == System::STEREO &&
341+
if (mpTracker->mSensor == CameraType::STEREO &&
341342
mpLastMap->GetAllKeyFrames().size() < 5) // 12
342343
{
343344
// cout << "LoopClousure: Stereo KF inserted without check: " <<
@@ -558,7 +559,7 @@ bool LoopClosing::DetectAndReffineSim3FromLastKF(
558559

559560
bool bFixedScale =
560561
mbFixScale; // TODO CHECK; Solo para el monocular inertial
561-
if (mpTracker->mSensor == System::IMU_MONOCULAR &&
562+
if (mpTracker->mSensor == CameraType::IMU_MONOCULAR &&
562563
!pCurrentKF->GetMap()->GetIniertialBA2())
563564
bFixedScale = false;
564565
int numOptMatches =
@@ -697,7 +698,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(
697698
{
698699
// Geometric validation
699700
bool bFixedScale = mbFixScale;
700-
if (mpTracker->mSensor == System::IMU_MONOCULAR &&
701+
if (mpTracker->mSensor == CameraType::IMU_MONOCULAR &&
701702
!mpCurrentKF->GetMap()->GetIniertialBA2())
702703
bFixedScale = false;
703704

@@ -781,7 +782,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(
781782

782783
/* The bool below ('bFixedScale') is UNUSED and since all function calls below do not change state. this is not needed.
783784
bool bFixedScale = mbFixScale;
784-
if(mpTracker->mSensor==System::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2())
785+
if(mpTracker->mSensor==CameraType::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2())
785786
bFixedScale=false;
786787
*/
787788

@@ -1181,7 +1182,7 @@ void LoopClosing::CorrectLoop() {
11811182
// Optimize graph
11821183
bool bFixedScale = mbFixScale;
11831184
// TODO CHECK; Solo para el monocular inertial
1184-
if (mpTracker->mSensor == System::IMU_MONOCULAR &&
1185+
if (mpTracker->mSensor == CameraType::IMU_MONOCULAR &&
11851186
!mpCurrentKF->GetMap()->GetIniertialBA2())
11861187
bFixedScale = false;
11871188

@@ -1642,9 +1643,9 @@ void LoopClosing::MergeLocal() {
16421643
std::back_inserter(vpLocalCurrentWindowKFs));
16431644
std::copy(spMergeConnectedKFs.begin(), spMergeConnectedKFs.end(),
16441645
std::back_inserter(vpMergeConnectedKFs));
1645-
if (mpTracker->mSensor == System::IMU_MONOCULAR ||
1646-
mpTracker->mSensor == System::IMU_STEREO ||
1647-
mpTracker->mSensor == System::IMU_RGBD) {
1646+
if (mpTracker->mSensor == CameraType::IMU_MONOCULAR ||
1647+
mpTracker->mSensor == CameraType::IMU_STEREO ||
1648+
mpTracker->mSensor == CameraType::IMU_RGBD) {
16481649
Optimizer::MergeInertialBA(mpCurrentKF, mpMergeMatchedKF, &bStop,
16491650
pCurrentMap, vCorrectedSim3);
16501651
} else {
@@ -1673,7 +1674,7 @@ void LoopClosing::MergeLocal() {
16731674

16741675
if (vpCurrentMapKFs.size() == 0) {
16751676
} else {
1676-
if (mpTracker->mSensor == System::MONOCULAR) {
1677+
if (mpTracker->mSensor == CameraType::MONOCULAR) {
16771678
unique_lock<mutex> currentLock(
16781679
pCurrentMap->mMutexMapUpdate); // We update the current map with the
16791680
// Merge information
@@ -1742,7 +1743,7 @@ void LoopClosing::MergeLocal() {
17421743

17431744
// Optimize graph (and update the loop position for each element form the
17441745
// begining to the end)
1745-
if (mpTracker->mSensor != System::MONOCULAR) {
1746+
if (mpTracker->mSensor != CameraType::MONOCULAR) {
17461747
Optimizer::OptimizeEssentialGraph(mpCurrentKF, vpMergeConnectedKFs,
17471748
vpLocalCurrentWindowKFs,
17481749
vpCurrentMapKFs, vpCurrentMapMPs);
@@ -1890,9 +1891,9 @@ void LoopClosing::MergeLocal2() {
18901891

18911892
const int numKFnew = pCurrentMap->KeyFramesInMap();
18921893

1893-
if ((mpTracker->mSensor == System::IMU_MONOCULAR ||
1894-
mpTracker->mSensor == System::IMU_STEREO ||
1895-
mpTracker->mSensor == System::IMU_RGBD) &&
1894+
if ((mpTracker->mSensor == CameraType::IMU_MONOCULAR ||
1895+
mpTracker->mSensor == CameraType::IMU_STEREO ||
1896+
mpTracker->mSensor == CameraType::IMU_RGBD) &&
18961897
!pCurrentMap->GetIniertialBA2()) {
18971898
// Map is not completly initialized
18981899
Eigen::Vector3d bg, ba;

0 commit comments

Comments
 (0)