finishe migration

.gitignore

@@ -1,3 +1,12 @@
 build/
 cmake-build-debug/
-.idea/
+
+
+Slam_latest_Map.bin
+Thirdparty/DBoW2/lib/
+Thirdparty/g2o/lib/
+Thirdparty/g2o/config.h
+src/ORB_SLAM2/Vocabulary/ORBvoc.bin
+src/ORB_SLAM2/Vocabulary/ORBvoc.txt
+CameraTrajectory.txt
+FrameInfo.txt

CMakeLists.txt

@@ -63,7 +63,7 @@ message(${PCL_LIBRARY_DIRS})
 
 SET(SRC_FOLDER src)
 #FILE(GLOB_RECURSE SRC_FILES "${SRC_FOLDER}/AugmentedVR/*.cpp" "${SRC_FOLDER}/ORB_SLAM2/src/*.cc" "${SRC_FOLDER}/*.cpp")
-FILE(GLOB_RECURSE SRC_FILES "${SRC_FOLDER}/AugmentedVR/*.cpp" "${SRC_FOLDER}/ORB_SLAM2/src/*.cc" "${SRC_FOLDER}/*.cpp")
+FILE(GLOB_RECURSE SRC_FILES "${SRC_FOLDER}/AugmentedVR/*.cpp" "${SRC_FOLDER}/ORB_SLAM2/src/*.cc")
 
 #message(${SRC_FILES})
 

CamCalib.yaml

@@ -15,10 +15,10 @@ Camera.k2: 0.0
 Camera.p1: 0.0
 Camera.p2: 0.0
 
-Camera.width: 672
-Camera.height: 376
-#Camera.width: 1280
-#Camera.height: 720
+#Camera.width: 672
+#Camera.height: 376
+Camera.width: 1280
+Camera.height: 720
 
 # Camera frames per second 
 Camera.fps: 10.0
@@ -37,7 +37,7 @@ ThDepth: 35
 #--------------------------------------------------------------------------------------------
 
 # ORB Extractor: Number of features per image
-ORBextractor.nFeatures: 500
+ORBextractor.nFeatures: 2000
 
 # ORB Extractor: Scale factor between levels in the scale pyramid 	
 ORBextractor.scaleFactor: 1.2

app/main.cpp

@@ -80,8 +80,21 @@ using namespace cv;
 
 
 
+VCluster* mV;
+std::thread* mvThread;
 
+void run(){
 
+    mV->run();
+}
+
+
+void close() {
+    quit = true;
+    mvThread->join();
+    mV->exit();
+    cout << "System shuts down" << endl;
+}
 ///////////////////////////////////////////////////////////main  function////////////////////////////////////////////////////
 int main(int argc, char **argv) {
 
@@ -121,15 +134,18 @@ int main(int argc, char **argv) {
         cout << "Map File: " << mapFile << endl;
     }
 
-    VCluster* mV = new VCluster(false, mapFile, argc, argv, VPath[0]);
+    mV = new VCluster(false, mapFile, argc, argv, VPath[0]);
 
 #ifdef EVAL
     gettimeofday(&tInit, NULL);
 #endif
 
-    mV->run();
-
-    delete mV;
-    cout << "AVR shuts down" << endl;
+//    mV->run();
+    mvThread = new std::thread(run);
+//    while(mV->VNode[0]->getFrameSeq()<5);
+    glutCloseFunc(close);
+    glutMainLoop();
     return 0;
 }
+
+

include/AugmentedVR.hpp

@@ -40,6 +40,8 @@ struct ZED_DATA{
     cv::Mat FrameRightGray;
 
     cv::Mat pointcloud;
+//    sl::Mat pointcloud_sl;
+
     cv::Mat PC_noColor;
 
 
@@ -59,6 +61,8 @@ struct STEREO_DATA{
     cv::Mat FrameRightGray;
 
     cv::Mat pointcloud;
+//    sl::Mat pointcloud_sl;
+
     cv::Mat PC_noColor;
 
     cv::Mat CamMotionMat; // Tcw // (4,4,CV_32F) rotation mat and translation mat Tcw (Rcw, tcw) in ORBSLAM
@@ -108,7 +112,11 @@ class AugmentedVR {
 
     cv::Mat ZED_LRes;
     cv::Mat BufferXYZRGBA;
-    cv::Mat pointcloud;
+
+    cv::Mat pointcloud_cv;
+//    sl::Mat pointcloud_sl;
+    sl::Mat pointcloud_sl_gpu;
+
     cv::Mat PC_noColor;
 
     cv::Mat initPC;
@@ -120,7 +128,7 @@ class AugmentedVR {
 //    vector<cv::Mat> lastpointcloud;
     vector<STEREO_DATA> lastStereoData;
 //    vector<cv::Mat> PCMotionVec;
-    sl::Mat BufferXYZRGBA_gpu;
+
     cv::Mat depth_mat;
 
 
@@ -169,6 +177,9 @@ class AugmentedVR {
 //    AugmentedVR(int CamId);
     AugmentedVR(int CamId, sl::InitParameters initParam, sl::RuntimeParameters runtimeParam, int ZEDConfidence);
     ~AugmentedVR();
+    void exit();
+
+
     int initZEDCam(int startFrameID);
 
     void initSLAMStereo(string VocFile, string CalibrationFile, bool bReuseMap = false, string mapFile = "default.txt");
@@ -241,7 +252,7 @@ class AugmentedVR {
 
     void dead_reckoning_onRxDynamicPC();
 
-    bool load_NextFrame();
+    bool loadSlamFrameAsCurrentFrame();
 
     void detectFeature();
 

include/Displayer.hpp

@@ -17,21 +17,24 @@
 
 class Displayer {
 
+public:
     AugmentedVR** VNode;
 //    PointCloud* mPC;
 //    Viewer* mPCViewer;
 
 // SDK 2.0 Point cloud viewer
-    GLViewer mGLViewer;
-
-    sl::Mat PC_gpu;
+    GLViewer* mGLViewer;
+    std::thread display_callback;
+//    sl::Mat PC_gpu;
 
     int PCwidth, PCheight; //
-    CUcontext context;
+//    CUcontext context;
 public:
     Displayer(AugmentedVR **VNode);
 
-    virtual ~Displayer();
+    ~Displayer();
+
+    void exit();
 
     void saveFrame();
 
@@ -47,9 +50,11 @@ class Displayer {
 
     void showPC(cv::Mat mat);
 
+    void showPC(sl::Mat& mat);
+
     void checkResetPCViewer(int width, int height);
 
-    void pushPC_Mat(cv::Mat mat);
+    void pushPC_Mat(cv::Mat& mat);
 
     void debugPC(cv::Mat DebugPC);
 

include/ObjReceiver.hpp

@@ -23,7 +23,7 @@ class ObjReceiver {
 public:
 
     ObjReceiver(AugmentedVR *myAVR, const int CamId, string commPath);
-
+    ~ObjReceiver();
 
     cv::Mat readFrame(int frameSeq);
     cv::Mat readTcw(int frameSeq);
@@ -34,7 +34,7 @@ class ObjReceiver {
     cv::Mat readLowPassObjectMotionVec(int frameSeq);
 
 
-    virtual ~ObjReceiver();
+
 };
 
 

include/ObjSender.hpp

@@ -24,7 +24,7 @@ class ObjSender {
 public:
     ObjSender(AugmentedVR *myAVR, string commPath);
 
-    virtual ~ObjSender();
+    ~ObjSender();
 
     void writeFrame();
     void writeTcw();

include/VCluster.hpp

@@ -23,13 +23,16 @@ class VCluster {
     int frameSeqRx;
     long timeRx;
 
+    std::thread* displayThread;
+
 public://public variables
     AugmentedVR** VNode;
 
 public:
     VCluster();
     VCluster(bool live, const string mapFile, int argc, char** argv, string VPath);
     ~VCluster();
+    void exit();
 
     void run();
 
@@ -42,6 +45,8 @@ class VCluster {
     void PreProcess();
 
     void segmentation();
+
+    void initDisplay();
 };
 
 

include/globals.hpp

@@ -20,8 +20,8 @@
 
 
 //#define EVAL
-#define SIMPLEEVAL
-#define PIPELINE
+//#define SIMPLEEVAL
+//#define PIPELINE
 
 #define PI 3.14159265
 
@@ -96,7 +96,11 @@ extern int lengthInFrame;
 
 
 extern timeval tInit;
-#endif //PROJECT_GLOBALS_H
+
+extern bool quit;
+
 
 cv::Mat slMat2cvMat(sl::Mat& input);
-sl::Mat cvMat2slMat(cv::Mat& input);
+void cvMat2slMat(cv::Mat& input, sl::Mat& output);
+
+#endif //PROJECT_GLOBALS_H

include/pcCodec.hpp

@@ -39,7 +39,7 @@ using namespace std;
 using namespace pcl;
 
 class pcCodec {
-    float* data_cloud;
+//    float* data_cloud;
     std::mutex mutex_input;
     int width, height;
 

pre_build.sh

@@ -14,11 +14,3 @@ mkdir build
 cd build
 cmake .. -DCMAKE_BUILD_TYPE=Release
 make -j
-
-cd ../../../
-
-echo "Uncompress vocabulary ..."
-
-cd Vocabulary
-tar -xf ORBvoc.txt.tar.gz
-cd ..