Use CLAHE for histeq, fixed not having slam features in sparse map, r…

…efine FAST feature corner extractions
rpng · goldbattle · Dec 20, 2020 · Aug 27, 2020 · Sep 28, 2020 · Sep 28, 2020
commit c102e6f1e00d9a869100aea03a7f56d7e2d39c86
diff --git a/ov_core/src/track/Grider_FAST.h b/ov_core/src/track/Grider_FAST.h
@@ -30,6 +30,7 @@
 #include <opencv/cv.hpp>
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
 
 
 namespace ov_core {
@@ -117,8 +118,8 @@ namespace ov_core {
                     // So we should append the location of that ROI in the image
                     for (size_t i = 0; i < (size_t) num_features_grid && i < pts_new.size(); i++) {
                         cv::KeyPoint pt_cor = pts_new.at(i);
-                        pt_cor.pt.x += x;
-                        pt_cor.pt.y += y;
+                        pt_cor.pt.x += (float)x;
+                        pt_cor.pt.y += (float)y;
                         collection.at(r).push_back(pt_cor);
                     }
                 }
@@ -129,6 +130,30 @@ namespace ov_core {
                 pts.insert(pts.end(),collection.at(r).begin(),collection.at(r).end());
             }
 
+            // Return if no points
+            if(pts.empty())
+                return;
+
+            // Sub-pixel refinement parameters
+            cv::Size win_size = cv::Size( 5, 5 );
+            cv::Size zero_zone = cv::Size( -1, -1 );
+            cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 20, 0.001);
+
+            // Get vector of points
+            std::vector<cv::Point2f> pts_refined;
+            for(size_t i=0; i<pts.size(); i++) {
+                pts_refined.push_back(pts.at(i).pt);
+            }
+
+            // Finally get sub-pixel for all extracted features
+            cv::cornerSubPix(img, pts_refined, win_size, zero_zone, term_crit);
+
+            // Save the refined points!
+            for(size_t i=0; i<pts.size(); i++) {
+                pts.at(i).pt = pts_refined.at(i);
+            }
+
+
         }
 
 

diff --git a/ov_core/src/track/TrackBase.h b/ov_core/src/track/TrackBase.h
@@ -177,6 +177,7 @@ namespace ov_core {
             // If we are calibrating camera intrinsics our normalize coordinates will be stale
             // This is because we appended them to the database with the current best guess *at that timestep*
             // Thus here since we have a change in calibration, re-normalize all the features we have
+            correct_active = false;
             if(correct_active) {
 
                 // Get all features in this database

diff --git a/ov_core/src/track/TrackKLT.cpp b/ov_core/src/track/TrackKLT.cpp
@@ -33,7 +33,8 @@ void TrackKLT::feed_monocular(double timestamp, cv::Mat &img, size_t cam_id) {
     std::unique_lock<std::mutex> lck(mtx_feeds.at(cam_id));
 
     // Histogram equalize
-    cv::equalizeHist(img, img);
+    //cv::equalizeHist(img, img);
+    clahe->apply(img, img);
 
     // Extract the new image pyramid
     std::vector<cv::Mat> imgpyr;
@@ -141,8 +142,10 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r
 
     // Histogram equalize
     cv::Mat img_left, img_right;
-    boost::thread t_lhe = boost::thread(cv::equalizeHist, boost::cref(img_leftin), boost::ref(img_left));
-    boost::thread t_rhe = boost::thread(cv::equalizeHist, boost::cref(img_rightin), boost::ref(img_right));
+    //boost::thread t_lhe = boost::thread(cv::equalizeHist, boost::cref(img_leftin), boost::ref(img_left));
+    //boost::thread t_rhe = boost::thread(cv::equalizeHist, boost::cref(img_rightin), boost::ref(img_right));
+    boost::thread t_lhe = boost::thread(&cv::CLAHE::apply, clahe.get(), boost::cref(img_leftin), boost::ref(img_left));
+    boost::thread t_rhe = boost::thread(&cv::CLAHE::apply, clahe.get(), boost::cref(img_rightin), boost::ref(img_right));
     t_lhe.join();
     t_rhe.join();
 
@@ -468,7 +471,7 @@ void TrackKLT::perform_detection_stereo(const std::vector<cv::Mat> &img0pyr, con
             // Note: but this might cause failure in cases of repeated textures (eg. checkerboard)
             std::vector<uchar> mask;
             std::vector<float> error;
-            cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 15, 0.01);
+            cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 10, 0.01);
             cv::calcOpticalFlowPyrLK(img0pyr, img1pyr, pts0_new, pts1_new, mask, error, win_size, pyr_levels, term_crit, cv::OPTFLOW_USE_INITIAL_FLOW);
 
             // Loop through and record only ones that are valid

diff --git a/ov_core/src/track/TrackKLT.h b/ov_core/src/track/TrackKLT.h
@@ -139,7 +139,12 @@ namespace ov_core {
 
         // How many pyramid levels to track on and the window size to reduce by
         int pyr_levels = 3;
-        cv::Size win_size = cv::Size(11, 11);
+        cv::Size win_size = cv::Size(15, 15);
+
+        // Alternative histagram equalization
+        double eq_clip_limit = 10.0;
+        cv::Size eq_win_size = cv::Size(8,8);
+        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(eq_clip_limit, eq_win_size);
 
         // Last set of image pyramids
         std::map<size_t, std::vector<cv::Mat>> img_pyramid_last;

diff --git a/ov_msckf/launch/display.rviz b/ov_msckf/launch/display.rviz
@@ -4,8 +4,8 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded: ~
-      Splitter Ratio: 0.600000024
-    Tree Height: 407
+      Splitter Ratio: 0.6000000238418579
+    Tree Height: 140
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -14,7 +14,7 @@ Panels:
       - /2D Nav Goal1
       - /Publish Point1
     Name: Tool Properties
-    Splitter Ratio: 0.588679016
+    Splitter Ratio: 0.5886790156364441
   - Class: rviz/Views
     Expanded:
       - /Current View1
@@ -25,6 +25,8 @@ Panels:
     Name: Time
     SyncMode: 0
     SyncSource: Image
+Preferences:
+  PromptSaveOnExit: true
 Toolbars:
   toolButtonStyle: 2
 Visualization Manager:
@@ -36,7 +38,7 @@ Visualization Manager:
       Color: 112; 112; 115
       Enabled: true
       Line Style:
-        Line Width: 0.0299999993
+        Line Width: 0.029999999329447746
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -96,21 +98,21 @@ Visualization Manager:
       Class: rviz/Path
       Color: 0; 255; 0
       Enabled: true
-      Head Diameter: 0.300000012
-      Head Length: 0.200000003
-      Length: 0.300000012
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
       Line Style: Billboards
-      Line Width: 0.00999999978
+      Line Width: 0.009999999776482582
       Name: Path VIO
       Offset:
         X: 0
         Y: 0
         Z: 0
       Pose Color: 255; 85; 255
       Pose Style: None
-      Radius: 0.0299999993
-      Shaft Diameter: 0.100000001
-      Shaft Length: 0.100000001
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
       Topic: /ov_msckf/pathimu
       Unreliable: false
       Value: true
@@ -119,25 +121,25 @@ Visualization Manager:
       Class: rviz/Path
       Color: 0; 255; 255
       Enabled: true
-      Head Diameter: 0.300000012
-      Head Length: 0.200000003
-      Length: 0.300000012
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
       Line Style: Billboards
-      Line Width: 0.0299999993
+      Line Width: 0.029999999329447746
       Name: Path GT
       Offset:
         X: 0
         Y: 0
         Z: 0
       Pose Color: 255; 85; 255
       Pose Style: None
-      Radius: 0.0299999993
-      Shaft Diameter: 0.100000001
-      Shaft Length: 0.100000001
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
       Topic: /ov_msckf/pathgt
       Unreliable: false
       Value: true
-    - Alpha: 0.800000012
+    - Alpha: 0.800000011920929
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
         Max Value: 10
@@ -146,9 +148,9 @@ Visualization Manager:
       Axis: Z
       Channel Name: intensity
       Class: rviz/PointCloud2
-      Color: 255; 255; 255
+      Color: 252; 175; 62
       Color Transformer: FlatColor
-      Decay Time: 2
+      Decay Time: 0
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
@@ -159,8 +161,8 @@ Visualization Manager:
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.00999999978
+      Size (Pixels): 8
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /ov_msckf/points_msckf
       Unreliable: false
@@ -190,7 +192,7 @@ Visualization Manager:
       Queue Size: 10
       Selectable: true
       Size (Pixels): 8
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /ov_msckf/points_slam
       Unreliable: false
@@ -220,13 +222,43 @@ Visualization Manager:
       Queue Size: 10
       Selectable: true
       Size (Pixels): 15
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /ov_msckf/points_aruco
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: ACTIVE Features
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /ov_msckf/loop_feats
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -250,13 +282,25 @@ Visualization Manager:
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Points
       Topic: /ov_msckf/points_sim
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /ov_msckf/loop_depth_colored
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Current Depths
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 44; 44; 44
@@ -272,7 +316,10 @@ Visualization Manager:
     - Class: rviz/FocusCamera
     - Class: rviz/Measure
     - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
       Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
     - Class: rviz/SetGoal
       Topic: /move_base_simple/goal
     - Class: rviz/PublishPoint
@@ -282,35 +329,37 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 22.1880836
+      Distance: 27.257526397705078
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.0599999987
+        Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.739498615
-        Y: -2.16416121
-        Z: 0.0424862541
+        X: 0
+        Y: 0
+        Z: 0
       Focal Shape Fixed Size: false
-      Focal Shape Size: 0.0500000007
+      Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.00999999978
-      Pitch: 1.22979665
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.8453981280326843
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.05039191
+      Yaw: 0.8753980994224548
     Saved: ~
 Window Geometry:
+  Current Depths:
+    collapsed: false
   Displays:
     collapsed: false
-  Height: 1056
+  Height: 1052
   Hide Left Dock: false
   Hide Right Dock: true
   Image:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -319,6 +368,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1855
-  X: 5825
-  Y: 24
+  Width: 1920
+  X: 0
+  Y: 0