Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
90 changes: 51 additions & 39 deletions Examples/Monocular/vslamlab_orbslam2_mono.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2,11 +2,11 @@
#include<algorithm>
#include<fstream>
#include<chrono>
#include<thread>
#include<opencv2/core/core.hpp>
#include <yaml-cpp/yaml.h>

#include "sys/types.h"
#include "sys/sysinfo.h"
#include <unistd.h>

#include<System.h>

Expand Down Expand Up @@ -116,51 +116,63 @@ int main(int argc, char **argv)
cout << "Start processing sequence ..." << endl;
cout << "Images in the sequence: " << nImages << endl << endl;

// Main loop
cv::Mat im;
for(size_t ni = 0; ni < nImages; ni++)
{
// Read image from file
im = cv::imread(imageFilenames[ni],cv::IMREAD_UNCHANGED);
ORB_SLAM2::Seconds tframe = timestamps[ni];
auto processSequence = [&]() {
cv::Mat im;
for(size_t ni = 0; ni < nImages; ni++)
{
// Read image from file
im = cv::imread(imageFilenames[ni],cv::IMREAD_UNCHANGED);
ORB_SLAM2::Seconds tframe = timestamps[ni];

// Pass the image to the SLAM system
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
SLAM.TrackMonocular(im,tframe);
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
// Pass the image to the SLAM system
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
SLAM.TrackMonocular(im,tframe);
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();

ORB_SLAM2::Seconds ttrack = std::chrono::duration_cast<std::chrono::duration<ORB_SLAM2::Seconds> >(t2 - t1).count();
vTimesTrack[ni] = ttrack;
ORB_SLAM2::Seconds ttrack = std::chrono::duration_cast<std::chrono::duration<ORB_SLAM2::Seconds> >(t2 - t1).count();
vTimesTrack[ni] = ttrack;

// Wait to load the next frame
ORB_SLAM2::Seconds T = 0.0;
if(ni < nImages-1)
T = timestamps[ni+1] - tframe;
else if(ni > 0)
T = tframe - timestamps[ni-1];
// Wait to load the next frame
ORB_SLAM2::Seconds T = 0.0;
if(ni < nImages-1)
T = timestamps[ni+1] - tframe;
else if(ni > 0)
T = tframe - timestamps[ni-1];

if(ttrack < T)
usleep((T-ttrack) * 1e6);
if(ttrack < T)
usleep((T-ttrack) * 1e6);

}

// Stop all threads
SLAM.Shutdown();
}

// Stop all threads
SLAM.Shutdown();

// Tracking time statistics
sort(vTimesTrack.begin(),vTimesTrack.end());
ORB_SLAM2::Seconds totaltime = 0.0;
for(int ni = 0; ni < nImages; ni++)
{
totaltime+=vTimesTrack[ni];
}
cout << "-------" << endl << endl;
cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
cout << "mean tracking time: " << totaltime/nImages << endl;

// Tracking time statistics
sort(vTimesTrack.begin(),vTimesTrack.end());
ORB_SLAM2::Seconds totaltime = 0.0;
for(int ni = 0; ni < nImages; ni++)
// Save camera trajectory
string resultsPath_expId = exp_folder + "/" + paddingZeros(exp_id);
SLAM.SaveKeyFrameTrajectoryVSLAMLAB(resultsPath_expId + "_" + "KeyFrameTrajectory.csv");
};

if(SLAM.ViewerRunsOnMainThread())
{
totaltime+=vTimesTrack[ni];
std::thread trackingThread(processSequence);
SLAM.RunViewer();
trackingThread.join();
}
else
{
processSequence();
}
cout << "-------" << endl << endl;
cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
cout << "mean tracking time: " << totaltime/nImages << endl;

// Save camera trajectory
string resultsPath_expId = exp_folder + "/" + paddingZeros(exp_id);
SLAM.SaveKeyFrameTrajectoryVSLAMLAB(resultsPath_expId + "_" + "KeyFrameTrajectory.csv");

return 0;
}
Expand Down
92 changes: 52 additions & 40 deletions Examples/RGB-D/vslamlab_orbslam2_rgbd.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2,10 +2,10 @@
#include<algorithm>
#include<fstream>
#include<chrono>
#include<thread>
#include<opencv2/core/core.hpp>

#include "sys/types.h"
#include "sys/sysinfo.h"
#include <unistd.h>

#include<System.h>

Expand Down Expand Up @@ -127,52 +127,64 @@ int main(int argc, char **argv)
cout << "Start processing sequence ..." << endl;
cout << "Images in the sequence: " << nImages << endl << endl;

// Main loop
cv::Mat im, imD;
for(size_t ni = 0; ni < nImages; ni++)
{
// Read image from file
im = cv::imread(imageFilenames[ni],cv::IMREAD_UNCHANGED);
imD = cv::imread(depthFilenames[ni],cv::IMREAD_UNCHANGED);
ORB_SLAM2::Seconds tframe = timestamps[ni];
auto processSequence = [&]() {
cv::Mat im, imD;
for(size_t ni = 0; ni < nImages; ni++)
{
// Read image from file
im = cv::imread(imageFilenames[ni],cv::IMREAD_UNCHANGED);
imD = cv::imread(depthFilenames[ni],cv::IMREAD_UNCHANGED);
ORB_SLAM2::Seconds tframe = timestamps[ni];

// Pass the image to the SLAM system
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
SLAM.TrackRGBD(im,imD,tframe);
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();

ORB_SLAM2::Seconds ttrack = std::chrono::duration_cast<std::chrono::duration<ORB_SLAM2::Seconds> >(t2 - t1).count();
vTimesTrack[ni] = ttrack;

// Wait to load the next frame
ORB_SLAM2::Seconds T = 0.0;
if(ni < nImages-1)
T = timestamps[ni+1] - tframe;
else if(ni > 0)
T = tframe - timestamps[ni-1];

if(ttrack < T)
usleep((T-ttrack) * 1e6);

// Pass the image to the SLAM system
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
SLAM.TrackRGBD(im,imD,tframe);
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
}

ORB_SLAM2::Seconds ttrack = std::chrono::duration_cast<std::chrono::duration<ORB_SLAM2::Seconds> >(t2 - t1).count();
vTimesTrack[ni] = ttrack;
// Stop all threads
SLAM.Shutdown();

// Wait to load the next frame
ORB_SLAM2::Seconds T = 0.0;
if(ni < nImages-1)
T = timestamps[ni+1] - tframe;
else if(ni > 0)
T = tframe - timestamps[ni-1];
// Tracking time statistics
sort(vTimesTrack.begin(),vTimesTrack.end());
ORB_SLAM2::Seconds totaltime = 0.0;
for(int ni = 0; ni < nImages; ni++)
{
totaltime+=vTimesTrack[ni];
}
cout << "-------" << endl << endl;
cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
cout << "mean tracking time: " << totaltime/nImages << endl;

if(ttrack < T)
usleep((T-ttrack) * 1e6);
// Save camera trajectory
string resultsPath_expId = exp_folder + "/" + paddingZeros(exp_id);
SLAM.SaveKeyFrameTrajectoryVSLAMLAB(resultsPath_expId + "_" + "KeyFrameTrajectory.csv");
};

if(SLAM.ViewerRunsOnMainThread())
{
std::thread trackingThread(processSequence);
SLAM.RunViewer();
trackingThread.join();
}

// Stop all threads
SLAM.Shutdown();

// Tracking time statistics
sort(vTimesTrack.begin(),vTimesTrack.end());
ORB_SLAM2::Seconds totaltime = 0.0;
for(int ni = 0; ni < nImages; ni++)
else
{
totaltime+=vTimesTrack[ni];
processSequence();
}
cout << "-------" << endl << endl;
cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
cout << "mean tracking time: " << totaltime/nImages << endl;

// Save camera trajectory
string resultsPath_expId = exp_folder + "/" + paddingZeros(exp_id);
SLAM.SaveKeyFrameTrajectoryVSLAMLAB(resultsPath_expId + "_" + "KeyFrameTrajectory.csv");

return 0;
}
Expand Down
102 changes: 57 additions & 45 deletions Examples/Stereo/vslamlab_orbslam2_stereo.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2,10 +2,10 @@
#include<algorithm>
#include<fstream>
#include<chrono>
#include<thread>
#include<opencv2/core/core.hpp>

#include "sys/types.h"
#include "sys/sysinfo.h"
#include <unistd.h>

#include<System.h>

Expand Down Expand Up @@ -122,57 +122,69 @@ int main(int argc, char **argv)
cout << "Start processing sequence ..." << endl;
cout << "Images in the sequence: " << nImages << endl << endl;

// Main loop
cv::Mat imLeft, imRight, imLeftRect, imRightRect;
cv::Mat M1l, M2l, M1r, M2r;
SLAM.getStereoRectification(M1l, M2l, M1r, M2r);
for(size_t ni = 0; ni < nImages; ni++)
{
// Read image from file
imLeft = cv::imread(imageFilenames_l[ni], cv::IMREAD_UNCHANGED);
imRight = cv::imread(imageFilenames_r[ni], cv::IMREAD_UNCHANGED);
cv::remap(imLeft, imLeftRect, M1l, M2l, cv::INTER_LINEAR);
cv::remap(imRight, imRightRect, M1r, M2r, cv::INTER_LINEAR);

ORB_SLAM2::Seconds tframe = timestamps[ni];
auto processSequence = [&]() {
cv::Mat imLeft, imRight, imLeftRect, imRightRect;
cv::Mat M1l, M2l, M1r, M2r;
SLAM.getStereoRectification(M1l, M2l, M1r, M2r);
for(size_t ni = 0; ni < nImages; ni++)
{
// Read image from file
imLeft = cv::imread(imageFilenames_l[ni], cv::IMREAD_UNCHANGED);
imRight = cv::imread(imageFilenames_r[ni], cv::IMREAD_UNCHANGED);
cv::remap(imLeft, imLeftRect, M1l, M2l, cv::INTER_LINEAR);
cv::remap(imRight, imRightRect, M1r, M2r, cv::INTER_LINEAR);

ORB_SLAM2::Seconds tframe = timestamps[ni];

// Pass the image to the SLAM system
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
SLAM.TrackStereo(imLeftRect,imRightRect,tframe);
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();

ORB_SLAM2::Seconds ttrack = std::chrono::duration_cast<std::chrono::duration<ORB_SLAM2::Seconds> >(t2 - t1).count();
vTimesTrack[ni] = ttrack;

// Wait to load the next frame
ORB_SLAM2::Seconds T = 0.0;
if(ni < nImages-1)
T = timestamps[ni+1] - tframe;
else if(ni > 0)
T = tframe - timestamps[ni-1];

if(ttrack < T)
usleep((T-ttrack) * 1e6);

// Pass the image to the SLAM system
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
SLAM.TrackStereo(imLeftRect,imRightRect,tframe);
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
}

ORB_SLAM2::Seconds ttrack = std::chrono::duration_cast<std::chrono::duration<ORB_SLAM2::Seconds> >(t2 - t1).count();
vTimesTrack[ni] = ttrack;
// Stop all threads
SLAM.Shutdown();

// Wait to load the next frame
ORB_SLAM2::Seconds T = 0.0;
if(ni < nImages-1)
T = timestamps[ni+1] - tframe;
else if(ni > 0)
T = tframe - timestamps[ni-1];
// Tracking time statistics
sort(vTimesTrack.begin(),vTimesTrack.end());
ORB_SLAM2::Seconds totaltime = 0.0;
for(int ni = 0; ni < nImages; ni++)
{
totaltime+=vTimesTrack[ni];
}
cout << "-------" << endl << endl;
cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
cout << "mean tracking time: " << totaltime/nImages << endl;

if(ttrack < T)
usleep((T-ttrack) * 1e6);
// Save camera trajectory
string resultsPath_expId = exp_folder + "/" + paddingZeros(exp_id);
SLAM.SaveKeyFrameTrajectoryVSLAMLAB(resultsPath_expId + "_" + "KeyFrameTrajectory.csv");
};

if(SLAM.ViewerRunsOnMainThread())
{
std::thread trackingThread(processSequence);
SLAM.RunViewer();
trackingThread.join();
}

// Stop all threads
SLAM.Shutdown();

// Tracking time statistics
sort(vTimesTrack.begin(),vTimesTrack.end());
ORB_SLAM2::Seconds totaltime = 0.0;
for(int ni = 0; ni < nImages; ni++)
else
{
totaltime+=vTimesTrack[ni];
processSequence();
}
cout << "-------" << endl << endl;
cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
cout << "mean tracking time: " << totaltime/nImages << endl;

// Save camera trajectory
string resultsPath_expId = exp_folder + "/" + paddingZeros(exp_id);
SLAM.SaveKeyFrameTrajectoryVSLAMLAB(resultsPath_expId + "_" + "KeyFrameTrajectory.csv");

return 0;
}
Expand Down
3 changes: 1 addition & 2 deletions Thirdparty/DBoW2/DBoW2/FORB.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
#include <vector>
#include <string>
#include <sstream>
#include <stdint-gcc.h>
#include <cstdint>

#include "FORB.h"

Expand Down Expand Up @@ -190,4 +190,3 @@ void FORB::toMat8U(const std::vector<TDescriptor> &descriptors,

} // namespace DBoW2


6 changes: 5 additions & 1 deletion Thirdparty/g2o/g2o/core/estimate_propagator.h
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@
#include <set>
#include <limits>

#ifdef _MSC_VER
#if defined(_MSC_VER) || defined(__APPLE__)
#include <unordered_map>
#else
#include <tr1/unordered_map>
Expand Down Expand Up @@ -135,7 +135,11 @@ namespace g2o {
size_t operator ()(const OptimizableGraph::Vertex* v) const { return v->id();}
};

#if defined(_MSC_VER) || defined(__APPLE__)
typedef std::unordered_map<OptimizableGraph::Vertex*, AdjacencyMapEntry, VertexIDHashFunction> AdjacencyMap;
#else
typedef std::tr1::unordered_map<OptimizableGraph::Vertex*, AdjacencyMapEntry, VertexIDHashFunction> AdjacencyMap;
#endif

public:
EstimatePropagator(OptimizableGraph* g);
Expand Down
Loading