From e828af803f37a0a25b65b23cbf762f80df7c23c0 Mon Sep 17 00:00:00 2001
From: "John U. Balis" <phobossystems@gmail.com>
Date: Sun, 21 Mar 2021 00:02:57 -0500
Subject: [PATCH] several fixes as recommended by
 https://github.com/raulmur/ORB_SLAM2/issues/337#issuecomment-659310138 and
 https://github.com/zhenzhenxiang/orb_slam_2_ros-1/commit/1e6a52a28b3a0ba8d7615df476582e21f2c9b746,
 and additionally added yaml for basic SLAM test on miniworld images

---
 Examples/Monocular/TUM2.yaml       |  5 ++-
 Examples/Monocular/miniworld1.yaml | 61 ++++++++++++++++++++++++++++++
 include/System.h                   |  2 +
 src/Initializer.cc                 |  5 ++-
 src/ORBmatcher.cc                  |  2 +-
 5 files changed, 72 insertions(+), 3 deletions(-)
 create mode 100644 Examples/Monocular/miniworld1.yaml

diff --git a/Examples/Monocular/TUM2.yaml b/Examples/Monocular/TUM2.yaml
index 914fbcf652..8fc5339062 100644
--- a/Examples/Monocular/TUM2.yaml
+++ b/Examples/Monocular/TUM2.yaml
@@ -55,4 +55,7 @@ Viewer.ViewpointX: 0
 Viewer.ViewpointY: -0.7
 Viewer.ViewpointZ: -1.8
 Viewer.ViewpointF: 500
-
+#Viewer.ViewpointX: 0
+#Viewer.ViewpointY: -0.7
+#Viewer.ViewpointZ: -1.8
+#Viewer.ViewpointF: 500
diff --git a/Examples/Monocular/miniworld1.yaml b/Examples/Monocular/miniworld1.yaml
new file mode 100644
index 0000000000..84efb4560d
--- /dev/null
+++ b/Examples/Monocular/miniworld1.yaml
@@ -0,0 +1,61 @@
+%YAML:1.0
+
+#--------------------------------------------------------------------------------------------
+# Camera Parameters. Adjust them!
+#--------------------------------------------------------------------------------------------
+
+# Camera calibration and distortion parameters (OpenCV) 
+Camera.fx: 520.908620
+Camera.fy: 521.007327
+Camera.cx: 325.141442
+Camera.cy: 249.701764
+
+Camera.k1: 0.231222
+Camera.k2: -0.784899
+Camera.p1: -0.003257
+Camera.p2: -0.000105
+Camera.k3: 0.917205
+
+# Camera frames per second 
+Camera.fps: .5
+
+# 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
+#Viewer.ViewpointX: 0
+#Viewer.ViewpointY: -0.7
+#Viewer.ViewpointZ: -1.8
+#Viewer.ViewpointF: 500
diff --git a/include/System.h b/include/System.h
index b377b453d1..c64753fe91 100644
--- a/include/System.h
+++ b/include/System.h
@@ -26,6 +26,8 @@
 #include<thread>
 #include<opencv2/core/core.hpp>
 
+#include <unistd.h>
+
 #include "Tracking.h"
 #include "FrameDrawer.h"
 #include "MapDrawer.h"
diff --git a/src/Initializer.cc b/src/Initializer.cc
index 6094b8f51b..29071b71a1 100644
--- a/src/Initializer.cc
+++ b/src/Initializer.cc
@@ -887,10 +887,13 @@ int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::Ke
 
         vCosParallax.push_back(cosParallax);
         vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2));
-        nGood++;
+        
 
         if(cosParallax<0.99998)
+            {
             vbGood[vMatches12[i].first]=true;
+            nGood++;
+            }
     }
 
     if(nGood>0)
diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc
index 56bf279d0f..e19ac232c5 100644
--- a/src/ORBmatcher.cc
+++ b/src/ORBmatcher.cc
@@ -475,7 +475,7 @@ int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector<cv::Point2f
                     float rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle;
                     if(rot<0.0)
                         rot+=360.0f;
-                    int bin = round(rot*factor);
+                    int bin = round(rot/(360.0f*factor));
                     if(bin==HISTO_LENGTH)
                         bin=0;
                     assert(bin>=0 && bin<HISTO_LENGTH);