Misc. fix-ups

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}
+)
+

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<FramePtr>& coreKeyframes() { return core_kfs_; }
+  const set<FramePtr>& coreKeyframes();
 
   /// Return the feature track to visualize the KLT tracking during initialization.
-  const vector<cv::Point2f>& initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; }
-  const vector<cv::Point2f>& initFeatureTrackCurPx() const { return klt_homography_init_.px_cur_; }
+  const vector<cv::Point2f>& initFeatureTrackRefPx() const;
+  const vector<cv::Point2f>& 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(

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<FramePtr>& FrameHandlerMono::coreKeyframes() { return core_kfs_; }
+const vector<cv::Point2f>& FrameHandlerMono::initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; }
+const vector<cv::Point2f>& 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());

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}
-)
+)

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}
+)

svo_ros/launch/webcam.launch

@@ -0,0 +1,38 @@
+<launch>
+
+    <arg name="width" default="640" />
+    <arg name="height" default="480" />
+
+    <node pkg="svo_ros" type="vo" name="svo" clear_params="true" output="screen">
+
+        <!-- Camera topic to subscribe to -->
+        <param name="cam_topic" value="/webcam/image_mono" type="str" />
+
+        <!-- Camera calibration file -->
+        <rosparam file="$(find svo_ros)/param/camera_webcam.yaml" />
+        <param name="cam_width" value="$(arg width)" />
+        <param name="cam_height" value="$(arg height)" />
+
+        <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
+        <rosparam file="$(find svo_ros)/param/vo_fast.yaml" />
+
+        <rosparam name="publish_dense_input" value="1" type="int" />
+
+    </node>
+
+    <!--include file="$(find video_stream_opencv)/launch/webcam.launch">
+        <arg name="width" value="$(arg width)" />
+        <arg name="height" value="$(arg height)" />
+    </include-->
+
+    <!--include file="$(find image_proc)/launch/image_proc.launch" ns="webcam">
+        <arg name="manager" value="svo_image_proc" />
+    </include-->
+
+    <node pkg="libuvc_camera" type="camera_node" name="camera_stream" ns="webcam">
+        <param name="vendor" value="0x05a3" />
+    </node>
+
+    <node pkg="image_proc" type="image_proc" name="svo_image_proc" ns="webcam" />
+
+</launch>

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

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<bool>("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<bool>("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");