Skip to content

Commit

Permalink
Viewer Update (#74)
Browse files Browse the repository at this point in the history 
* Remove unused variable

* July 15 update

* Decouple drawing header files

* Reduce viewer coupling

* Viewer is now not a dependency of any system component

* Move enum out of system

* Remove unused member variables and local variables

* fix some warnings

* Start moving atlas to be Atlas_ptr

* Fix stereo_euroc example

* Fix all examples, resolves #55

Co-authored-by: Soldann <[email protected]>
  • Loading branch information
@DavidPetkovsek @Soldann
DavidPetkovsek and Soldann authored Jul 21, 2022
1 parent 635dcba commit e2d034b
Show file tree
Hide file tree
Showing 45 changed files with 1,065 additions and 1,205 deletions.
9 changes: 5 additions & 4 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ project(MORB_SLAM VERSION 1.0 LANGUAGES CXX)
# SET(CMAKE_BUILD_TYPE Release)
# ENDIF()

MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})
MESSAGE(STATUS "Build type: " ${CMAKE_BUILD_TYPE})

set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3")
Expand All @@ -29,8 +29,8 @@ find_package(OpenCV 4.4)
message(FATAL_ERROR "OpenCV > 4.4 not found.")
endif()

MESSAGE("OPENCV VERSION:")
MESSAGE(${OpenCV_VERSION})
MESSAGE(STATUS "OPENCV VERSION:")
MESSAGE(STATUS ${OpenCV_VERSION})

find_package(Eigen3 3.1.0 REQUIRED)
find_package(Pangolin REQUIRED)
Expand Down Expand Up @@ -219,7 +219,7 @@ if (CMAKE_BUILD_TYPE MATCHES Debug OR CMAKE_BUILD_TYPE MATCHES RelWithDebInfo)
target_link_libraries(rgbd_inertial_realsense_D435i ${PROJECT_NAME})
endif()

#Stereo examples
# #Stereo examples
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo)

add_executable(stereo_kitti
Expand Down Expand Up @@ -315,6 +315,7 @@ if (CMAKE_BUILD_TYPE MATCHES Debug OR CMAKE_BUILD_TYPE MATCHES RelWithDebInfo)
target_link_libraries(stereo_inertial_realsense_D435i ${PROJECT_NAME})
endif()

# Calibration examples
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Calibration)
if(realsense2_FOUND)
add_executable(recorder_realsense_D435i
Expand Down
7 changes: 3 additions & 4 deletions Examples/Calibration/recorder_realsense_D435i.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,7 @@

#include <condition_variable>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>

#include <librealsense2/rs.hpp>
#include "librealsense2/rsutil.h"
Expand Down Expand Up @@ -267,11 +266,11 @@ int main(int argc, char **argv) {
//assert(vAccel.size() == vAccel_times.size());
//assert(vGyro.size() == vGyro_times.size());

for(int i=0; i<vAccel.size(); ++i){
for(size_t i=0; i<vAccel.size(); ++i){
accFile << std::setprecision(15) << vAccel_times[i] << "," << vAccel[i].x << "," << vAccel[i].y << "," << vAccel[i].z << endl;
}

for(int i=0; i<vGyro.size(); ++i){
for(size_t i=0; i<vGyro.size(); ++i){
gyroFile << std::setprecision(15) << vGyro_times[i] << "," << vGyro[i].x << "," << vGyro[i].y << "," << vGyro[i].z << endl;
}

Expand Down
9 changes: 5 additions & 4 deletions Examples/Calibration/recorder_realsense_T265.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,7 @@

#include <condition_variable>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
#include "opencv2/imgproc/imgproc.hpp"

#include <librealsense2/rs.hpp>
Expand All @@ -49,6 +48,7 @@ void exit_loop_handler(int s){
b_continue_session = false;
}

/* // UNUSED
static rs2_option get_sensor_option(const rs2::sensor& sensor)
{
// Sensors usually have several options to control their properties
Expand Down Expand Up @@ -90,6 +90,7 @@ static rs2_option get_sensor_option(const rs2::sensor& sensor)
uint32_t selected_sensor_option = 0;
return static_cast<rs2_option>(selected_sensor_option);
}
*/

int main(int argc, char **argv) {

Expand Down Expand Up @@ -266,11 +267,11 @@ int main(int argc, char **argv) {
//assert(vAccel.size() == vAccel_times.size());
//assert(vGyro.size() == vGyro_times.size());

for(int i=0; i<vAccel.size(); ++i){
for(size_t i=0; i<vAccel.size(); ++i){
accFile << std::setprecision(15) << vAccel_times[i] << "," << vAccel[i].x << "," << vAccel[i].y << "," << vAccel[i].z << endl;
}

for(int i=0; i<vGyro.size(); ++i){
for(size_t i=0; i<vGyro.size(); ++i){
gyroFile << std::setprecision(15) << vGyro_times[i] << "," << vGyro[i].x << "," << vGyro[i].y << "," << vGyro[i].z << endl;
}

Expand Down
31 changes: 17 additions & 14 deletions Examples/Monocular-Inertial/mono_inertial_euroc.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,10 @@
#include <ctime>
#include <sstream>

#include<opencv2/core/core.hpp>
#include<opencv2/opencv.hpp>

#include<System.h>
#include<Viewer.h>
#include "ImuTypes.h"

using namespace std;
Expand Down Expand Up @@ -117,12 +118,14 @@ int main(int argc, char *argv[])
cout.precision(17);

// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true);
float imageScale = SLAM.GetImageScale();
ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_MONOCULAR);
ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
float imageScale = SLAM->GetImageScale();

#ifdef REGISTER_TIMES
double t_resize = 0.f;
double t_track = 0.f;

#endif
int proccIm=0;
for (seq = 0; seq<num_seq; seq++)
{
Expand Down Expand Up @@ -156,7 +159,7 @@ int main(int argc, char *argv[])
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point t_End_Resize = std::chrono::steady_clock::now();
t_resize = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Resize - t_Start_Resize).count();
SLAM.InsertResizeTime(t_resize);
SLAM->InsertResizeTime(t_resize);
#endif
}

Expand All @@ -180,13 +183,13 @@ int main(int argc, char *argv[])

// Pass the image to the SLAM system
// cout << "tframe = " << tframe << endl;
SLAM.TrackMonocular(im,tframe,vImuMeas); // TODO change to monocular_inertial
auto pos = SLAM->TrackMonocular(im,tframe,vImuMeas); // TODO change to monocular_inertial

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

viewer.update(pos);
#ifdef REGISTER_TIMES
t_track = t_resize + std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t2 - t1).count();
SLAM.InsertTrackTime(t_track);
SLAM->InsertTrackTime(t_track);
#endif

double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
Expand All @@ -209,25 +212,25 @@ int main(int argc, char *argv[])
{
cout << "Changing the dataset" << endl;

SLAM.ChangeDataset();
SLAM->ChangeDataset();
}
}

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

// Save camera trajectory
if (bFileName)
{
const string kf_file = "kf_" + string(argv[argc-1]) + ".txt";
const string f_file = "f_" + string(argv[argc-1]) + ".txt";
SLAM.SaveTrajectoryEuRoC(f_file);
SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);
SLAM->SaveTrajectoryEuRoC(f_file);
SLAM->SaveKeyFrameTrajectoryEuRoC(kf_file);
}
else
{
SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");
SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
SLAM->SaveTrajectoryEuRoC("CameraTrajectory.txt");
SLAM->SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
}

return 0;
Expand Down
25 changes: 14 additions & 11 deletions Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,12 +27,13 @@

#include <condition_variable>

#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>

#include <librealsense2/rs.hpp>
#include "librealsense2/rsutil.h"

#include <System.h>
#include <Viewer.h>

using namespace std;

Expand Down Expand Up @@ -287,8 +288,9 @@ int main(int argc, char **argv) {


// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true, 0, file_name);
float imageScale = SLAM.GetImageScale();
ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_MONOCULAR, file_name);
ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
float imageScale = SLAM->GetImageScale();

double timestamp;
cv::Mat im;
Expand All @@ -299,10 +301,11 @@ int main(int argc, char **argv) {
v_accel_data_sync.clear();
v_accel_timestamp_sync.clear();

#ifdef REGISTER_TIMES
double t_resize = 0.f;
double t_track = 0.f;

while (!SLAM.isShutDown())
#endif
while (viewer.isOpen())
{
std::vector<rs2_vector> vGyro;
std::vector<double> vGyro_times;
Expand All @@ -314,7 +317,7 @@ int main(int argc, char **argv) {
if(!image_ready)
cond_image_rec.wait(lk);

std::chrono::steady_clock::time_point time_Start_Process = std::chrono::steady_clock::now();
// std::chrono::steady_clock::time_point time_Start_Process = std::chrono::steady_clock::now(); // UNUSED


if(count_im_buffer>1)
Expand Down Expand Up @@ -351,7 +354,7 @@ int main(int argc, char **argv) {
}


for(int i=0; i<vGyro.size(); ++i)
for(size_t i=0; i<vGyro.size(); ++i)
{
ORB_SLAM3::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
vGyro[i].x, vGyro[i].y, vGyro[i].z,
Expand All @@ -370,21 +373,21 @@ int main(int argc, char **argv) {
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point t_End_Resize = std::chrono::steady_clock::now();
t_resize = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Resize - t_Start_Resize).count();
SLAM.InsertResizeTime(t_resize);
SLAM->InsertResizeTime(t_resize);
#endif
}

#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point t_Start_Track = std::chrono::steady_clock::now();
#endif
// Pass the image to the SLAM system
SLAM.TrackMonocular(im, timestamp, vImuMeas);
auto pos = SLAM->TrackMonocular(im, timestamp, vImuMeas);
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point t_End_Track = std::chrono::steady_clock::now();
t_track = t_resize + std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Track - t_Start_Track).count();
SLAM.InsertTrackTime(t_track);
SLAM->InsertTrackTime(t_track);
#endif

viewer.update(pos);


// Clear the previous IMU measurements to load the new ones
Expand Down
39 changes: 23 additions & 16 deletions Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,11 +25,12 @@
#include <ctime>
#include <sstream>

#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>

#include <librealsense2/rs.hpp>

#include <System.h>
#include <Viewer.h>
#include <condition_variable>
#include "ImuTypes.h"

Expand Down Expand Up @@ -57,15 +58,16 @@ int main(int argc, char **argv)
}

string file_name;
bool bFileName = false;
// bool bFileName = false; // UNUSED

if (argc == 4) {
file_name = string(argv[argc - 1]);
bFileName = true;
// bFileName = true; // UNUSED
}

ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true, 0, file_name);
float imageScale = SLAM.GetImageScale();
ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_MONOCULAR, file_name);
ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
float imageScale = SLAM->GetImageScale();

struct sigaction sigIntHandler;

Expand Down Expand Up @@ -215,11 +217,12 @@ int main(int argc, char **argv)
v_accel_data_sync.clear();
v_accel_timestamp_sync.clear();

#ifdef REGISTER_TIMES
double t_resize = 0.f;
double t_track = 0.f;

#endif
std::chrono::steady_clock::time_point time_Start_Process;
while (!SLAM.isShutDown()){
while (viewer.isOpen()){
std::vector<rs2_vector> vGyro;
std::vector<double> vGyro_times;
std::vector<rs2_vector> vAccel;
Expand All @@ -230,7 +233,7 @@ int main(int argc, char **argv)
while(!image_ready)
cond_image_rec.wait(lk);

std::chrono::steady_clock::time_point time_Start_Process = std::chrono::steady_clock::now();
// std::chrono::steady_clock::time_point time_Start_Process = std::chrono::steady_clock::now(); // UNUSED

if(count_im_buffer>1)
cout << count_im_buffer -1 << " dropped frs\n";
Expand Down Expand Up @@ -261,7 +264,7 @@ int main(int argc, char **argv)
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point t_End_Resize = std::chrono::steady_clock::now();
t_resize = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Resize - t_Start_Resize).count();
SLAM.InsertResizeTime(t_resize);
SLAM->InsertResizeTime(t_resize);
#endif
}

Expand All @@ -282,7 +285,7 @@ int main(int argc, char **argv)
}


for(int i=0; i<vGyro.size(); ++i){
for(size_t i=0; i<vGyro.size(); ++i){
ORB_SLAM3::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
vGyro[i].x, vGyro[i].y, vGyro[i].z,
vGyro_times[i]);
Expand All @@ -296,19 +299,23 @@ int main(int argc, char **argv)
}
}

#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point t_Start_Track = std::chrono::steady_clock::now();
#endif
// Pass the image to the SLAM system
SLAM.TrackMonocular(im, timestamp, vImuMeas);
auto pos = SLAM->TrackMonocular(im, timestamp, vImuMeas);

#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point t_End_Track = std::chrono::steady_clock::now();

#endif
viewer.update(pos);
#ifdef REGISTER_TIMES
t_track = t_resize + std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Track - t_Start_Track).count();
SLAM.InsertTrackTime(t_track);
SLAM->InsertTrackTime(t_track);
#endif

double timeProcess = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_Start_Track - time_Start_Process).count();
double timeSLAM = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Track - t_Start_Track).count();
// double timeProcess = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_Start_Track - time_Start_Process).count(); // UNUSED
// double timeSLAM = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Track - t_Start_Track).count(); // UNUSED

// cout << "Time process: " << timeProcess << endl;
// cout << "Time SLAM: " << timeSLAM << endl;
Expand All @@ -317,7 +324,7 @@ int main(int argc, char **argv)
vImuMeas.clear();
}

SLAM.Shutdown();
// SLAM.Shutdown();

return 0;
}
Expand Down
Loading

0 comments on commit e2d034b

Please sign in to comment.