Odom.txt

.gitignore

@@ -1,4 +1,5 @@
 CMakeLists.txt.user
+Examples/OdomMono/odom_mono_nv
 Examples/Monocular/mono_euroc
 Examples/Monocular/mono_kitti
 Examples/Monocular/mono_tum
@@ -21,4 +22,4 @@ Vocabulary/ORBvoc.txt
 build/
 *~
 lib/
-
+Debug/

CMakeLists.txt

@@ -68,6 +68,7 @@ src/KeyFrameDatabase.cc
 src/Sim3Solver.cc
 src/Initializer.cc
 src/Viewer.cc
+src/Se2.cpp
 )
 
 target_link_libraries(${PROJECT_NAME}
@@ -80,34 +81,40 @@ ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
 
 # Build examples
 
-set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D)
+#set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D)
 
-add_executable(rgbd_tum
-Examples/RGB-D/rgbd_tum.cc)
-target_link_libraries(rgbd_tum ${PROJECT_NAME})
+#add_executable(rgbd_tum
+#Examples/RGB-D/rgbd_tum.cc)
+#target_link_libraries(rgbd_tum ${PROJECT_NAME})
 
-set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo)
+#set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo)
 
-add_executable(stereo_kitti
-Examples/Stereo/stereo_kitti.cc)
-target_link_libraries(stereo_kitti ${PROJECT_NAME})
+#add_executable(stereo_kitti
+#Examples/Stereo/stereo_kitti.cc)
+#target_link_libraries(stereo_kitti ${PROJECT_NAME})
 
-add_executable(stereo_euroc
-Examples/Stereo/stereo_euroc.cc)
-target_link_libraries(stereo_euroc ${PROJECT_NAME})
+#add_executable(stereo_euroc
+#Examples/Stereo/stereo_euroc.cc)
+#target_link_libraries(stereo_euroc ${PROJECT_NAME})
 
 
-set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular)
+#set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular)
 
-add_executable(mono_tum
-Examples/Monocular/mono_tum.cc)
-target_link_libraries(mono_tum ${PROJECT_NAME})
+#add_executable(mono_tum
+#Examples/Monocular/mono_tum.cc)
+#target_link_libraries(mono_tum ${PROJECT_NAME})
 
-add_executable(mono_kitti
-Examples/Monocular/mono_kitti.cc)
-target_link_libraries(mono_kitti ${PROJECT_NAME})
+#add_executable(mono_kitti
+#Examples/Monocular/mono_kitti.cc)
+#target_link_libraries(mono_kitti ${PROJECT_NAME})
 
-add_executable(mono_euroc
-Examples/Monocular/mono_euroc.cc)
-target_link_libraries(mono_euroc ${PROJECT_NAME})
+#add_executable(mono_euroc
+#Examples/Monocular/mono_euroc.cc)
+#target_link_libraries(mono_euroc ${PROJECT_NAME})
+
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/OdomMono)
+
+add_executable(odom_mono_nv
+Examples/OdomMono/odom_mono_nv.cc)
+target_link_libraries(odom_mono_nv ${PROJECT_NAME})
 

Examples/OdomMono/odom_mono_nv.cc

@@ -0,0 +1,165 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include<iostream>
+#include<algorithm>
+#include<fstream>
+#include<chrono>
+
+#include<System.h>
+
+using namespace std;
+void LoadImages(const string &strFile, vector<string> &vstrImageFilenames,
+                vector<double> &vTimestamps);
+
+int main(int argc, char **argv)
+{
+    if(argc != 4)
+    {
+        cerr << endl << "Usage: ./odom_mono_nv path_to_vocabulary path_to_settings path_to_sequence" << endl;
+        return 1;
+    }
+    string orbBinFile = argv[1];
+    string settingFile = argv[2];
+    string dataPath = argv[3];
+
+    // Create SLAM system. It initializes all system threads and gets ready to process frames.
+
+    ORB_SLAM2::System SLAM(orbBinFile, settingFile, ORB_SLAM2::System::MONOCULAR,true);
+
+    cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ);
+
+    // Number of images
+    int nImages = (int)fsSettings["Images.Number"];
+
+    // Between-frame duration
+    double T = (double)fsSettings["Images.FrameDuration"];
+
+    // Camera parameters for pre-process of undistortion
+    cv::Mat preK, preD;
+    fsSettings["Camera.Pre.K"] >> preK;
+    fsSettings["Camera.Pre.D"] >> preD;
+
+    cout << endl << "-------" << endl;
+    cout << "Start processing sequence ..." << endl;
+
+    string fullOdomName = dataPath + "/odo_raw.txt";
+    ifstream odomFile(fullOdomName);
+    float x, y, theta;
+    string odomLine;
+
+    // Vector for tracking time statistics
+    vector<float> vTimesTrack;
+    vTimesTrack.resize(nImages);
+
+    // Main loop
+    cv::Mat im, imraw;
+    for(int ni=0; ni<nImages; ni++)
+    {
+        // Read image from file
+        string fullImgName = dataPath + "/image/" + to_string(ni) + ".bmp";
+        imraw = cv::imread(fullImgName, CV_LOAD_IMAGE_GRAYSCALE);
+        cv::undistort(imraw, im, preK, preD);
+        if(im.empty())
+        {
+            cerr << endl << "Failed to load image at: "
+                 << fullImgName << endl;
+            return 1;
+        }
+
+        // Read odometry data
+        getline(odomFile, odomLine);
+        istringstream iss(odomLine);
+        iss >> x >> y >> theta;
+        Se2 odom(x, y, theta);
+
+        double tframe = 0;
+
+#ifdef COMPILEDWITHC11
+        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
+#else
+        std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
+#endif
+
+        SLAM.TrackOdomMono(im, odom, tframe);
+
+#ifdef COMPILEDWITHC11
+        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
+#else
+        std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
+#endif
+
+        double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
+
+        vTimesTrack[ni]=ttrack;
+
+        // Wait to load the next frame
+        if(ttrack<T)
+            usleep((T-ttrack)*1e6);
+    }
+
+    // Stop all threads
+    SLAM.Shutdown();
+
+    // Tracking time statistics
+    sort(vTimesTrack.begin(),vTimesTrack.end());
+    float totaltime = 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;
+
+    // Save camera trajectory
+    SLAM.SaveKeyFrameTrajectoryVN("KeyFrameTrajectory.txt");
+
+    return 0;
+}
+void LoadImages(const string &strFile, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
+{
+    ifstream f;
+    f.open(strFile.c_str());
+
+    // skip first three lines
+    string s0;
+    getline(f,s0);
+    getline(f,s0);
+    getline(f,s0);
+
+    while(!f.eof())
+    {
+        string s;
+        getline(f,s);
+        if(!s.empty())
+        {
+            stringstream ss;
+            ss << s;
+            double t;
+            string sRGB;
+            ss >> t;
+            vTimestamps.push_back(t);
+            ss >> sRGB;
+            vstrImageFilenames.push_back(sRGB);
+        }
+    }
+}

Examples/OdomMono/vn1.yaml

@@ -0,0 +1,92 @@
+%YAML:1.0
+
+#--------------------------------------------------------------------------------------------
+# Camera Parameters. Adjust them!
+#--------------------------------------------------------------------------------------------
+
+# Camera calibration and distortion parameters (OpenCV) 
+Camera.fx: 231.976033627644090
+Camera.fy: 232.157224036901510
+Camera.cx: 326.92392097053931
+Camera.cy: 227.83848839534838
+
+Camera.k1: 0
+Camera.k2: 0
+Camera.p1: 0
+Camera.p2: 0
+Camera.k3: 0
+
+Camera.Pre.D: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: f
+   data: [ -0.207406506100898 , 0.032194071349429 , 0.001120166051888 , 0.000859411522110 , 0.000000000000000 ]
+Camera.Pre.K: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: f
+   data: [ 231.976033627644090, 0., 326.923920970539310, 0.,
+       232.157224036901510, 227.838488395348380, 0., 0., 1. ]
+
+# Camera frames per second 
+Camera.fps: 30.0
+
+# Camera extrinsics
+Tbc.tbc.x: 123.91
+Tbc.tbc.y: 399.1023
+Tbc.tbc.z: 0.0
+Tbc.rbc.x: -0.0121
+Tbc.rbc.y: 0.0362
+Tbc.rbc.z: -1.6077
+
+#
+Images.Number: 3106
+Images.FrameDuration: 0.03
+
+# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
+Camera.RGB: 1
+
+#--------------------------------------------------------------------------------------------
+# ORB Parameters
+#--------------------------------------------------------------------------------------------
+
+# ORB Extractor: Number of features per image
+ORBextractor.nFeatures: 1000
+
+# ORB Extractor: Scale factor between levels in the scale pyramid 	
+ORBextractor.scaleFactor: 1.2
+
+# ORB Extractor: Number of levels in the scale pyramid	
+ORBextractor.nLevels: 8
+
+# ORB Extractor: Fast threshold
+# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
+# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
+# You can lower these values if your images have low contrast			
+ORBextractor.iniThFAST: 20
+ORBextractor.minThFAST: 7
+
+#--------------------------------------------------------------------------------------------
+# Viewer Parameters
+#--------------------------------------------------------------------------------------------
+Viewer.KeyFrameSize: 0.05
+Viewer.KeyFrameLineWidth: 1
+Viewer.GraphLineWidth: 0.9
+Viewer.PointSize: 2
+Viewer.CameraSize: 0.08
+Viewer.CameraLineWidth: 3
+Viewer.ViewpointX: 0
+Viewer.ViewpointY: -0.7
+Viewer.ViewpointZ: -1.8
+Viewer.ViewpointF: 500
+# Visual.KeyFrameSize: 0.4
+# Visual.KeyFrameLineWidth: 1
+# Visual.GraphLineWidth: 0.9
+# Visual.PointSize: 2
+# Visual.CameraSize: 0.6
+# Visual.CameraLineWidth: 3
+# Visual.ViewpointX: 0
+# Visual.ViewpointY: -0.7
+# Visual.ViewpointZ: -1.8
+# Visual.ViewpointF: 500
+Visual.Ratio: 0.002