From e9c94436b0957a8c79437a4f5ddb10ee057273c1 Mon Sep 17 00:00:00 2001 From: Chang Liu Date: Wed, 30 Sep 2015 14:15:11 +0100 Subject: [PATCH 1/6] Initial commit for modifications. --- svo/src/depth_filter.cpp | 2 +- svo_ros/include/svo_ros/visualizer.h | 3 +- svo_ros/param/camera_atan.yaml | 12 ++++---- svo_ros/param/camera_pinhole.yaml | 18 +++++------ svo_ros/param/ueye_atan.yaml | 8 +++++ svo_ros/param/vo_mine.yaml | 4 +++ svo_ros/param/vo_px4.yaml | 4 +++ svo_ros/src/visualizer.cpp | 11 +++++-- svo_ros/src/vo_node.cpp | 45 +++++++++++++++++++++++----- 9 files changed, 79 insertions(+), 28 deletions(-) create mode 100644 svo_ros/param/ueye_atan.yaml create mode 100644 svo_ros/param/vo_mine.yaml create mode 100644 svo_ros/param/vo_px4.yaml diff --git a/svo/src/depth_filter.cpp b/svo/src/depth_filter.cpp index 1f70be89..bf3a686d 100644 --- a/svo/src/depth_filter.cpp +++ b/svo/src/depth_filter.cpp @@ -225,7 +225,7 @@ void DepthFilter::updateSeeds(FramePtr frame) ++it; // behind the camera continue; } - if(!it->ftr->frame->cam_->isInFrame(it->ftr->frame->f2c(xyz_f).cast())) { + if(!frame->cam_->isInFrame(frame->f2c(xyz_f).cast())) { ++it; // point does not project in image continue; } diff --git a/svo_ros/include/svo_ros/visualizer.h b/svo_ros/include/svo_ros/visualizer.h index dc8936da..c5188f31 100644 --- a/svo_ros/include/svo_ros/visualizer.h +++ b/svo_ros/include/svo_ros/visualizer.h @@ -62,7 +62,8 @@ class Visualizer bool publish_map_every_frame_; ros::Duration publish_points_display_time_; SE3 T_world_from_vision_; - + Matrix3d camera_facing_; + Visualizer(); ~Visualizer() {}; diff --git a/svo_ros/param/camera_atan.yaml b/svo_ros/param/camera_atan.yaml index e3c625c1..a53c8506 100644 --- 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 \ No newline at end of file +cam_fx: 1.06326 +cam_fy: 1.41474 +cam_cx: 0.488711 +cam_cy: 0.478752 +cam_d0: 0.171631 \ No newline at end of file diff --git a/svo_ros/param/camera_pinhole.yaml b/svo_ros/param/camera_pinhole.yaml index 4898bcde..aa4ea9cb 100644 --- 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 \ No newline at end of file +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 \ No newline at end of file diff --git a/svo_ros/param/ueye_atan.yaml b/svo_ros/param/ueye_atan.yaml new file mode 100644 index 00000000..c7649a78 --- /dev/null +++ b/svo_ros/param/ueye_atan.yaml @@ -0,0 +1,8 @@ +cam_model: ATAN +cam_width: 752 +cam_height: 480 +cam_fx: 0.439782 +cam_fy: 0.6874 +cam_cx: 0.510006 +cam_cy: 0.458044 +cam_d0: 0.960589 diff --git a/svo_ros/param/vo_mine.yaml b/svo_ros/param/vo_mine.yaml new file mode 100644 index 00000000..9b3bdbba --- /dev/null +++ b/svo_ros/param/vo_mine.yaml @@ -0,0 +1,4 @@ +grid_size: 30 +max_n_kfs: 10 +loba_num_iter: 2 +max_fts: 180 \ No newline at end of file diff --git a/svo_ros/param/vo_px4.yaml b/svo_ros/param/vo_px4.yaml new file mode 100644 index 00000000..9fd87aff --- /dev/null +++ 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/src/visualizer.cpp b/svo_ros/src/visualizer.cpp index a17589f5..ffa451e3 100644 --- a/svo_ros/src/visualizer.cpp +++ b/svo_ros/src/visualizer.cpp @@ -178,7 +178,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,11 +205,12 @@ 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::publishHexacopterMarker( @@ -242,6 +244,8 @@ void Visualizer::removeDeletedPts(const Map& map) } } + +// feature map!!!!!!!!!!!!!!! void Visualizer::displayKeyframeWithMps(const FramePtr& frame, int ts) { // publish keyframe @@ -267,6 +271,7 @@ void Visualizer::displayKeyframeWithMps(const FramePtr& frame, int 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 index 8a6f88c2..bab55a26 100644 --- a/svo_ros/src/vo_node.cpp +++ b/svo_ros/src/vo_node.cpp @@ -35,6 +35,9 @@ #include #include +#include //#####################################3 +#include //##################### + namespace svo { /// SVO Interface @@ -55,6 +58,8 @@ 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::Publisher pub_usereset_; }; VoNode::VoNode() : @@ -74,17 +79,19 @@ VoNode::VoNode() : throw std::runtime_error("Camera model not correctly specified."); // Get initial position and orientation + visualizer_.camera_facing_ = vk::rpy2dcm(Vector3d(vk::getParam("svo/init_rx", 0.0), + vk::getParam("svo/init_ry", 0.0), + vk::getParam("svo/init_rz", 0.0))); // #############log the camera setup (downward facing) visualizer_.T_world_from_vision_ = Sophus::SE3( - vk::rpy2dcm(Vector3d(vk::getParam("svo/init_rx", 0.0), - vk::getParam("svo/init_ry", 0.0), - vk::getParam("svo/init_rz", 0.0))), + visualizer_.camera_facing_, Eigen::Vector3d(vk::getParam("svo/init_tx", 0.0), vk::getParam("svo/init_ty", 0.0), vk::getParam("svo/init_tz", 0.0))); + // Init VO and start vo_ = new svo::FrameHandlerMono(cam_); - vo_->start(); + vo_->reset(); } VoNode::~VoNode() @@ -140,18 +147,38 @@ 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"); +} + + } // namespace svo int main(int argc, char **argv) @@ -166,9 +193,11 @@ int main(int argc, char **argv) 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); - + // 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("/svo/usereset", 10);; + // start processing callbacks while(ros::ok() && !vo_node.quit_) { From 65d119a1c1dcfe19b93776237ba333a34cea5727 Mon Sep 17 00:00:00 2001 From: Chang Liu Date: Fri, 9 Oct 2015 15:47:35 +0100 Subject: [PATCH 2/6] Automatic reinitialisation when initialisation step fails. --- svo/src/frame_handler_base.cpp | 3 +++ 1 file changed, 3 insertions(+) mode change 100644 => 100755 svo/src/frame_handler_base.cpp diff --git a/svo/src/frame_handler_base.cpp b/svo/src/frame_handler_base.cpp old mode 100644 new mode 100755 index 175ee01a..78476709 --- a/svo/src/frame_handler_base.cpp +++ b/svo/src/frame_handler_base.cpp @@ -136,7 +136,10 @@ int FrameHandlerBase::finishFrameProcessingCommon( tracking_quality_ = TRACKING_INSUFFICIENT; } else if (dropout == RESULT_FAILURE) + { resetAll(); + stage_ = STAGE_FIRST_FRAME; + } if(set_reset_) resetAll(); From 7f8435582eee313db18aa551bfeccc049095c963 Mon Sep 17 00:00:00 2001 From: Chang Liu Date: Fri, 9 Oct 2015 16:34:12 +0100 Subject: [PATCH 3/6] Automatic reinitialisation when initialisation step fails. --- svo/src/frame_handler_base.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/svo/src/frame_handler_base.cpp b/svo/src/frame_handler_base.cpp index 175ee01a..78476709 100644 --- a/svo/src/frame_handler_base.cpp +++ b/svo/src/frame_handler_base.cpp @@ -136,7 +136,10 @@ int FrameHandlerBase::finishFrameProcessingCommon( tracking_quality_ = TRACKING_INSUFFICIENT; } else if (dropout == RESULT_FAILURE) + { resetAll(); + stage_ = STAGE_FIRST_FRAME; + } if(set_reset_) resetAll(); From 11d1c583306f37e9dcd342a6bdbe1c9a56c741aa Mon Sep 17 00:00:00 2001 From: Chang Liu Date: Sun, 5 Mar 2017 18:34:51 +0000 Subject: [PATCH 4/6] dummy update to the latest version --- rqt_svo/CMakeLists.txt | 0 rqt_svo/README.md | 0 rqt_svo/package.xml | 0 rqt_svo/plugin.xml | 0 rqt_svo/resource/widget.ui | 0 rqt_svo/setup.py | 0 rqt_svo/src/rqt_svo/.gitignore | 0 rqt_svo/src/rqt_svo/__init__.py | 0 rqt_svo/src/rqt_svo/svo.py | 0 rqt_svo/src/rqt_svo/svo_widget.py | 0 svo/CMakeLists.txt | 0 svo/CMakeModules/FindEigen.cmake | 0 svo/CMakeModules/FindG2O.cmake | 0 svo/Doxyfile | 0 svo/doc/.gitignore | 0 svo/doc/notation.ipe | 0 svo/doc/notation.pdf | Bin svo/doc/notation.png | Bin svo/include/svo/bundle_adjustment.h | 0 svo/include/svo/config.h | 0 svo/include/svo/depth_filter.h | 0 svo/include/svo/feature.h | 0 svo/include/svo/feature_alignment.h | 0 svo/include/svo/feature_detection.h | 0 svo/include/svo/frame.h | 0 svo/include/svo/frame_handler_base.h | 0 svo/include/svo/frame_handler_mono.h | 0 svo/include/svo/global.h | 0 svo/include/svo/initialization.h | 0 svo/include/svo/map.h | 0 svo/include/svo/matcher.h | 0 svo/include/svo/point.h | 0 svo/include/svo/pose_optimizer.h | 0 svo/include/svo/reprojector.h | 0 svo/include/svo/sparse_img_align.h | 0 svo/package.xml | 0 svo/src/bundle_adjustment.cpp | 0 svo/src/config.cpp | 0 svo/src/depth_filter.cpp | 0 svo/src/feature_alignment.cpp | 0 svo/src/feature_detection.cpp | 0 svo/src/frame.cpp | 0 svo/src/frame_handler_base.cpp | 3 ++- svo/src/frame_handler_mono.cpp | 0 svo/src/initialization.cpp | 4 +-- svo/src/map.cpp | 0 svo/src/matcher.cpp | 0 svo/src/point.cpp | 0 svo/src/pose_optimizer.cpp | 0 svo/src/reprojector.cpp | 0 svo/src/sparse_img_align.cpp | 0 svo/test/README.md | 0 svo/test/benchmark.csv | 0 svo/test/data/.gitignore | 0 svo/test/results/.gitignore | 0 svo/test/test_depth_filter.cpp | 0 svo/test/test_feature_alignment.cpp | 0 svo/test/test_feature_detection.cpp | 0 svo/test/test_matcher.cpp | 0 svo/test/test_pipeline.cpp | 0 svo/test/test_pose_optimizer.cpp | 0 svo/test/test_sparse_img_align.cpp | 0 svo/test/test_utils.h | 0 svo_analysis/.gitignore | 0 svo_analysis/CMakeLists.txt | 0 svo_analysis/README.md | 0 svo_analysis/comparisons/speed.yaml | 0 svo_analysis/comparisons/textures.yaml | 0 svo_analysis/experiments/.gitignore | 0 svo_analysis/experiments/asl2_acc.yaml | 0 svo_analysis/experiments/asl2_fast.yaml | 0 .../experiments/flying_room_1_fast.yaml | 0 svo_analysis/experiments/synthetic.yaml | 0 svo_analysis/package.xml | 0 svo_analysis/results/.gitignore | 0 svo_analysis/setup.py | 0 svo_analysis/src/svo_analysis/__init__.py | 0 .../src/svo_analysis/analyse_dataset.py | 0 .../svo_analysis/filter_groundtruth_smooth.py | 0 .../src/svo_analysis/hand_eye_calib.py | 0 .../tum_benchmark_tools/__init__.py | 0 svo_msgs/CMakeLists.txt | 0 svo_msgs/msg/DenseInput.msg | 0 svo_msgs/msg/Feature.msg | 0 svo_msgs/msg/Info.msg | 0 svo_msgs/msg/NbvTrajectory.msg | 0 svo_msgs/package.xml | 0 svo_ros/CMakeLists.txt | 0 svo_ros/include/svo_ros/dataset_img.h | 0 svo_ros/include/svo_ros/visualizer.h | 2 ++ svo_ros/launch/live.launch | 0 svo_ros/launch/test_rig3.launch | 0 svo_ros/package.xml | 0 svo_ros/param/VO2701_atan.yaml | 8 ++++++ svo_ros/param/camera_atan.yaml | 0 svo_ros/param/camera_pinhole.yaml | 0 svo_ros/param/ueyeIR_atan.yaml | 9 +++++++ svo_ros/param/ueye_atan.yaml | 10 ++++---- svo_ros/param/vo_accurate.yaml | 0 svo_ros/param/vo_fast.yaml | 9 ++++++- svo_ros/param/vo_mine.yaml | 16 ++++++++++-- svo_ros/param/vo_px4.yaml | 0 svo_ros/rqt.perspective | 0 svo_ros/rviz_config.rviz | 0 svo_ros/src/benchmark_node.cpp | 0 svo_ros/src/visualizer.cpp | 16 +++++++----- svo_ros/src/vo_node.cpp | 23 ++++++++++++++++-- 107 files changed, 81 insertions(+), 19 deletions(-) mode change 100644 => 100755 rqt_svo/CMakeLists.txt mode change 100644 => 100755 rqt_svo/README.md mode change 100644 => 100755 rqt_svo/package.xml mode change 100644 => 100755 rqt_svo/plugin.xml mode change 100644 => 100755 rqt_svo/resource/widget.ui mode change 100644 => 100755 rqt_svo/setup.py mode change 100644 => 100755 rqt_svo/src/rqt_svo/.gitignore mode change 100644 => 100755 rqt_svo/src/rqt_svo/__init__.py mode change 100644 => 100755 rqt_svo/src/rqt_svo/svo.py mode change 100644 => 100755 rqt_svo/src/rqt_svo/svo_widget.py mode change 100644 => 100755 svo/CMakeLists.txt mode change 100644 => 100755 svo/CMakeModules/FindEigen.cmake mode change 100644 => 100755 svo/CMakeModules/FindG2O.cmake mode change 100644 => 100755 svo/Doxyfile mode change 100644 => 100755 svo/doc/.gitignore mode change 100644 => 100755 svo/doc/notation.ipe mode change 100644 => 100755 svo/doc/notation.pdf mode change 100644 => 100755 svo/doc/notation.png mode change 100644 => 100755 svo/include/svo/bundle_adjustment.h mode change 100644 => 100755 svo/include/svo/config.h mode change 100644 => 100755 svo/include/svo/depth_filter.h mode change 100644 => 100755 svo/include/svo/feature.h mode change 100644 => 100755 svo/include/svo/feature_alignment.h mode change 100644 => 100755 svo/include/svo/feature_detection.h mode change 100644 => 100755 svo/include/svo/frame.h mode change 100644 => 100755 svo/include/svo/frame_handler_base.h mode change 100644 => 100755 svo/include/svo/frame_handler_mono.h mode change 100644 => 100755 svo/include/svo/global.h mode change 100644 => 100755 svo/include/svo/initialization.h mode change 100644 => 100755 svo/include/svo/map.h mode change 100644 => 100755 svo/include/svo/matcher.h mode change 100644 => 100755 svo/include/svo/point.h mode change 100644 => 100755 svo/include/svo/pose_optimizer.h mode change 100644 => 100755 svo/include/svo/reprojector.h mode change 100644 => 100755 svo/include/svo/sparse_img_align.h mode change 100644 => 100755 svo/package.xml mode change 100644 => 100755 svo/src/bundle_adjustment.cpp mode change 100644 => 100755 svo/src/config.cpp mode change 100644 => 100755 svo/src/depth_filter.cpp mode change 100644 => 100755 svo/src/feature_alignment.cpp mode change 100644 => 100755 svo/src/feature_detection.cpp mode change 100644 => 100755 svo/src/frame.cpp mode change 100644 => 100755 svo/src/frame_handler_mono.cpp mode change 100644 => 100755 svo/src/initialization.cpp mode change 100644 => 100755 svo/src/map.cpp mode change 100644 => 100755 svo/src/matcher.cpp mode change 100644 => 100755 svo/src/point.cpp mode change 100644 => 100755 svo/src/pose_optimizer.cpp mode change 100644 => 100755 svo/src/reprojector.cpp mode change 100644 => 100755 svo/src/sparse_img_align.cpp mode change 100644 => 100755 svo/test/README.md mode change 100644 => 100755 svo/test/benchmark.csv mode change 100644 => 100755 svo/test/data/.gitignore mode change 100644 => 100755 svo/test/results/.gitignore mode change 100644 => 100755 svo/test/test_depth_filter.cpp mode change 100644 => 100755 svo/test/test_feature_alignment.cpp mode change 100644 => 100755 svo/test/test_feature_detection.cpp mode change 100644 => 100755 svo/test/test_matcher.cpp mode change 100644 => 100755 svo/test/test_pipeline.cpp mode change 100644 => 100755 svo/test/test_pose_optimizer.cpp mode change 100644 => 100755 svo/test/test_sparse_img_align.cpp mode change 100644 => 100755 svo/test/test_utils.h mode change 100644 => 100755 svo_analysis/.gitignore mode change 100644 => 100755 svo_analysis/CMakeLists.txt mode change 100644 => 100755 svo_analysis/README.md mode change 100644 => 100755 svo_analysis/comparisons/speed.yaml mode change 100644 => 100755 svo_analysis/comparisons/textures.yaml mode change 100644 => 100755 svo_analysis/experiments/.gitignore mode change 100644 => 100755 svo_analysis/experiments/asl2_acc.yaml mode change 100644 => 100755 svo_analysis/experiments/asl2_fast.yaml mode change 100644 => 100755 svo_analysis/experiments/flying_room_1_fast.yaml mode change 100644 => 100755 svo_analysis/experiments/synthetic.yaml mode change 100644 => 100755 svo_analysis/package.xml mode change 100644 => 100755 svo_analysis/results/.gitignore mode change 100644 => 100755 svo_analysis/setup.py mode change 100644 => 100755 svo_analysis/src/svo_analysis/__init__.py mode change 100644 => 100755 svo_analysis/src/svo_analysis/analyse_dataset.py mode change 100644 => 100755 svo_analysis/src/svo_analysis/filter_groundtruth_smooth.py mode change 100644 => 100755 svo_analysis/src/svo_analysis/hand_eye_calib.py mode change 100644 => 100755 svo_analysis/src/svo_analysis/tum_benchmark_tools/__init__.py mode change 100644 => 100755 svo_msgs/CMakeLists.txt mode change 100644 => 100755 svo_msgs/msg/DenseInput.msg mode change 100644 => 100755 svo_msgs/msg/Feature.msg mode change 100644 => 100755 svo_msgs/msg/Info.msg mode change 100644 => 100755 svo_msgs/msg/NbvTrajectory.msg mode change 100644 => 100755 svo_msgs/package.xml mode change 100644 => 100755 svo_ros/CMakeLists.txt mode change 100644 => 100755 svo_ros/include/svo_ros/dataset_img.h mode change 100644 => 100755 svo_ros/include/svo_ros/visualizer.h mode change 100644 => 100755 svo_ros/launch/live.launch mode change 100644 => 100755 svo_ros/launch/test_rig3.launch mode change 100644 => 100755 svo_ros/package.xml create mode 100755 svo_ros/param/VO2701_atan.yaml mode change 100644 => 100755 svo_ros/param/camera_atan.yaml mode change 100644 => 100755 svo_ros/param/camera_pinhole.yaml create mode 100755 svo_ros/param/ueyeIR_atan.yaml mode change 100644 => 100755 svo_ros/param/ueye_atan.yaml mode change 100644 => 100755 svo_ros/param/vo_accurate.yaml mode change 100644 => 100755 svo_ros/param/vo_fast.yaml mode change 100644 => 100755 svo_ros/param/vo_mine.yaml mode change 100644 => 100755 svo_ros/param/vo_px4.yaml mode change 100644 => 100755 svo_ros/rqt.perspective mode change 100644 => 100755 svo_ros/rviz_config.rviz mode change 100644 => 100755 svo_ros/src/benchmark_node.cpp mode change 100644 => 100755 svo_ros/src/visualizer.cpp mode change 100644 => 100755 svo_ros/src/vo_node.cpp diff --git a/rqt_svo/CMakeLists.txt b/rqt_svo/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/rqt_svo/README.md b/rqt_svo/README.md old mode 100644 new mode 100755 diff --git a/rqt_svo/package.xml b/rqt_svo/package.xml old mode 100644 new mode 100755 diff --git a/rqt_svo/plugin.xml b/rqt_svo/plugin.xml old mode 100644 new mode 100755 diff --git a/rqt_svo/resource/widget.ui b/rqt_svo/resource/widget.ui old mode 100644 new mode 100755 diff --git a/rqt_svo/setup.py b/rqt_svo/setup.py old mode 100644 new mode 100755 diff --git a/rqt_svo/src/rqt_svo/.gitignore b/rqt_svo/src/rqt_svo/.gitignore old mode 100644 new mode 100755 diff --git a/rqt_svo/src/rqt_svo/__init__.py b/rqt_svo/src/rqt_svo/__init__.py old mode 100644 new mode 100755 diff --git a/rqt_svo/src/rqt_svo/svo.py b/rqt_svo/src/rqt_svo/svo.py old mode 100644 new mode 100755 diff --git a/rqt_svo/src/rqt_svo/svo_widget.py b/rqt_svo/src/rqt_svo/svo_widget.py old mode 100644 new mode 100755 diff --git a/svo/CMakeLists.txt b/svo/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/svo/CMakeModules/FindEigen.cmake b/svo/CMakeModules/FindEigen.cmake old mode 100644 new mode 100755 diff --git a/svo/CMakeModules/FindG2O.cmake b/svo/CMakeModules/FindG2O.cmake old mode 100644 new mode 100755 diff --git a/svo/Doxyfile b/svo/Doxyfile old mode 100644 new mode 100755 diff --git a/svo/doc/.gitignore b/svo/doc/.gitignore old mode 100644 new mode 100755 diff --git a/svo/doc/notation.ipe b/svo/doc/notation.ipe old mode 100644 new mode 100755 diff --git a/svo/doc/notation.pdf b/svo/doc/notation.pdf old mode 100644 new mode 100755 diff --git a/svo/doc/notation.png b/svo/doc/notation.png old mode 100644 new mode 100755 diff --git a/svo/include/svo/bundle_adjustment.h b/svo/include/svo/bundle_adjustment.h old mode 100644 new mode 100755 diff --git a/svo/include/svo/config.h b/svo/include/svo/config.h old mode 100644 new mode 100755 diff --git a/svo/include/svo/depth_filter.h b/svo/include/svo/depth_filter.h old mode 100644 new mode 100755 diff --git a/svo/include/svo/feature.h b/svo/include/svo/feature.h old mode 100644 new mode 100755 diff --git a/svo/include/svo/feature_alignment.h b/svo/include/svo/feature_alignment.h old mode 100644 new mode 100755 diff --git a/svo/include/svo/feature_detection.h b/svo/include/svo/feature_detection.h old mode 100644 new mode 100755 diff --git a/svo/include/svo/frame.h b/svo/include/svo/frame.h old mode 100644 new mode 100755 diff --git a/svo/include/svo/frame_handler_base.h b/svo/include/svo/frame_handler_base.h old mode 100644 new mode 100755 diff --git a/svo/include/svo/frame_handler_mono.h b/svo/include/svo/frame_handler_mono.h old mode 100644 new mode 100755 diff --git a/svo/include/svo/global.h b/svo/include/svo/global.h old mode 100644 new mode 100755 diff --git a/svo/include/svo/initialization.h b/svo/include/svo/initialization.h old mode 100644 new mode 100755 diff --git a/svo/include/svo/map.h b/svo/include/svo/map.h old mode 100644 new mode 100755 diff --git a/svo/include/svo/matcher.h b/svo/include/svo/matcher.h old mode 100644 new mode 100755 diff --git a/svo/include/svo/point.h b/svo/include/svo/point.h old mode 100644 new mode 100755 diff --git a/svo/include/svo/pose_optimizer.h b/svo/include/svo/pose_optimizer.h old mode 100644 new mode 100755 diff --git a/svo/include/svo/reprojector.h b/svo/include/svo/reprojector.h old mode 100644 new mode 100755 diff --git a/svo/include/svo/sparse_img_align.h b/svo/include/svo/sparse_img_align.h old mode 100644 new mode 100755 diff --git a/svo/package.xml b/svo/package.xml old mode 100644 new mode 100755 diff --git a/svo/src/bundle_adjustment.cpp b/svo/src/bundle_adjustment.cpp old mode 100644 new mode 100755 diff --git a/svo/src/config.cpp b/svo/src/config.cpp old mode 100644 new mode 100755 diff --git a/svo/src/depth_filter.cpp b/svo/src/depth_filter.cpp old mode 100644 new mode 100755 diff --git a/svo/src/feature_alignment.cpp b/svo/src/feature_alignment.cpp old mode 100644 new mode 100755 diff --git a/svo/src/feature_detection.cpp b/svo/src/feature_detection.cpp old mode 100644 new mode 100755 diff --git a/svo/src/frame.cpp b/svo/src/frame.cpp old mode 100644 new mode 100755 diff --git a/svo/src/frame_handler_base.cpp b/svo/src/frame_handler_base.cpp index 78476709..8616fbed 100755 --- a/svo/src/frame_handler_base.cpp +++ b/svo/src/frame_handler_base.cpp @@ -138,7 +138,8 @@ int FrameHandlerBase::finishFrameProcessingCommon( else if (dropout == RESULT_FAILURE) { resetAll(); - stage_ = STAGE_FIRST_FRAME; + 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 old mode 100644 new mode 100755 diff --git a/svo/src/initialization.cpp b/svo/src/initialization.cpp old mode 100644 new mode 100755 index 84b9baf3..f6d58277 --- 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 old mode 100644 new mode 100755 diff --git a/svo/src/matcher.cpp b/svo/src/matcher.cpp old mode 100644 new mode 100755 diff --git a/svo/src/point.cpp b/svo/src/point.cpp old mode 100644 new mode 100755 diff --git a/svo/src/pose_optimizer.cpp b/svo/src/pose_optimizer.cpp old mode 100644 new mode 100755 diff --git a/svo/src/reprojector.cpp b/svo/src/reprojector.cpp old mode 100644 new mode 100755 diff --git a/svo/src/sparse_img_align.cpp b/svo/src/sparse_img_align.cpp old mode 100644 new mode 100755 diff --git a/svo/test/README.md b/svo/test/README.md old mode 100644 new mode 100755 diff --git a/svo/test/benchmark.csv b/svo/test/benchmark.csv old mode 100644 new mode 100755 diff --git a/svo/test/data/.gitignore b/svo/test/data/.gitignore old mode 100644 new mode 100755 diff --git a/svo/test/results/.gitignore b/svo/test/results/.gitignore old mode 100644 new mode 100755 diff --git a/svo/test/test_depth_filter.cpp b/svo/test/test_depth_filter.cpp old mode 100644 new mode 100755 diff --git a/svo/test/test_feature_alignment.cpp b/svo/test/test_feature_alignment.cpp old mode 100644 new mode 100755 diff --git a/svo/test/test_feature_detection.cpp b/svo/test/test_feature_detection.cpp old mode 100644 new mode 100755 diff --git a/svo/test/test_matcher.cpp b/svo/test/test_matcher.cpp old mode 100644 new mode 100755 diff --git a/svo/test/test_pipeline.cpp b/svo/test/test_pipeline.cpp old mode 100644 new mode 100755 diff --git a/svo/test/test_pose_optimizer.cpp b/svo/test/test_pose_optimizer.cpp old mode 100644 new mode 100755 diff --git a/svo/test/test_sparse_img_align.cpp b/svo/test/test_sparse_img_align.cpp old mode 100644 new mode 100755 diff --git a/svo/test/test_utils.h b/svo/test/test_utils.h old mode 100644 new mode 100755 diff --git a/svo_analysis/.gitignore b/svo_analysis/.gitignore old mode 100644 new mode 100755 diff --git a/svo_analysis/CMakeLists.txt b/svo_analysis/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/svo_analysis/README.md b/svo_analysis/README.md old mode 100644 new mode 100755 diff --git a/svo_analysis/comparisons/speed.yaml b/svo_analysis/comparisons/speed.yaml old mode 100644 new mode 100755 diff --git a/svo_analysis/comparisons/textures.yaml b/svo_analysis/comparisons/textures.yaml old mode 100644 new mode 100755 diff --git a/svo_analysis/experiments/.gitignore b/svo_analysis/experiments/.gitignore old mode 100644 new mode 100755 diff --git a/svo_analysis/experiments/asl2_acc.yaml b/svo_analysis/experiments/asl2_acc.yaml old mode 100644 new mode 100755 diff --git a/svo_analysis/experiments/asl2_fast.yaml b/svo_analysis/experiments/asl2_fast.yaml old mode 100644 new mode 100755 diff --git a/svo_analysis/experiments/flying_room_1_fast.yaml b/svo_analysis/experiments/flying_room_1_fast.yaml old mode 100644 new mode 100755 diff --git a/svo_analysis/experiments/synthetic.yaml b/svo_analysis/experiments/synthetic.yaml old mode 100644 new mode 100755 diff --git a/svo_analysis/package.xml b/svo_analysis/package.xml old mode 100644 new mode 100755 diff --git a/svo_analysis/results/.gitignore b/svo_analysis/results/.gitignore old mode 100644 new mode 100755 diff --git a/svo_analysis/setup.py b/svo_analysis/setup.py old mode 100644 new mode 100755 diff --git a/svo_analysis/src/svo_analysis/__init__.py b/svo_analysis/src/svo_analysis/__init__.py old mode 100644 new mode 100755 diff --git a/svo_analysis/src/svo_analysis/analyse_dataset.py b/svo_analysis/src/svo_analysis/analyse_dataset.py old mode 100644 new mode 100755 diff --git a/svo_analysis/src/svo_analysis/filter_groundtruth_smooth.py b/svo_analysis/src/svo_analysis/filter_groundtruth_smooth.py old mode 100644 new mode 100755 diff --git a/svo_analysis/src/svo_analysis/hand_eye_calib.py b/svo_analysis/src/svo_analysis/hand_eye_calib.py old mode 100644 new mode 100755 diff --git a/svo_analysis/src/svo_analysis/tum_benchmark_tools/__init__.py b/svo_analysis/src/svo_analysis/tum_benchmark_tools/__init__.py old mode 100644 new mode 100755 diff --git a/svo_msgs/CMakeLists.txt b/svo_msgs/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/svo_msgs/msg/DenseInput.msg b/svo_msgs/msg/DenseInput.msg old mode 100644 new mode 100755 diff --git a/svo_msgs/msg/Feature.msg b/svo_msgs/msg/Feature.msg old mode 100644 new mode 100755 diff --git a/svo_msgs/msg/Info.msg b/svo_msgs/msg/Info.msg old mode 100644 new mode 100755 diff --git a/svo_msgs/msg/NbvTrajectory.msg b/svo_msgs/msg/NbvTrajectory.msg old mode 100644 new mode 100755 diff --git a/svo_msgs/package.xml b/svo_msgs/package.xml old mode 100644 new mode 100755 diff --git a/svo_ros/CMakeLists.txt b/svo_ros/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/svo_ros/include/svo_ros/dataset_img.h b/svo_ros/include/svo_ros/dataset_img.h old mode 100644 new mode 100755 diff --git a/svo_ros/include/svo_ros/visualizer.h b/svo_ros/include/svo_ros/visualizer.h old mode 100644 new mode 100755 index c5188f31..535e5c3e --- a/svo_ros/include/svo_ros/visualizer.h +++ b/svo_ros/include/svo_ros/visualizer.h @@ -64,6 +64,8 @@ class Visualizer 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 old mode 100644 new mode 100755 diff --git a/svo_ros/launch/test_rig3.launch b/svo_ros/launch/test_rig3.launch old mode 100644 new mode 100755 diff --git a/svo_ros/package.xml b/svo_ros/package.xml old mode 100644 new mode 100755 diff --git a/svo_ros/param/VO2701_atan.yaml b/svo_ros/param/VO2701_atan.yaml new file mode 100755 index 00000000..3356d54c --- /dev/null +++ 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 old mode 100644 new mode 100755 diff --git a/svo_ros/param/camera_pinhole.yaml b/svo_ros/param/camera_pinhole.yaml old mode 100644 new mode 100755 diff --git a/svo_ros/param/ueyeIR_atan.yaml b/svo_ros/param/ueyeIR_atan.yaml new file mode 100755 index 00000000..c68210d4 --- /dev/null +++ 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 old mode 100644 new mode 100755 index c7649a78..d615af0d --- a/svo_ros/param/ueye_atan.yaml +++ b/svo_ros/param/ueye_atan.yaml @@ -1,8 +1,8 @@ cam_model: ATAN cam_width: 752 cam_height: 480 -cam_fx: 0.439782 -cam_fy: 0.6874 -cam_cx: 0.510006 -cam_cy: 0.458044 -cam_d0: 0.960589 +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 old mode 100644 new mode 100755 diff --git a/svo_ros/param/vo_fast.yaml b/svo_ros/param/vo_fast.yaml old mode 100644 new mode 100755 index 3df87dd6..7a11d30e --- 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 \ No newline at end of file +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 old mode 100644 new mode 100755 index 9b3bdbba..698cf8ac --- a/svo_ros/param/vo_mine.yaml +++ b/svo_ros/param/vo_mine.yaml @@ -1,4 +1,16 @@ grid_size: 30 max_n_kfs: 10 -loba_num_iter: 2 -max_fts: 180 \ No newline at end of file +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 \ No newline at end of file diff --git a/svo_ros/param/vo_px4.yaml b/svo_ros/param/vo_px4.yaml old mode 100644 new mode 100755 diff --git a/svo_ros/rqt.perspective b/svo_ros/rqt.perspective old mode 100644 new mode 100755 diff --git a/svo_ros/rviz_config.rviz b/svo_ros/rviz_config.rviz old mode 100644 new mode 100755 diff --git a/svo_ros/src/benchmark_node.cpp b/svo_ros/src/benchmark_node.cpp old mode 100644 new mode 100755 diff --git a/svo_ros/src/visualizer.cpp b/svo_ros/src/visualizer.cpp old mode 100644 new mode 100755 index a000880e..6439774e --- a/svo_ros/src/visualizer.cpp +++ b/svo_ros/src/visualizer.cpp @@ -47,12 +47,13 @@ Visualizer() : publish_world_in_cam_frame_(vk::getParam("svo/publish_world_in_cam_frame", true)), publish_map_every_frame_(vk::getParam("svo/publish_map_every_frame", false)), publish_points_display_time_(vk::getParam("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("keyframes", 10); pub_points_ = pnh_.advertise("points", 1000); - pub_pose_ = pnh_.advertise("pose",10); + pub_pose_ = pnh_.advertise("pose",1); pub_info_ = pnh_.advertise("info", 10); pub_dense_ = pnh_.advertise("dense_input",10); @@ -82,13 +83,16 @@ void Visualizer::publishMinimal( msg_info.keyframes.resize(slam.map().keyframes_.size()); for(list::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(slam.stage()); msg_info.tracking_quality = static_cast(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) @@ -213,12 +217,12 @@ void Visualizer::visualizeMarkers( // 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); @@ -265,7 +269,7 @@ 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; } diff --git a/svo_ros/src/vo_node.cpp b/svo_ros/src/vo_node.cpp old mode 100644 new mode 100755 index bab55a26..76e5ff03 --- a/svo_ros/src/vo_node.cpp +++ b/svo_ros/src/vo_node.cpp @@ -37,6 +37,7 @@ #include //#####################################3 #include //##################### +#include namespace svo { @@ -59,6 +60,10 @@ class VoNode 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_; }; @@ -91,7 +96,7 @@ VoNode::VoNode() : // Init VO and start vo_ = new svo::FrameHandlerMono(cam_); - vo_->reset(); + vo_->start(); } VoNode::~VoNode() @@ -178,9 +183,22 @@ void VoNode::resetCallback(const geometry_msgs::TransformStamped::ConstPtr& msgi 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"); @@ -191,12 +209,13 @@ int main(int argc, char **argv) // subscribe to cam msgs std::string cam_topic(vk::getParam("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); + 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("/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_) From c1995ad68075888c22e7a0fc037def216c34079a Mon Sep 17 00:00:00 2001 From: Chang Liu Date: Sun, 5 Mar 2017 18:38:05 +0000 Subject: [PATCH 5/6] added a launch file to work with UEYE camera --- svo_ros/launch/testueyeSVO.launch | 40 +++++++++++++++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 svo_ros/launch/testueyeSVO.launch diff --git a/svo_ros/launch/testueyeSVO.launch b/svo_ros/launch/testueyeSVO.launch new file mode 100644 index 00000000..85d77430 --- /dev/null +++ b/svo_ros/launch/testueyeSVO.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From e8cad76de6c780b38f7a9566dc914ff50e00e6f5 Mon Sep 17 00:00:00 2001 From: Chang Liu Date: Wed, 5 Apr 2017 00:42:40 +0100 Subject: [PATCH 6/6] corrected topic as relative direction for namespace swarm --- svo_ros/src/vo_node.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/svo_ros/src/vo_node.cpp b/svo_ros/src/vo_node.cpp index 76e5ff03..a2411f82 100755 --- a/svo_ros/src/vo_node.cpp +++ b/svo_ros/src/vo_node.cpp @@ -213,9 +213,9 @@ int main(int argc, char **argv) // 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("/svo/usereset", 10);; - vo_node.sub_quality_ = nh.subscribe("/aslam/quality", 100, &svo::VoNode::qualityCallback, &vo_node); + vo_node.sub_remote_key_ = nh.subscribe("Allreset", 10, &svo::VoNode::resetCallback, &vo_node); + vo_node.pub_usereset_ = nh.advertise("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_)