Gui threshhold

CMakeLists.txt

@@ -69,6 +69,10 @@ add_service_files( DIRECTORY srv
 
 generate_messages(DEPENDENCIES std_msgs sensor_msgs)
 
+generate_dynamic_reconfigure_options(
+    cfg/thresholdParams.cfg
+)
+
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES ${PROJECT_NAME} $
@@ -110,6 +114,7 @@ set(MCPTAM_FUNCTIONS_LIB_SRC
     src/EntropyComputation.cpp
     src/GLWindow2.cpp
     src/GLWindowMenu.cpp
+    src/GuiCallBacks.cpp
     src/KeyFrame.cpp
     src/KeyFrameViewer.cpp
     src/Map.cpp
@@ -184,7 +189,7 @@ target_link_libraries(mcptam_functions ${MCPTAM_LIBS})
 
 # add executables and link them to the libraries
 add_executable(mcptam ${MCPTAM_SRC} )
-add_dependencies(mcptam ${PROJECT_NAME}_gencpp)
+add_dependencies(mcptam ${PROJECT_NAME}_gencpp ${PROJECT_NAME}_gencfg)
 target_link_libraries(mcptam ${MCPTAM_LIBS} mcptam_functions)
 
 add_executable(mcptam_client ${MCPTAM_CLIENT_SRC})

cfg/thresholdParams.cfg

@@ -0,0 +1,16 @@
+#!/usr/bin/env python
+PACKAGE = "mcptam"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# Threshold
+thresh = gen.add_group("THRESH", type="collapse", state=True)
+thresh.add("level_0", int_t, 0, "Level 0", 10,  5, 40)
+thresh.add("level_1", int_t, 0, "Level 1", 15, 5, 40)
+thresh.add("level_2", int_t, 0, "Level 2", 15, 5, 40)
+thresh.add("level_3", int_t, 0, "Level 3", 10, 5, 40)
+
+
+exit(gen.generate(PACKAGE, "mcptam", "Threshold"))

include/mcptam/GuiCallBacks.h

@@ -0,0 +1,16 @@
+#ifndef GUI_CALLBACKS_H
+#define GUI_CALLBACKS_H
+
+#include <mcptam/ThresholdConfig.h>
+#include <dynamic_reconfigure/server.h>
+
+extern int level0;
+extern int level1;
+extern int level2;
+extern int level3;
+
+extern int threshold_state;
+
+void threshold_gui_callback(mcptam::ThresholdConfig &config, uint32_t level);
+
+#endif

include/mcptam/KeyFrame.h

@@ -88,6 +88,7 @@
 #include <opencv2/features2d/features2d.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/photo/photo.hpp>
+#include <mcptam/GuiCallBacks.h>
 
 class MapPoint;
 class SmallBlurryImage;

launch/mcptam.launch

@@ -9,6 +9,8 @@
 <arg name="debug_prefix" value="" unless="$(arg debug)"/>
 
 <!-- mcptam node -->
+    <!-- rqt_reconfigure node -->
+    <node pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false" name="rqt_reconfigure" output="screen"/>
 <node name="mcptam" pkg="mcptam" type="mcptam" clear_params="true" output="screen" launch-prefix="$(arg debug_prefix)">
   <rosparam command="load" file="$(find mcptam)/groups/$(arg group_name).yaml" />
   <rosparam command="load" file="$(find mcptam)/params/CPER.yaml" />
@@ -36,7 +38,7 @@
   <param name="mm_max_scaled_mkf_dist" type="double" value="0.3" />
   <param name="mm_outlier_multiplier" type="double" value="1.1" />
   <param name="mm_init_cov_thresh" type="double" value="0.01" />
-  <param name="kf_adaptive_thresh" type="bool" value="true" />
+  <param name="kf_adaptive_thresh" type="bool" value="false" />
   <param name="level_zero_points" type="bool" value="true" />
 
   <param name="adjuster_recent_min_size" type="int" value="60" />

src/GuiCallBacks.cpp

@@ -0,0 +1,13 @@
+#include <mcptam/GuiCallBacks.h>
+
+void threshold_gui_callback(mcptam::ThresholdConfig &config, uint32_t level)
+{
+    //ROS_ERROR_STREAM("threshold gui callback: state " << config.groups.thresh.state);
+
+    level0 = config.level_0;
+    level1 = config.level_1;
+    level2 = config.level_2;
+    level3 = config.level_3;
+
+    threshold_state = config.groups.thresh.state;
+} 

src/KeyFrame.cpp

@@ -37,6 +37,7 @@
 //=========================================================================================
 
 #include <mcptam/KeyFrame.h>
+#include <mcptam/GuiCallBacks.h>
 #include <mcptam/ShiTomasi.h>
 #include <mcptam/SmallBlurryImage.h>
 #include <mcptam/MapPoint.h>
@@ -73,6 +74,12 @@ std::string KeyFrame::ssCandidateCriterion = "percent";
 bool KeyFrame::sbAdaptiveThresh = true;
 int Level::snNumPrev = 2;
 
+int level0 = 10;
+int level1 = 15;
+int level2 = 15;
+int level3 = 10;
+int threshold_state = 0;
+
 KeyFrame::KeyFrame(MultiKeyFrame* pMKF, std::string name) : mCamName(name), mpParent(pMKF)
 {
   mpSBI = NULL;
@@ -346,25 +353,65 @@ std::tuple<double, double, double> KeyFrame::MakeKeyFrame_Lite(CVD::Image<CVD::b
           // whose aim is to balance the different levels' relative feature densities.
 
       if (i == 0)
+      {
+          if(threshold_state==1)
           {
-              fast_corner_detect_10(lev.image, lev.vCorners, 10);
-              lev.nFastThresh = 10;
+            fast_corner_detect_10(lev.image, lev.vCorners, level0);
+            lev.nFastThresh = level0;
+            //ROS_INFO_STREAM("KeyFrame.cpp Level0: " << level0);
           }
+          else
+          {
+            fast_corner_detect_10(lev.image, lev.vCorners, 10);
+            lev.nFastThresh = 10;
+            //ROS_INFO_STREAM("KeyFrame.cpp Level0: " << level0);
+          }
+      }
       if (i == 1)
+      {
+          if(threshold_state==1)
+          {
+            fast_corner_detect_10(lev.image, lev.vCorners, level1);
+            lev.nFastThresh = level1;
+            //ROS_INFO_STREAM("KeyFrame.cpp Level1: " << level1);
+          }
+          else
           {
-              fast_corner_detect_10(lev.image, lev.vCorners, 15);
-              lev.nFastThresh = 15;
+            fast_corner_detect_10(lev.image, lev.vCorners, 15);
+            lev.nFastThresh = 15;
+            //ROS_INFO_STREAM("KeyFrame.cpp Level1 state not 1: " << level1);
           }
+      }
       if (i == 2)
+      {
+          if(threshold_state==1)
+          {
+            fast_corner_detect_10(lev.image, lev.vCorners, level2);
+            lev.nFastThresh = level2;
+            //ROS_INFO_STREAM("KeyFrame.cpp Level2: " << level2);
+          }
+          else
           {
-              fast_corner_detect_10(lev.image, lev.vCorners, 15);
-              lev.nFastThresh = 15;
+            fast_corner_detect_10(lev.image, lev.vCorners, 15);
+            lev.nFastThresh = 15;
+            //ROS_INFO_STREAM("KeyFrame.cpp Level2: " << level2);
           }
+      }
       if (i == 3)
+      {
+          if(threshold_state==1)
+          {
+            fast_corner_detect_10(lev.image, lev.vCorners, level3);
+            lev.nFastThresh = level3;
+            //ROS_INFO_STREAM("KeyFrame.cpp Level3: " << level3);
+          }
+          else
           {
-              fast_corner_detect_10(lev.image, lev.vCorners, 10);
-              lev.nFastThresh = 10;
+            fast_corner_detect_10(lev.image, lev.vCorners, 10);
+            lev.nFastThresh = 10;
+            //ROS_INFO_STREAM("KeyFrame.cpp Level3: " << level3);
           }
+      }
 
           //do nonmax suppression
 

src/Main.cpp

@@ -40,11 +40,13 @@
  ****************************************************************************************/
 
 #include <mcptam/System.h>
+#include <mcptam/ThresholdConfig.h>
+#include <mcptam/GuiCallBacks.h> 
 #include <mcptam/LoadStaticParamsGeneral.h>
 #include <mcptam/LoadStaticParamsClient.h>
 #include <mcptam/LoadStaticParamsServer.h>
 #include <ros/ros.h>
-
+#include <dynamic_reconfigure/server.h>
 #include <ros/console.h>
 #include <log4cxx/logger.h>
 
@@ -88,6 +90,11 @@ int main(int argc, char** argv)
   LoadStaticParamsClient();
   LoadStaticParamsServer();
 
+  dynamic_reconfigure::Server<mcptam::ThresholdConfig> threshold;
+  dynamic_reconfigure::Server<mcptam::ThresholdConfig>::CallbackType f;
+  f = boost::bind(&threshold_gui_callback, _1, _2);
+  threshold.setCallback(f);
+
   try
   {
     System sys;

src/System.cpp

@@ -199,6 +199,7 @@ void System::Run()
   // Loop until instructed to stop
   while (!mbDone && ros::ok())
   {
+    ros::spinOnce(); //SABA spin
     if (mpGLWindow)
     {
       // Required before drawing

testgui.txt

@@ -0,0 +1,83 @@
+[ INFO] [1471566210.921360047]:   Welcome to mcptam 
+[ INFO] [1471566210.921410275]:   --------------- 
+[ INFO] [1471566210.921423191]:   Multiple Camera Parallel Tracking and Mapping (MCPTAM
+[ INFO] [1471566210.921434976]:   Copyright 2014 Adam Harmat, McGill University
+[ INFO] [1471566210.921447778]:                  Michael Tribou, University of Waterloo
+[ INFO] [1471566210.921458610]:   
+[ INFO] [1471566210.921471629]:   Multi-Camera Parallel Tracking and Mapping (MCPTAM) is free software:
+[ INFO] [1471566210.921483936]:   you can redistribute it and/or modify it under the terms of the GNU 
+[ INFO] [1471566210.921496916]:   General Public License as published by the Free Software Foundation,
+[ INFO] [1471566210.921509934]:   either version 3 of the License, or (at your option) any later
+[ INFO] [1471566210.921522973]:   version.
+[ INFO] [1471566210.921536647]:   
+[ INFO] [1471566210.921549735]:   This program is distributed in the hope that it will be useful,
+[ INFO] [1471566210.921563597]:   but WITHOUT ANY WARRANTY; without even the implied warranty of
+[ INFO] [1471566210.921576525]:   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+[ INFO] [1471566210.921590865]:   GNU General Public License for more details.
+[ INFO] [1471566210.921603177]:   
+[ INFO] [1471566210.921616082]:   You should have received a copy of the GNU General Public License
+[ INFO] [1471566210.921628856]:   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+[ INFO] [1471566210.921640885]:   
+[ INFO] [1471566210.921653723]:   Based on Parallel Tracking and Mapping (PTAM) software
+[ INFO] [1471566210.921666096]:   Copyright 2008 Isis Innovation Limited
+[ INFO] [1471566210.921678250]:   
+[ INFO] [1471566210.953190727]: VideoSourceMulti: Initializing
+[ INFO] [1471566210.954122249]: CameraGroupsParser: Adding camera camera1 to group 0
+[ INFO] [1471566210.954155647]: CameraGroupsParser: Adding camera camera2 to group 0
+[ INFO] [1471566210.954173374]: CameraGroupsParser: Adding camera camera3 to group 0
+[ INFO] [1471566210.954197477]: VideoSourceMulti: Creating camera group from camera1
+[ INFO] [1471566210.954229142]: VideoSourceMulti: Creating camera group from camera2
+[ INFO] [1471566210.954245361]: VideoSourceMulti: Creating camera group from camera3
+[ INFO] [1471566210.972145881]: CameraGroupSubscriber: Loading 3 cameras
+[ INFO] [1471566211.039596284]: CameraGroupSubscriber: Waiting for camera info from all cameras
+... logging to /home/saba/.ros/log/ebdc81b2-65a1-11e6-bd85-e4b3189fb77b/roslaunch-saba-27379.log
+Checking log directory for disk usage. This may take awhile.
+Press Ctrl-C to interrupt
+Done checking log file disk usage. Usage is <1GB.
+]2;/home/saba/catkin_ws/src/mcptam/launch/mcptam.launch
+started roslaunch server http://saba:44680/
+
+SUMMARY
+========
+
+CLEAR PARAMETERS
+ * /mcptam/
+
+PARAMETERS
+ * /mcptam/MKF_BUFFER_CAPACITY: 55
+ * /mcptam/USE_CPER: False
+ * /mcptam/adjuster_recent_min_size: 60
+ * /mcptam/cam_group_list: [['camera1', 'cam...
+ * /mcptam/dynamic_sync: True
+ * /mcptam/get_pose_separately: False
+ * /mcptam/headless: False
+ * /mcptam/image_transport: compressed
+ * /mcptam/kf_adaptive_thresh: True
+ * /mcptam/kf_distance_mean_diff_fraction: 0.1
+ * /mcptam/level_zero_points: True
+ * /mcptam/mm_init_cov_thresh: 0.01
+ * /mcptam/mm_init_point_max_num: 200
+ * /mcptam/mm_init_point_mode: both
+ * /mcptam/mm_max_scaled_mkf_dist: 0.3
+ * /mcptam/mm_outlier_multiplier: 1.1
+ * /rosdistro: indigo
+ * /rosversion: 1.11.20
+
+NODES
+  /
+    mcptam (mcptam/mcptam)
+    rqt_reconfigure (rqt_reconfigure/rqt_reconfigure)
+    world_tf (tf/static_transform_publisher)
+
+ROS_MASTER_URI=http://localhost:11311
+]2;/home/saba/catkin_ws/src/mcptam/launch/mcptam.launch http://localhost:11311
+core service [/rosout] found
+process[rqt_reconfigure-1]: started with pid [27398]
+process[mcptam-2]: started with pid [27399]
+process[world_tf-3]: started with pid [27400]
+[world_tf-3] killing on exit
+[mcptam-2] killing on exit
+[rqt_reconfigure-1] killing on exit
+shutting down processing monitor...
+... shutting down processing monitor complete
+done