From 52922233e7a61cbc04b1663b6833f196a9b4b6ce Mon Sep 17 00:00:00 2001 From: Ludvig Ericson Date: Thu, 9 Mar 2017 13:26:24 +0100 Subject: [PATCH 1/5] Add install directives to CMakeLists.txt --- svo/CMakeLists.txt | 8 ++++++++ svo_ros/CMakeLists.txt | 9 +++++++++ 2 files changed, 17 insertions(+) diff --git a/svo/CMakeLists.txt b/svo/CMakeLists.txt index 703e379e..5724121a 100644 --- a/svo/CMakeLists.txt +++ b/svo/CMakeLists.txt @@ -148,3 +148,11 @@ TARGET_LINK_LIBRARIES(test_sparse_img_align svo) ADD_EXECUTABLE(test_pose_optimizer test/test_pose_optimizer.cpp) TARGET_LINK_LIBRARIES(test_pose_optimizer svo) + +install(DIRECTORY include/svo DESTINATION ${CMAKE_INSTALL_PREFIX}/include) +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) + diff --git a/svo_ros/CMakeLists.txt b/svo_ros/CMakeLists.txt index 32df7034..744438dd 100644 --- a/svo_ros/CMakeLists.txt +++ b/svo_ros/CMakeLists.txt @@ -70,3 +70,12 @@ ADD_EXECUTABLE(vo src/vo_node.cpp) TARGET_LINK_LIBRARIES(vo svo_visualizer) ADD_EXECUTABLE(benchmark src/benchmark_node.cpp) TARGET_LINK_LIBRARIES(benchmark svo_visualizer) + +install(DIRECTORY include/svo_ros DESTINATION ${CMAKE_INSTALL_PREFIX}/include) +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY param DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(TARGETS svo_visualizer vo benchmark + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) From de2fe0a76d2800be740eff5432ea0448b8f5ccd4 Mon Sep 17 00:00:00 2001 From: Ludvig Ericson Date: Thu, 9 Mar 2017 13:30:01 +0100 Subject: [PATCH 2/5] Add sensors_msgs dependency --- svo_msgs/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/svo_msgs/CMakeLists.txt b/svo_msgs/CMakeLists.txt index 23d4e3be..56150206 100644 --- a/svo_msgs/CMakeLists.txt +++ b/svo_msgs/CMakeLists.txt @@ -27,9 +27,9 @@ generate_messages( # export the dependencis of this package for who ever depends on us catkin_package( - CATKIN_DEPENDS message_runtime geometry_msgs + CATKIN_DEPENDS message_runtime geometry_msgs sensor_msgs ) include_directories( ${catkin_INCLUDE_DIRS} -) \ No newline at end of file +) From 026b5190cffc2212b2b0898ee05b9eedcd1d486b Mon Sep 17 00:00:00 2001 From: Ludvig Ericson Date: Thu, 9 Mar 2017 13:30:22 +0100 Subject: [PATCH 3/5] Add webcam launch files --- svo_ros/launch/webcam.launch | 38 ++++++++++++++++++++++++++++++++ svo_ros/param/camera_webcam.yaml | 12 ++++++++++ 2 files changed, 50 insertions(+) create mode 100644 svo_ros/launch/webcam.launch create mode 100644 svo_ros/param/camera_webcam.yaml diff --git a/svo_ros/launch/webcam.launch b/svo_ros/launch/webcam.launch new file mode 100644 index 00000000..c45b0e1f --- /dev/null +++ b/svo_ros/launch/webcam.launch @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/svo_ros/param/camera_webcam.yaml b/svo_ros/param/camera_webcam.yaml new file mode 100644 index 00000000..504d643e --- /dev/null +++ b/svo_ros/param/camera_webcam.yaml @@ -0,0 +1,12 @@ +cam_model: Pinhole +# Set in launch file webcam.launch instead. +cam_width: 640 +cam_height: 480 +cam_fx: 346.0229416619373 +cam_fy: 343.426914274612 +cam_cx: 318.0860682913412 +cam_cy: 236.7022431168731 +cam_d0: -0.3731376888278783 +cam_d1: 0.08374307890580708 +cam_d2: 0.006608615041262322 +cam_d3: -0.0006764314755213373 From e4864d92f342b122baf22f74d8297bfc9913bd85 Mon Sep 17 00:00:00 2001 From: Ludvig Ericson Date: Thu, 9 Mar 2017 13:27:17 +0100 Subject: [PATCH 4/5] Move implementations from header Causes major breakage in LLVM C++ compiler where shared pointers are somehow always NULL. --- svo/include/svo/frame_handler_mono.h | 10 +++++----- svo/src/frame_handler_mono.cpp | 6 ++++++ 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/svo/include/svo/frame_handler_mono.h b/svo/include/svo/frame_handler_mono.h index aed73be4..722d9f80 100644 --- a/svo/include/svo/frame_handler_mono.h +++ b/svo/include/svo/frame_handler_mono.h @@ -41,17 +41,17 @@ class FrameHandlerMono : public FrameHandlerBase void setFirstFrame(const FramePtr& first_frame); /// Get the last frame that has been processed. - FramePtr lastFrame() { return last_frame_; } + FramePtr lastFrame(); /// Get the set of spatially closest keyframes of the last frame. - const set& coreKeyframes() { return core_kfs_; } + const set& coreKeyframes(); /// Return the feature track to visualize the KLT tracking during initialization. - const vector& initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; } - const vector& initFeatureTrackCurPx() const { return klt_homography_init_.px_cur_; } + const vector& initFeatureTrackRefPx() const; + const vector& initFeatureTrackCurPx() const; /// Access the depth filter. - DepthFilter* depthFilter() const { return depth_filter_; } + DepthFilter* depthFilter() const; /// An external place recognition module may know where to relocalize. bool relocalizeFrameAtPose( diff --git a/svo/src/frame_handler_mono.cpp b/svo/src/frame_handler_mono.cpp index 3050ba77..3dbd3cbb 100644 --- a/svo/src/frame_handler_mono.cpp +++ b/svo/src/frame_handler_mono.cpp @@ -89,6 +89,12 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp) finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs()); } +FramePtr FrameHandlerMono::lastFrame() { return last_frame_; } +const set& FrameHandlerMono::coreKeyframes() { return core_kfs_; } +const vector& FrameHandlerMono::initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; } +const vector& FrameHandlerMono::initFeatureTrackCurPx() const { return klt_homography_init_.px_cur_; } +DepthFilter* FrameHandlerMono::depthFilter() const { return depth_filter_; } + FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame() { new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero()); From 36aa3b01499cd0dc96e53eca87747f31d04168b3 Mon Sep 17 00:00:00 2001 From: Ludvig Ericson Date: Thu, 9 Mar 2017 13:29:21 +0100 Subject: [PATCH 5/5] Warn when not receiving images from camera --- svo_ros/src/vo_node.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/svo_ros/src/vo_node.cpp b/svo_ros/src/vo_node.cpp index 8a6f88c2..b8a4ef26 100644 --- a/svo_ros/src/vo_node.cpp +++ b/svo_ros/src/vo_node.cpp @@ -50,6 +50,7 @@ class VoNode std::string remote_input_; vk::AbstractCamera* cam_; bool quit_; + ros::Time last_frame_t_; VoNode(); ~VoNode(); void imgCb(const sensor_msgs::ImageConstPtr& msg); @@ -63,7 +64,8 @@ VoNode::VoNode() : publish_dense_input_(vk::getParam("svo/publish_dense_input", false)), remote_input_(""), cam_(NULL), - quit_(false) + quit_(false), + last_frame_t_(ros::Time::now()) { // Start user input thread in parallel thread that listens to console keys if(vk::getParam("svo/accept_console_user_input", true)) @@ -98,14 +100,15 @@ VoNode::~VoNode() void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg) { cv::Mat img; + last_frame_t_ = (msg->header.stamp != ros::Time(0)) ? msg->header.stamp : ros::Time::now(); try { img = cv_bridge::toCvShare(msg, "mono8")->image; } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); } processUserActions(); - vo_->addImage(img, msg->header.stamp.toSec()); - visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec()); + vo_->addImage(img, last_frame_t_.toSec()); + visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, last_frame_t_.toSec()); if(publish_markers_ && vo_->stage() != FrameHandlerBase::STAGE_PAUSED) visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map()); @@ -158,7 +161,7 @@ int main(int argc, char **argv) { ros::init(argc, argv, "svo"); ros::NodeHandle nh; - std::cout << "create vo_node" << std::endl; + std::cout << "Creating vo_node" << std::endl; svo::VoNode vo_node; // subscribe to cam msgs @@ -173,7 +176,9 @@ int main(int argc, char **argv) while(ros::ok() && !vo_node.quit_) { ros::spinOnce(); - // TODO check when last image was processed. when too long ago. publish warning that no msgs are received! + if (ros::Time::now() - vo_node.last_frame_t_ > ros::Duration(5)) { + ROS_WARN_DELAYED_THROTTLE(1.0, "Not receiving any image callbacks, hung camera?"); + } } printf("SVO terminated.\n");