uzh-rpg · Changliu52 · Sep 30, 2015 · Sep 30, 2015 · Oct 9, 2015 · Oct 9, 2015
diff --git a/rqt_svo/CMakeLists.txt b/rqt_svo/CMakeLists.txt
diff --git a/rqt_svo/README.md b/rqt_svo/README.md
diff --git a/rqt_svo/package.xml b/rqt_svo/package.xml
diff --git a/rqt_svo/plugin.xml b/rqt_svo/plugin.xml
diff --git a/rqt_svo/resource/widget.ui b/rqt_svo/resource/widget.ui
diff --git a/rqt_svo/setup.py b/rqt_svo/setup.py
diff --git a/rqt_svo/src/rqt_svo/.gitignore b/rqt_svo/src/rqt_svo/.gitignore
diff --git a/rqt_svo/src/rqt_svo/__init__.py b/rqt_svo/src/rqt_svo/__init__.py
diff --git a/rqt_svo/src/rqt_svo/svo.py b/rqt_svo/src/rqt_svo/svo.py
diff --git a/rqt_svo/src/rqt_svo/svo_widget.py b/rqt_svo/src/rqt_svo/svo_widget.py
diff --git a/svo/CMakeLists.txt b/svo/CMakeLists.txt
diff --git a/svo/CMakeModules/FindEigen.cmake b/svo/CMakeModules/FindEigen.cmake
diff --git a/svo/CMakeModules/FindG2O.cmake b/svo/CMakeModules/FindG2O.cmake
diff --git a/svo/Doxyfile b/svo/Doxyfile
diff --git a/svo/doc/.gitignore b/svo/doc/.gitignore
diff --git a/svo/doc/notation.ipe b/svo/doc/notation.ipe
diff --git a/svo/doc/notation.pdf b/svo/doc/notation.pdf
diff --git a/svo/doc/notation.png b/svo/doc/notation.png
diff --git a/svo/include/svo/bundle_adjustment.h b/svo/include/svo/bundle_adjustment.h
diff --git a/svo/include/svo/config.h b/svo/include/svo/config.h
diff --git a/svo/include/svo/depth_filter.h b/svo/include/svo/depth_filter.h
diff --git a/svo/include/svo/feature.h b/svo/include/svo/feature.h
diff --git a/svo/include/svo/feature_alignment.h b/svo/include/svo/feature_alignment.h
diff --git a/svo/include/svo/feature_detection.h b/svo/include/svo/feature_detection.h
diff --git a/svo/include/svo/frame.h b/svo/include/svo/frame.h
diff --git a/svo/include/svo/frame_handler_base.h b/svo/include/svo/frame_handler_base.h
diff --git a/svo/include/svo/frame_handler_mono.h b/svo/include/svo/frame_handler_mono.h
diff --git a/svo/include/svo/global.h b/svo/include/svo/global.h
diff --git a/svo/include/svo/initialization.h b/svo/include/svo/initialization.h
diff --git a/svo/include/svo/map.h b/svo/include/svo/map.h
diff --git a/svo/include/svo/matcher.h b/svo/include/svo/matcher.h
diff --git a/svo/include/svo/point.h b/svo/include/svo/point.h
diff --git a/svo/include/svo/pose_optimizer.h b/svo/include/svo/pose_optimizer.h
diff --git a/svo/include/svo/reprojector.h b/svo/include/svo/reprojector.h
diff --git a/svo/include/svo/sparse_img_align.h b/svo/include/svo/sparse_img_align.h
diff --git a/svo/package.xml b/svo/package.xml
diff --git a/svo/src/bundle_adjustment.cpp b/svo/src/bundle_adjustment.cpp
diff --git a/svo/src/config.cpp b/svo/src/config.cpp
diff --git a/svo/src/depth_filter.cpp b/svo/src/depth_filter.cpp
diff --git a/svo/src/feature_alignment.cpp b/svo/src/feature_alignment.cpp
diff --git a/svo/src/feature_detection.cpp b/svo/src/feature_detection.cpp
diff --git a/svo/src/frame.cpp b/svo/src/frame.cpp
diff --git a/svo/src/frame_handler_base.cpp b/svo/src/frame_handler_base.cpp
@@ -136,7 +136,11 @@ int FrameHandlerBase::finishFrameProcessingCommon(
     tracking_quality_ = TRACKING_INSUFFICIENT;
   }
   else if (dropout == RESULT_FAILURE)
+  {
     resetAll();
+    stage_ = STAGE_RELOCALIZING;
+    tracking_quality_ = TRACKING_INSUFFICIENT;
+  }
   if(set_reset_)
     resetAll();
 

diff --git a/svo/src/frame_handler_mono.cpp b/svo/src/frame_handler_mono.cpp
diff --git a/svo/src/initialization.cpp b/svo/src/initialization.cpp
@@ -30,9 +30,9 @@ InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
 {
   reset();
   detectFeatures(frame_ref, px_ref_, f_ref_);
-  if(px_ref_.size() < 100)
+  if(px_ref_.size() < 50)
   {
-    SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 100 features. Retry in more textured environment.");
+    SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 50 features. Retry in more textured environment.");
     return FAILURE;
   }
   frame_ref_ = frame_ref;

diff --git a/svo/src/map.cpp b/svo/src/map.cpp
diff --git a/svo/src/matcher.cpp b/svo/src/matcher.cpp
diff --git a/svo/src/point.cpp b/svo/src/point.cpp
diff --git a/svo/src/pose_optimizer.cpp b/svo/src/pose_optimizer.cpp
diff --git a/svo/src/reprojector.cpp b/svo/src/reprojector.cpp
diff --git a/svo/src/sparse_img_align.cpp b/svo/src/sparse_img_align.cpp
diff --git a/svo/test/README.md b/svo/test/README.md
diff --git a/svo/test/benchmark.csv b/svo/test/benchmark.csv
diff --git a/svo/test/data/.gitignore b/svo/test/data/.gitignore
diff --git a/svo/test/results/.gitignore b/svo/test/results/.gitignore
diff --git a/svo/test/test_depth_filter.cpp b/svo/test/test_depth_filter.cpp
diff --git a/svo/test/test_feature_alignment.cpp b/svo/test/test_feature_alignment.cpp
diff --git a/svo/test/test_feature_detection.cpp b/svo/test/test_feature_detection.cpp
diff --git a/svo/test/test_matcher.cpp b/svo/test/test_matcher.cpp
diff --git a/svo/test/test_pipeline.cpp b/svo/test/test_pipeline.cpp
diff --git a/svo/test/test_pose_optimizer.cpp b/svo/test/test_pose_optimizer.cpp
diff --git a/svo/test/test_sparse_img_align.cpp b/svo/test/test_sparse_img_align.cpp
diff --git a/svo/test/test_utils.h b/svo/test/test_utils.h
diff --git a/svo_analysis/.gitignore b/svo_analysis/.gitignore
diff --git a/svo_analysis/CMakeLists.txt b/svo_analysis/CMakeLists.txt
diff --git a/svo_analysis/README.md b/svo_analysis/README.md
diff --git a/svo_analysis/comparisons/speed.yaml b/svo_analysis/comparisons/speed.yaml
diff --git a/svo_analysis/comparisons/textures.yaml b/svo_analysis/comparisons/textures.yaml
diff --git a/svo_analysis/experiments/.gitignore b/svo_analysis/experiments/.gitignore
diff --git a/svo_analysis/experiments/asl2_acc.yaml b/svo_analysis/experiments/asl2_acc.yaml
diff --git a/svo_analysis/experiments/asl2_fast.yaml b/svo_analysis/experiments/asl2_fast.yaml
diff --git a/svo_analysis/experiments/flying_room_1_fast.yaml b/svo_analysis/experiments/flying_room_1_fast.yaml
diff --git a/svo_analysis/experiments/synthetic.yaml b/svo_analysis/experiments/synthetic.yaml
diff --git a/svo_analysis/package.xml b/svo_analysis/package.xml
diff --git a/svo_analysis/results/.gitignore b/svo_analysis/results/.gitignore
diff --git a/svo_analysis/setup.py b/svo_analysis/setup.py
diff --git a/svo_analysis/src/svo_analysis/__init__.py b/svo_analysis/src/svo_analysis/__init__.py
diff --git a/svo_analysis/src/svo_analysis/analyse_dataset.py b/svo_analysis/src/svo_analysis/analyse_dataset.py
diff --git a/svo_analysis/src/svo_analysis/filter_groundtruth_smooth.py b/svo_analysis/src/svo_analysis/filter_groundtruth_smooth.py
diff --git a/svo_analysis/src/svo_analysis/hand_eye_calib.py b/svo_analysis/src/svo_analysis/hand_eye_calib.py
diff --git a/svo_analysis/src/svo_analysis/tum_benchmark_tools/__init__.py b/svo_analysis/src/svo_analysis/tum_benchmark_tools/__init__.py
diff --git a/svo_msgs/CMakeLists.txt b/svo_msgs/CMakeLists.txt
diff --git a/svo_msgs/msg/DenseInput.msg b/svo_msgs/msg/DenseInput.msg
diff --git a/svo_msgs/msg/Feature.msg b/svo_msgs/msg/Feature.msg
diff --git a/svo_msgs/msg/Info.msg b/svo_msgs/msg/Info.msg
diff --git a/svo_msgs/msg/NbvTrajectory.msg b/svo_msgs/msg/NbvTrajectory.msg
diff --git a/svo_msgs/package.xml b/svo_msgs/package.xml
diff --git a/svo_ros/CMakeLists.txt b/svo_ros/CMakeLists.txt
diff --git a/svo_ros/include/svo_ros/dataset_img.h b/svo_ros/include/svo_ros/dataset_img.h
diff --git a/svo_ros/include/svo_ros/visualizer.h b/svo_ros/include/svo_ros/visualizer.h
@@ -62,7 +62,10 @@ class Visualizer
   bool publish_map_every_frame_;
   ros::Duration publish_points_display_time_;
   SE3 T_world_from_vision_;
-
+  Matrix3d camera_facing_;
+
+  double quality_reading_;//############## stores active slam quality estimation
+
   Visualizer();
 
   ~Visualizer() {};

diff --git a/svo_ros/launch/live.launch b/svo_ros/launch/live.launch
diff --git a/svo_ros/launch/test_rig3.launch b/svo_ros/launch/test_rig3.launch
diff --git a/svo_ros/launch/testueyeSVO.launch b/svo_ros/launch/testueyeSVO.launch
@@ -0,0 +1,40 @@
+<launch>
+  <arg name="nodelet_manager_name" value="nodelet_manager" />
+  <arg name="camera_name" value="camera" />
+
+<node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)" args="manager" output="screen" launch-prefix="taskset -c 0"/>
+
+<node pkg="nodelet" type="nodelet" name="ueye_cam_nodelet" args="load ueye_cam/ueye_cam_nodelet $(arg nodelet_manager_name)" launch-prefix="taskset -c 4">
+
+    <param name="camera_name" type="str" value="$(arg camera_name)" /> <!-- == namespace for topics and services -->
+    <param name="camera_topic" type="str" value="image_raw" />
+    <param name="camera_id" type="int" value="0" /> <!-- 0 = any camera; 1+: camera ID -->
+
+    <param name="ext_trigger_mode" type="bool" value="False" />
+    <param name="color_mode" type="str" value="mono8" /> <!-- valid options: 'rgb8', 'mono8', 'bayer_rggb8' -->
+
+    <param name="image_width" type="int" value="752" />
+    <param name="image_height" type="int" value="480" />
+
+    <param name="auto_gain" type="bool" value="False" />
+    <param name="master_gain" type="int" value="0" />
+    <param name="gain_boost" type="bool" value="False" />
+
+    <param name="auto_exposure" type="bool" value="False" />
+    <param name="exposure" type="int" value="2" />  <!-- in ms -->
+
+    <param name="auto_frame_rate" type="bool" value="False" />
+    <param name="frame_rate" type="double" value="50.0" />
+
+
+</node>
+
+
+<node pkg="svo_ros" type="vo" name="svo" clear_params="true"  launch-prefix="taskset -c 4">
+        <param name="cam_topic" value="/camera/image_raw" type="str" />
+        <rosparam file="$(find svo_ros)/param/ueyeIR_atan.yaml" />
+        <rosparam file="$(find svo_ros)/param/vo_mine.yaml" />
+	<param name="accept_console_user_input" value="true" />
+	<param name="publish_world_in_cam_frame" value="false" />
+	<param name="init_rx" value="3.14159265" />
+</node>
diff --git a/svo_ros/package.xml b/svo_ros/package.xml
diff --git a/svo_ros/param/VO2701_atan.yaml b/svo_ros/param/VO2701_atan.yaml
@@ -0,0 +1,8 @@
+cam_model: ATAN
+cam_width: 640
+cam_height: 480
+cam_fx: 0.4425
+cam_fy: 0.691
+cam_cx: 0.5052
+cam_cy: 0.4578
+cam_d0: 0.9645
diff --git a/svo_ros/param/camera_atan.yaml b/svo_ros/param/camera_atan.yaml
@@ -1,8 +1,8 @@
 cam_model: ATAN
-cam_width: 752
+cam_width: 640
 cam_height: 480
-cam_fx: 0.509326
-cam_fy: 0.796651
-cam_cx: 0.45905
-cam_cy: 0.510056
-cam_d0: 0.9320
+cam_fx: 1.06326
+cam_fy: 1.41474
+cam_cx: 0.488711
+cam_cy: 0.478752
+cam_d0: 0.171631
diff --git a/svo_ros/param/camera_pinhole.yaml b/svo_ros/param/camera_pinhole.yaml
@@ -1,11 +1,11 @@
 cam_model: Pinhole
-cam_width: 752
+cam_width: 640
 cam_height: 480
-cam_fx: 414.536145
-cam_fy: 414.284429
-cam_cx: 348.804988
-cam_cy: 240.076451
-cam_d0: -0.283076
-cam_d1: 0.066674
-cam_d2: 0.000896
-cam_d3: 0.000778
+cam_fx: 676.7234
+cam_fy: 674.9319
+cam_cx: 316.3583
+cam_cy: 226.1596
+cam_d0: 0.1317
+cam_d1: -0.9365
+cam_d2: 0.00011586
+cam_d3: 0.0032
diff --git a/svo_ros/param/ueyeIR_atan.yaml b/svo_ros/param/ueyeIR_atan.yaml
@@ -0,0 +1,9 @@
+cam_model: ATAN
+cam_width: 752
+cam_height: 480
+cam_fx: 0.451172
+cam_fy: 0.70574
+cam_cx: 0.500868
+cam_cy: 0.452328
+cam_d0: 0.966227
+
diff --git a/svo_ros/param/ueye_atan.yaml b/svo_ros/param/ueye_atan.yaml
@@ -0,0 +1,8 @@
+cam_model: ATAN
+cam_width: 752
+cam_height: 480
+cam_fx: 0.4425
+cam_fy: 0.691
+cam_cx: 0.5052
+cam_cy: 0.4578
+cam_d0: 0.9645
diff --git a/svo_ros/param/vo_accurate.yaml b/svo_ros/param/vo_accurate.yaml
diff --git a/svo_ros/param/vo_fast.yaml b/svo_ros/param/vo_fast.yaml
@@ -1,3 +1,10 @@
 grid_size: 30
 max_n_kfs: 10
-loba_num_iter: 0
+loba_num_iter: 0
+
+init_min_disparity: 30
+init_min_tracked: 35
+init_min_inliers: 20
+triang_min_corner_score: 10.0
+quality_min_fts: 30
+quality_max_drop_fts: 50
diff --git a/svo_ros/param/vo_mine.yaml b/svo_ros/param/vo_mine.yaml
@@ -0,0 +1,16 @@
+grid_size: 30
+max_n_kfs: 10
+loba_num_iter: 0
+n_pyr_levels: 5
+
+init_min_disparity: 30
+init_min_tracked: 35
+init_min_inliers: 20
+
+max_fts: 250
+triang_min_corner_score: 5.0
+quality_min_fts: 30
+quality_max_drop_fts: 130
+
+structureoptim_max_pts: 30
+structureoptim_num_iter: 5
diff --git a/svo_ros/param/vo_px4.yaml b/svo_ros/param/vo_px4.yaml
@@ -0,0 +1,4 @@
+grid_size: 30
+max_n_kfs: 10
+loba_num_iter: 0
+max_fts: 180
diff --git a/svo_ros/rqt.perspective b/svo_ros/rqt.perspective
diff --git a/svo_ros/rviz_config.rviz b/svo_ros/rviz_config.rviz
diff --git a/svo_ros/src/benchmark_node.cpp b/svo_ros/src/benchmark_node.cpp
diff --git a/svo_ros/src/visualizer.cpp b/svo_ros/src/visualizer.cpp
@@ -47,12 +47,13 @@ Visualizer() :
     publish_world_in_cam_frame_(vk::getParam<bool>("svo/publish_world_in_cam_frame", true)),
     publish_map_every_frame_(vk::getParam<bool>("svo/publish_map_every_frame", false)),
     publish_points_display_time_(vk::getParam<double>("svo/publish_point_display_time", 0)),
-    T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero())
+    T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero()),
+    quality_reading_(0.006)
 {
   // Init ROS Marker Publishers
   pub_frames_ = pnh_.advertise<visualization_msgs::Marker>("keyframes", 10);
   pub_points_ = pnh_.advertise<visualization_msgs::Marker>("points", 1000);
-  pub_pose_ = pnh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose",10);
+  pub_pose_ = pnh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose",1);
   pub_info_ = pnh_.advertise<svo_msgs::Info>("info", 10);
   pub_dense_ = pnh_.advertise<svo_msgs::DenseInput>("dense_input",10);
 
@@ -82,13 +83,16 @@ void Visualizer::publishMinimal(
     msg_info.keyframes.resize(slam.map().keyframes_.size());
     for(list<FramePtr>::const_iterator it=slam.map().keyframes_.begin(); it!=slam.map().keyframes_.end(); ++it)
       msg_info.keyframes.push_back((*it)->id_);
+
     msg_info.stage = static_cast<int>(slam.stage());
     msg_info.tracking_quality = static_cast<int>(slam.trackingQuality());
+
     if(frame != NULL)
       msg_info.num_matches = slam.lastNumObservations();
     else
       msg_info.num_matches = 0;
     pub_info_.publish(msg_info);
+
   }
 
   if(frame == NULL)
@@ -178,7 +182,8 @@ void Visualizer::publishMinimal(
     {
       // publish cam in world frame
       SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse());
-      q = Quaterniond(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose());
+      //q = Quaterniond(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose());
+      q = Quaterniond(T_world_from_cam.rotation_matrix() * camera_facing_.transpose());//#####################here remember to derotate the camera facing
       p = T_world_from_cam.translation();
       Cov = T_world_from_cam.Adj()*frame->Cov_*T_world_from_cam.inverse().Adj();
     }
@@ -204,19 +209,20 @@ void Visualizer::visualizeMarkers(
 {
   if(frame == NULL)
     return;
-
+  // Publish /tf
   vk::output_helper::publishTfTransform(
       frame->T_f_w_*T_world_from_vision_.inverse(),
       ros::Time(frame->timestamp_), "cam_pos", "world", br_);
-
+
+  // Publish markers
   if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0)
   {
-    vk::output_helper::publishCameraMarker(
+    vk::output_helper::publishHexacopterMarker(
         pub_frames_, "cam_pos", "cams", ros::Time(frame->timestamp_),
-        1, 0.3, Vector3d(0.,0.,1.));
+        1, 0, 0.3, Vector3d(0.,0.,1.));
     vk::output_helper::publishPointMarker(
         pub_points_, T_world_from_vision_*frame->pos(), "trajectory",
-        ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5));
+        ros::Time::now(), trace_id_, 0, quality_reading_, Vector3d(0.,0.,0.5)); //###########
     if(frame->isKeyframe() || publish_map_every_frame_)
       publishMapRegion(core_kfs);
     removeDeletedPts(map);
@@ -242,6 +248,8 @@ void Visualizer::removeDeletedPts(const Map& map)
   }
 }
 
+
+// feature map!!!!!!!!!!!!!!!
 void Visualizer::displayKeyframeWithMps(const FramePtr& frame, int ts)
 {
   // publish keyframe
@@ -261,12 +269,13 @@ void Visualizer::displayKeyframeWithMps(const FramePtr& frame, int ts)
 
     vk::output_helper::publishPointMarker(
         pub_points_, T_world_from_vision_*(*it)->point->pos_, "pts",
-        ros::Time::now(), (*it)->point->id_, 0, 0.005, Vector3d(1.0, 0., 1.0),
+        ros::Time::now(), (*it)->point->id_, 0, 0.02, Vector3d(1.0, 0., 1.0),
         publish_points_display_time_);
     (*it)->point->last_published_ts_ = ts;
   }
 }
 
+
 void Visualizer::exportToDense(const FramePtr& frame)
 {
   // publish air_ground_msgs

diff --git a/svo_ros/src/vo_node.cpp b/svo_ros/src/vo_node.cpp
@@ -35,6 +35,10 @@
 #include <vikit/camera_loader.h>
 #include <vikit/user_input_thread.h>
 
+#include <geometry_msgs/TransformStamped.h>//#####################################3
+#include <std_msgs/Bool.h>//#####################
+#include <math.h>
+
 namespace svo {
 
 /// SVO Interface
@@ -55,6 +59,12 @@ class VoNode
   void imgCb(const sensor_msgs::ImageConstPtr& msg);
   void processUserActions();
   void remoteKeyCb(const std_msgs::StringConstPtr& key_input);
+  void resetCallback(const geometry_msgs::TransformStamped::ConstPtr& msgin);//############################
+
+  ros::Subscriber sub_quality_;
+  void qualityCallback(const geometry_msgs::Vector3& msgin); // receive message from active slam
+
+  ros::Publisher pub_usereset_;
 };
 
 VoNode::VoNode() :
@@ -74,14 +84,16 @@ VoNode::VoNode() :
     throw std::runtime_error("Camera model not correctly specified.");
 
   // Get initial position and orientation
+  visualizer_.camera_facing_ = vk::rpy2dcm(Vector3d(vk::getParam<double>("svo/init_rx", 0.0),
+						    vk::getParam<double>("svo/init_ry", 0.0),
+						    vk::getParam<double>("svo/init_rz", 0.0))); // #############log the camera setup (downward facing)
   visualizer_.T_world_from_vision_ = Sophus::SE3(
-      vk::rpy2dcm(Vector3d(vk::getParam<double>("svo/init_rx", 0.0),
-                           vk::getParam<double>("svo/init_ry", 0.0),
-                           vk::getParam<double>("svo/init_rz", 0.0))),
+      visualizer_.camera_facing_,
       Eigen::Vector3d(vk::getParam<double>("svo/init_tx", 0.0),
                       vk::getParam<double>("svo/init_ty", 0.0),
                       vk::getParam<double>("svo/init_tz", 0.0)));
 
+
   // Init VO and start
   vo_ = new svo::FrameHandlerMono(cam_);
   vo_->start();
@@ -140,20 +152,53 @@ void VoNode::processUserActions()
       printf("SVO user input: RESET\n");
       break;
     case 's':
-      vo_->start();
+      pub_usereset_.publish(1);
+      //vo_->start();
       printf("SVO user input: START\n");
       break;
     default: ;
   }
 }
 
+// reset call back here!!####
 void VoNode::remoteKeyCb(const std_msgs::StringConstPtr& key_input)
 {
   remote_input_ = key_input->data;
 }
 
+//______________________________###############################################
+// Filter reset callback function
+//void VoNode::resetCallback(const std_msgs::Bool::ConstPtr& msgin)
+void VoNode::resetCallback(const geometry_msgs::TransformStamped::ConstPtr& msgin)
+{
+  visualizer_.T_world_from_vision_ = Sophus::SE3(
+	vk::quat2dcm(Vector4d((double)msgin->transform.rotation.w,
+			      (double)msgin->transform.rotation.x,
+			      (double)msgin->transform.rotation.y,
+                              (double)msgin->transform.rotation.z)) * visualizer_.camera_facing_,
+	Eigen::Vector3d((double)msgin->transform.translation.x,
+                        (double)msgin->transform.translation.y,
+                        (double)msgin->transform.translation.z));
+  vo_->start();
+  printf("SVO user input: START\n");
+}
+
+//__________________________________#####################
+// active slam quality message
+void VoNode::qualityCallback(const geometry_msgs::Vector3& msgin)
+{
+  if (msgin.x > 0.001) return;
+  visualizer_.quality_reading_ = msgin.x*700.0;
+  std::cout << "The scaled quality is" << visualizer_.quality_reading_ << std::endl;
+}
+
 } // namespace svo
 
+
+
+//_________________________
+// MIAN
+//_____________________________
 int main(int argc, char **argv)
 {
   ros::init(argc, argv, "svo");
@@ -164,11 +209,14 @@ int main(int argc, char **argv)
   // subscribe to cam msgs
   std::string cam_topic(vk::getParam<std::string>("svo/cam_topic", "camera/image_raw"));
   image_transport::ImageTransport it(nh);
-  image_transport::Subscriber it_sub = it.subscribe(cam_topic, 5, &svo::VoNode::imgCb, &vo_node);
-
-  // subscribe to remote input
-  vo_node.sub_remote_key_ = nh.subscribe("svo/remote_key", 5, &svo::VoNode::remoteKeyCb, &vo_node);
-
+  image_transport::Subscriber it_sub = it.subscribe(cam_topic, 1, &svo::VoNode::imgCb, &vo_node);
+
+  // subscribe to remote input#######################################################
+  //vo_node.sub_remote_key_ = nh.subscribe("svo/remote_key", 5, &svo::VoNode::remoteKeyCb, &vo_node);
+  vo_node.sub_remote_key_ = nh.subscribe("Allreset", 10, &svo::VoNode::resetCallback, &vo_node);
+  vo_node.pub_usereset_   = nh.advertise<std_msgs::Bool>("svo/usereset", 10);;
+  vo_node.sub_quality_    = nh.subscribe("aslam/quality", 100, &svo::VoNode::qualityCallback, &vo_node);
+
   // start processing callbacks
   while(ros::ok() && !vo_node.quit_)
   {