Modifications done on TrackWithMotionModel() and the functions it used

CMakeSettings.json

@@ -11,7 +11,7 @@
       "buildCommandArgs": "",
       "ctestCommandArgs": "",
       "intelliSenseMode": "windows-msvc-x64",
-      "cmakeToolchain": "E:/vcpkg/scripts/buildsystems/vcpkg.cmake",
+      "cmakeToolchain": "C:/Users/mhmds/vcpkg/scripts/buildsystems/vcpkg.cmake",
       "variables": [
         {
           "name": "MSVC_USE_STATIC_CRT",

Thirdparty/g2o/g2o/core/optimizable_graph.cpp

@@ -248,10 +248,7 @@ namespace g2o {
     bool OptimizableGraph::addEdge(HyperGraph::Edge *e_) {
         OptimizableGraph::Edge *e = dynamic_cast<OptimizableGraph::Edge *>(e_);
         assert(e && "Edge does not inherit from OptimizableGraph::Edge");
-        if (!e)
-            return false;
-        bool eresult = HyperGraph::addEdge(e);
-        if (!eresult)
+        if (!e || !HyperGraph::addEdge(e))
             return false;
         e->_internalId = _nextEdgeId++;
         if (!e->resolveParameters()) {

generalSettings.json

@@ -1,12 +1,12 @@
 {
-  "VocabularyPath": "E:\\rbdlabRepo\\simulatorMapping\\Thirdparty\\DBow3\\orbvoc.dbow3",
-  "DroneYamlPathSlam": "E:\\rbdlabRepo\\simulatorMapping\\config\\tello_9F5EC2_640.yaml",
-  "offlineVideoTestPath": "C:\\Users\\tzuk\\Downloads\\mapping.avi",
+  "VocabularyPath": "C:\\Users\\mhmds\\simulatorMapping\\Thirdparty\\DBow3\\orbvoc.dbow3",
+  "DroneYamlPathSlam": "C:\\Users\\mhmds\\simulatorMapping\\config\\tello_9F5EC2_640.yaml",
+  "offlineVideoTestPath": "C:\\Users\\mhmds\\mapping.avi",
   "onlineVideoPath": "udp://0.0.0.0:11111?overrun_nonfatal=1&fifo_size=1000",
   "loadMap": false,
   "loadMapPath": "/home/tzuk/slamMaps/HaifaLab.bin",
   "saveMap": true,
-  "simulatorOutputDir": "E:\\simulatorMapping\\slamMaps",
+  "simulatorOutputDir": "C:\\Users\\mhmds\\simulatorMapping\\slamMaps",
   "modelTextureNameToAlignTo": "floor",
   "mapInputDir": "/home/tzuk/slamMaps/example_mapping11/",
   "getPointDataCsv": "/home/liam/example.csv",
@@ -25,7 +25,7 @@
   "rollRad": 2.87,
   "movingScale": 0.2,
   "rotateScale": 0.2,
-  "modelPath": "E:\\simulatorMapping\\TLV-data\\drones_lab.obj",
+  "modelPath": "C:\\Users\\mhmds\\simulatorMapping\\TLV-data\\drones_lab.obj",
   "pointSize": 10,
   "xOffset": 0.0696954,
   "yOffset": 0.232057,

slam/include/Frame.h

@@ -204,6 +204,12 @@ namespace ORB_SLAM2 {
 
         static bool mbInitialComputations;
 
+        // Rotation, translation and camera center
+        cv::Mat mRcw;
+        cv::Mat mtcw;
+        cv::Mat mRwc;
+        cv::Mat mOw; //==mtwc
+
 
     private:
 
@@ -219,12 +225,6 @@ namespace ORB_SLAM2 {
 
         // Assign keypoints to the grid for speed up feature matching (called in the constructor).
         void AssignFeaturesToGrid();
-
-        // Rotation, translation and camera center
-        cv::Mat mRcw;
-        cv::Mat mtcw;
-        cv::Mat mRwc;
-        cv::Mat mOw; //==mtwc
     };
 
 }// namespace ORB_SLAM

slam/src/Frame.cc

@@ -331,7 +331,7 @@ namespace ORB_SLAM2 {
         mRcw = mTcw.rowRange(0, 3).colRange(0, 3);
         mRwc = mRcw.t();
         mtcw = mTcw.rowRange(0, 3).col(3);
-        mOw = -mRcw.t() * mtcw;
+        mOw = -mRwc * mtcw;
     }
 
     bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit) {

slam/src/MapPoint.cc

@@ -350,19 +350,16 @@ namespace ORB_SLAM2
             if (mObservations.count(pKF))
             {
                 int idx = mObservations[pKF];
+                nObs--;
                 if (pKF->mvuRight[idx] >= 0)
-                    nObs -= 2;
-                else
                     nObs--;
 
                 mObservations.erase(pKF);
 
-                if (mpRefKF == pKF)
-                    mpRefKF = mObservations.begin()->first;
+                mpRefKF = mpRefKF == pKF ? mObservations.begin()->first : mpRefKF;
 
                 // If only 2 observations or less, discard point
-                if (nObs <= 2)
-                    bBad = true;
+                bBad = nObs <= 2 ? true : bBad;
             }
         }