first commit

2014-03-12 16:39:58 +01:00
commit 76801f72ac
98 changed files with 14580 additions and 0 deletions
@@ -0,0 +1,62 @@
+SET(PROJECT_NAME svo_ros)
+PROJECT(${PROJECT_NAME})
+CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3)
+SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo
+SET(CMAKE_VERBOSE_MAKEFILE OFF)
+SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/")
+
+IF(CMAKE_COMPILER_IS_GNUCC)
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+ELSE()
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+ENDIF()
+
+
+# Add catkin and required ROS packages
+FIND_PACKAGE(catkin REQUIRED COMPONENTS 
+  roscpp nav_msgs std_msgs visualization_msgs sensor_msgs image_transport cv_bridge tf svo svo_msgs vikit_common vikit_ros
+)
+
+# Add plain cmake packages 
+FIND_PACKAGE(OpenCV REQUIRED)
+FIND_PACKAGE(Eigen REQUIRED)
+FIND_PACKAGE(Sophus REQUIRED)
+FIND_PACKAGE(Boost REQUIRED COMPONENTS thread signals)
+
+# Describe catkin Project
+catkin_package(
+  DEPENDS Eigen OpenCV Sophus Boost
+  CATKIN_DEPENDS roscpp nav_msgs std_msgs visualization_msgs sensor_msgs image_transport cv_bridge tf svo svo_msgs vikit_common vikit_ros
+  INCLUDE_DIRS include
+  LIBRARIES svo_visualizer
+)
+
+# Include dirs
+INCLUDE_DIRECTORIES(
+  include
+  ${Eigen_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+  ${Sophus_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+
+# Set link libraries
+LIST(APPEND LINK_LIBS 
+  ${OpenCV_LIBRARIES}
+  ${Sophus_LIBRARIES}
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+# Create visualizer library
+ADD_LIBRARY(svo_visualizer src/visualizer.cpp)
+TARGET_LINK_LIBRARIES(svo_visualizer ${LINK_LIBS})
+                                 
+# Create Executables                      
+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)
+ADD_EXECUTABLE(remote src/remote_node.cpp)
+TARGET_LINK_LIBRARIES(remote svo_visualizer)
@@ -0,0 +1,62 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef DATASET_IMG_H
+#define DATASET_IMG_H
+
+namespace FileType {
+
+#include <string>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+using ::Eigen::Quaterniond;
+using ::Eigen::Vector3d;
+
+/// Specification of Dataset which contains: Camera Image, Gyroscope Rotation
+class DatasetImg
+{
+public:
+  DatasetImg() {}
+  virtual ~DatasetImg() {}
+
+  double stamp_;
+  std::string imgname_;
+
+  friend std::ostream& operator <<(std::ostream& out, const DatasetImg& pair);
+  friend std::istream& operator >>(std::istream& in, DatasetImg& pair);
+};
+
+std::ostream& operator <<(std::ostream& out, const DatasetImg& gt)
+{
+  out
+    << gt.stamp_ << " "
+    << gt.imgname_ << " "
+    << std::endl;
+  return out;
+}
+
+std::istream& operator >>(std::istream& in, DatasetImg& gt)
+{
+  in >> gt.stamp_;
+  in >> gt.imgname_;
+  return in;
+}
+
+} // end namespace FileType
+
+
+#endif /* DATASET_IMG_H */
@@ -0,0 +1,70 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef DATASET_IMU_H_
+#define DATASET_IMU_H_
+
+namespace FileType {
+
+#include <string>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+using ::Eigen::Quaterniond;
+using ::Eigen::Vector3d;
+
+/// Specification of Dataset which contains: Camera Image, Gyroscope Rotation
+class DatasetImu
+{
+public:
+  DatasetImu() {}
+  virtual ~DatasetImu() {}
+
+  double stamp_;
+  Quaterniond q_imu_;
+
+  friend std::ostream& operator <<(std::ostream& out, const DatasetImu& pair);
+  friend std::istream& operator >>(std::istream& in, DatasetImu& pair);
+};
+
+std::ostream& operator <<(std::ostream& out, const DatasetImu& gt)
+{
+  out
+    << gt.stamp_ << " "
+    << gt.q_imu_.x() << " "
+    << gt.q_imu_.y() << " "
+    << gt.q_imu_.z() << " "
+    << gt.q_imu_.w() << " "
+    << std::endl;
+  return out;
+}
+
+std::istream& operator >>(std::istream& in, DatasetImu& gt)
+{
+  in >> gt.stamp_;
+  double x,y,z,w;
+  in >> x;
+  in >> y;
+  in >> z;
+  in >> w;
+  gt.q_imu_ = Quaterniond(w, x, y, z);
+  return in;
+}
+
+} // end namespace FileType
+
+
+#endif /* DATASET_IMU_H_ */
@@ -0,0 +1,75 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_VISUALIZER_H_
+#define SVO_VISUALIZER_H_
+
+#include <queue>
+#include <ros/ros.h>
+#include <svo/global.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <nav_msgs/Odometry.h>
+#include <std_msgs/ColorRGBA.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <boost/lexical_cast.hpp>
+#include <tf/transform_broadcaster.h>
+#include <image_transport/image_transport.h>
+#include <boost/thread.hpp>
+#include <boost/shared_ptr.hpp>
+#include <iostream>
+
+namespace svo {
+
+class Frame;
+class Point;
+class Map;
+class FrameHandlerMono;
+
+typedef boost::shared_ptr<Frame> FramePtr;
+
+/// This class bundles all functions to publish visualisation messages,
+/// trace data and output debugging information.
+class Visualizer
+{
+public:
+  ros::NodeHandle pnh_;
+  size_t trace_id_;
+  size_t img_pub_level_;
+  size_t img_pub_nth_;
+  size_t dense_pub_nth_;
+  ros::Publisher pub_frames_;
+  ros::Publisher pub_points_;
+  ros::Publisher pub_pose_;
+  ros::Publisher pub_info_;
+  ros::Publisher pub_dense_;
+  image_transport::Publisher pub_images_;
+  tf::TransformBroadcaster br_;
+
+  Visualizer();
+  ~Visualizer() {};
+  void visualizeMarkers(const FramePtr& frame, const set<FramePtr>& core_kfs, const Map& map);
+  void publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono& slam, const double timestamp);
+  void publishMapRegion(set<FramePtr> frames);
+  void removeDeletedPts(const Map& map);
+  void displayKeyframeWithMps(const FramePtr& frame, int ts);
+  void exportToDense(const FramePtr& frame);
+};
+
+} // end namespace svo
+
+#endif /* SVO_VISUALIZER_H_ */
@@ -0,0 +1,25 @@
+<launch>
+  
+    <node pkg="svo_ros" type="vo" name="svo" clear_params="true" output="screen">
+    
+        <param name="cam_topic" value="/camera/image_raw" type="str" />
+        <rosparam file="$(find rpg_calib)/cameras/bluefox_847_cfo_atan.yaml" />
+        <rosparam file="$(find svo_ros)/param/vo_fast.yaml" />
+
+        <param name="map_scale" value="0.5" />
+        <param name="init_rx" value="3.14" />
+        <param name="image_publishing_level" value="0" />
+        <param name="publish_every_nth_img" value="1" />
+        <param name="accept_console_user_input" value="True" />
+        
+        <param name="publish_dense_input" value="True" />
+        <param name="publish_every_nth_dense_input" value="5" />
+    </node>
+    
+    <!--
+    <node pkg="image_view" type="image_view" name="image_svo">
+        <remap from="image" to="/svo/image" />
+    </node>
+    -->
+    
+</launch>
@@ -0,0 +1,16 @@
+<launch>
+  <node pkg="svo_ros" type="vo" name="svo" clear_params="true" launch-prefix="gdb -ex run --args" output="screen"> 
+    <param name="cam_topic" value="/camera/image_raw" type="str" />
+    <param name="cam_model" value="ATAN" type="str" />
+    <param name="cam_width" value="752" />
+    <param name="cam_height" value="480" />
+    <param name="cam_fx" value="0.509326" />
+    <param name="cam_fy" value="0.796651" />
+    <param name="cam_cx" value="0.459051" />
+    <param name="cam_cy" value="0.510056" />
+    <param name="cam_d0" value="0.93207" />
+    <param name="init_rx" value="3.14" />
+    <rosparam file="$(find svo_ros)/param/vo_fast.yaml" />
+  </node>
+  <node name="svo_ros_rviz" pkg="rviz" type="rviz" args=" -d $(find svo_ros)/rviz_config.rviz" /> 
+</launch>
@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<package>
+  <name>svo_ros</name>
+  <version>0.1.0</version>
+  <description>
+    Monocular Visual Odometry - ROS Nodes
+  </description>
+  <author>Christian Forster</author>
+  <maintainer email="[email protected]">Christian Forster</maintainer>
+  <license>GPLv3</license>
+
+  <!-- Dependencies which this package needs to build itself. -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- Dependencies needed to compile this package. -->
+  <build_depend>roscpp</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>svo</build_depend>
+  <build_depend>svo_msgs</build_depend>
+  <build_depend>vikit_common</build_depend>
+  <build_depend>vikit_ros</build_depend>
+  
+  <!-- Dependencies needed after this package is compiled. -->
+  <run_depend>roscpp</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>image_transport</run_depend>
+  <run_depend>cv_bridge</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>svo</run_depend>
+  <run_depend>svo_msgs</run_depend>
+  <run_depend>vikit_common</run_depend>
+  <run_depend>vikit_ros</run_depend>
+</package>
@@ -0,0 +1,4 @@
+grid_size: 20
+max_n_kfs: 50
+loba_num_iter: 10
+max_fts: 200
@@ -0,0 +1,3 @@
+grid_size: 30
+max_n_kfs: 10
+loba_num_iter: 0
@@ -0,0 +1,293 @@
+{
+  "keys": {}, 
+  "groups": {
+    "pluginmanager": {
+      "keys": {
+        "running-plugins": {
+          "type": "repr", 
+          "repr": "{u'rqt_rviz/RViz': [1], u'rqt_reconfigure/Param': [1], u'rqt_image_view/ImageView': [1], u'svo_rqt/Svo': [1]}"
+        }
+      }, 
+      "groups": {
+        "plugin__rqt_rviz__RViz__1": {
+          "keys": {}, 
+          "groups": {
+            "dock_widget__": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "True"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__rqt_robot_steering__RobotSteering__1": {
+          "keys": {}, 
+          "groups": {
+            "dock_widget__RobotSteeringUi": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "u'true'"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }, 
+            "plugin": {
+              "keys": {
+                "topic": {
+                  "type": "repr", 
+                  "repr": "u'/cmd_vel'"
+                }, 
+                "vx_min": {
+                  "type": "repr", 
+                  "repr": "u'-1'"
+                }, 
+                "vw_min": {
+                  "type": "repr", 
+                  "repr": "u'-3'"
+                }, 
+                "vw_max": {
+                  "type": "repr", 
+                  "repr": "u'3'"
+                }, 
+                "vx_max": {
+                  "type": "repr", 
+                  "repr": "u'1'"
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__rqt_reconfigure__Param__1": {
+          "keys": {}, 
+          "groups": {
+            "dock_widget___plugincontainer_top_widget": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "True"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }, 
+            "plugin": {
+              "keys": {
+                "splitter": {
+                  "type": "repr(QByteArray.hex)", 
+                  "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000000000002000000c00000006401000000060100000001')", 
+                  "pretty-print": "         d     "
+                }, 
+                "_splitter": {
+                  "type": "repr(QByteArray.hex)", 
+                  "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000020000013e0000006401000000090100000002')", 
+                  "pretty-print": "       > d     "
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__rqt_image_view__ImageView__1": {
+          "keys": {}, 
+          "groups": {
+            "dock_widget__ImageViewWidget": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "True"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }, 
+            "plugin": {
+              "keys": {
+                "dynamic_range": {
+                  "type": "repr", 
+                  "repr": "False"
+                }, 
+                "max_range": {
+                  "type": "repr", 
+                  "repr": "10.0"
+                }, 
+                "topic": {
+                  "type": "repr", 
+                  "repr": "u'/svo/image'"
+                }, 
+                "zoom1": {
+                  "type": "repr", 
+                  "repr": "False"
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__svo_rqt__Svo__1": {
+          "keys": {}, 
+          "groups": {
+            "dock_widget__SvoWidget": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "True"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }, 
+            "dock_widget__SvoUi": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "u'true'"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__rqt_shell__Shell__1": {
+          "keys": {}, 
+          "groups": {
+            "plugin": {
+              "keys": {
+                "shell_type": {
+                  "type": "repr", 
+                  "repr": "u'0'"
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__rqt_console__Console__1": {
+          "keys": {}, 
+          "groups": {
+            "plugin": {
+              "keys": {
+                "show_highlighted_only": {
+                  "type": "repr", 
+                  "repr": "u'false'"
+                }, 
+                "message_limit": {
+                  "type": "repr", 
+                  "repr": "u'20000'"
+                }, 
+                "exclude_filters": {
+                  "type": "repr", 
+                  "repr": "u'severity'"
+                }, 
+                "table_splitter": {
+                  "type": "repr(QByteArray.hex)", 
+                  "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000020000004e000000ed01000000090100000002')", 
+                  "pretty-print": "       N       "
+                }, 
+                "highlight_filters": {
+                  "type": "repr", 
+                  "repr": "u'message'"
+                }, 
+                "paused": {
+                  "type": "repr", 
+                  "repr": "u'false'"
+                }, 
+                "settings_exist": {
+                  "type": "repr", 
+                  "repr": "u'true'"
+                }, 
+                "filter_splitter": {
+                  "type": "repr(QByteArray.hex)", 
+                  "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000000000002000000720000007201000000090100000002')", 
+                  "pretty-print": "       r r     "
+                }
+              }, 
+              "groups": {
+                "exclude_filter_0": {
+                  "keys": {
+                    "enabled": {
+                      "type": "repr", 
+                      "repr": "u'true'"
+                    }, 
+                    "itemlist": {
+                      "type": "repr", 
+                      "repr": "u''"
+                    }
+                  }, 
+                  "groups": {}
+                }, 
+                "highlight_filter_0": {
+                  "keys": {
+                    "regex": {
+                      "type": "repr", 
+                      "repr": "u'false'"
+                    }, 
+                    "text": {
+                      "type": "repr", 
+                      "repr": "u''"
+                    }, 
+                    "enabled": {
+                      "type": "repr", 
+                      "repr": "u'true'"
+                    }
+                  }, 
+                  "groups": {}
+                }
+              }
+            }
+          }
+        }
+      }
+    }, 
+    "mainwindow": {
+      "keys": {
+        "geometry": {
+          "type": "repr(QByteArray.hex)", 
+          "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb00010000000007a9fffffffc00001187000005a7000007b1000000180000117f0000059f000000000000')", 
+          "pretty-print": "                       "
+        }, 
+        "state": {
+          "type": "repr(QByteArray.hex)", 
+          "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd0000000100000003000009cf00000559fc0100000005fb0000002c00730076006f005f007200710074005f005f00530076006f005f005f0031005f005f00530076006f005500690100000000000003890000000000000000fb0000006a007200710074005f0072006f0062006f0074005f007300740065006500720069006e0067005f005f0052006f0062006f0074005300740065006500720069006e0067005f005f0031005f005f0052006f0062006f0074005300740065006500720069006e00670055006901000000000000020b0000000000000000fc00000000000003530000022e00fffffffc0200000004fb0000003400730076006f005f007200710074005f005f00530076006f005f005f0031005f005f00530076006f0057006900640067006500740100000019000000a30000009900fffffffb0000004c007200710074005f0063006f006e0073006f006c0065005f005f0043006f006e0073006f006c0065005f005f0031005f005f0043006f006e0073006f006c006500570069006400670065007401000000c3000003b50000000000000000fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d006100670065005600690065007700570069006400670065007401000000c2000002700000005100fffffffb0000006c007200710074005f007200650063006f006e006600690067007500720065005f005f0050006100720061006d005f005f0031005f005f005f0070006c007500670069006e0063006f006e007400610069006e00650072005f0074006f0070005f00770069006400670065007401000003380000023a0000010f00fffffffb00000026007200710074005f007200760069007a005f005f005200560069007a005f005f0031005f005f010000035900000676000000a700fffffffb00000040007200710074005f007300680065006c006c005f005f005300680065006c006c005f005f0031005f005f0058005400650072006d005700690064006700650074010000091c000000b30000000000000000000009cf0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", 
+          "pretty-print": "                 ,svo_rqt__Svo__1__SvoUi          jrqt_robot_steering__RobotSteering__1__RobotSteeringUi            S .                                                                                                                                              p Q                                                            8 :                           Y v                                                                                                       "
+        }
+      }, 
+      "groups": {
+        "toolbar_areas": {
+          "keys": {
+            "MinimizedDockWidgetsToolbar": {
+              "type": "repr", 
+              "repr": "8"
+            }
+          }, 
+          "groups": {}
+        }
+      }
+    }
+  }
+}
@@ -0,0 +1,153 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 108
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /TF1
+        - /Marker1
+        - /Marker2
+        - /Marker2/Namespaces1
+        - /Image1
+      Splitter Ratio: 0.5
+    Tree Height: 680
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Image
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /svo/keyframes
+      Name: Marker
+      Namespaces:
+        cams: true
+        kfs: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /svo/points
+      Name: Marker
+      Namespaces:
+        pts: true
+        trajectory: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /svo/image
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 255; 255; 255
+    Fixed Frame: /world
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 5.48911
+      Focal Point:
+        X: 0.777511
+        Y: 0.689927
+        Z: -0.445833
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.125204
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.45215
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1416
+  Hide Left Dock: false
+  Hide Right Dock: true
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000002cc00000542fc0200000009fb0000001200530065006c0065006300740069006f006e00000000280000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000355000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000383000001e70000001600ffffff000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003ce0000003efc0100000002fb0000000800540069006d00650000000000000003ce000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000006fd0000054200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 2511
+  X: 1962
+  Y: 24
@@ -0,0 +1,155 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <vector>
+#include <string>
+#include <iostream>
+#include <sophus/se3.h>
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <opencv2/opencv.hpp>
+#include <vikit/file_reader.h>
+#include <vikit/params_helper.h>
+#include <vikit/camera_loader.h>
+#include <vikit/abstract_camera.h>
+#include <vikit/user_input_thread.h>
+#include <svo/config.h>
+#include <svo/frame.h>
+#include <svo/map.h>
+#include <svo/frame_handler_mono.h>
+#include <svo_ros/visualizer.h>
+#include <svo_ros/dataset_img.h>
+
+namespace svo {
+
+class BenchmarkNode
+{
+  svo::FrameHandlerMono* vo_;
+  svo::Visualizer visualizer_;
+  vk::UserInputThread user_input_thread_;
+  int frame_count_;
+  std::ofstream trace_est_pose_;
+  vk::AbstractCamera* cam_;
+  bool quit_;
+
+public:
+  BenchmarkNode(ros::NodeHandle& nh);
+  ~BenchmarkNode();
+  void runCamImuBenchmark();
+  void processUserActions();
+};
+
+BenchmarkNode::BenchmarkNode(ros::NodeHandle& nh) :
+    vo_(NULL),
+    frame_count_(0),
+    quit_(false)
+{
+  // Create Camera
+  if(!vk::camera_loader::loadFromRosNs("svo", cam_))
+    throw std::runtime_error("Camera model not correctly specified.");
+
+  // Initialize VO
+  vo_ = new svo::FrameHandlerMono(cam_);
+  vo_->start();
+}
+
+BenchmarkNode::~BenchmarkNode()
+{
+  delete vo_;
+  delete cam_;
+}
+
+void BenchmarkNode::runCamImuBenchmark()
+{
+  // create image reader and load dataset
+  std::string benchmark_dir(vk::getParam<std::string>("svo/dataset_directory"));
+  std::string filename_benchmark(benchmark_dir + "/images.txt");
+  vk::FileReader<FileType::DatasetImg> dataset_reader(filename_benchmark);
+  dataset_reader.skipComments();
+  if(!dataset_reader.next())
+    std::runtime_error("Failed to open images file");
+  std::vector<FileType::DatasetImg> dataset;
+  dataset_reader.readAllEntries(dataset);
+
+  // create pose tracefile
+  std::string trace_est_name(Config::traceDir() + "/traj_estimate.txt");
+  trace_est_pose_.open(trace_est_name.c_str());
+  if(trace_est_pose_.fail())
+    throw std::runtime_error("ERROR:frame_count_ could not create tracefile. probably the specified folder does not exist.");
+
+  // process dataset
+  for(vk::vector<FileType::DatasetImg>::iterator it = dataset.begin();
+      it != dataset.end(); ++it, ++frame_count_)
+  {
+    // Read image
+    std::string img_filename(benchmark_dir + "/" + it->imgname_);
+    cv::Mat img(cv::imread(img_filename, 0));
+    if(frame_count_ == 0)
+      SVO_INFO_STREAM("Reading image "<<img_filename);
+    if(img.empty())
+      throw std::runtime_error("Image could not be loaded.");
+
+    // Add image to VO
+    processUserActions();
+    vo_->addImage(img, it->stamp_);
+
+    // Visualize
+    visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, it->stamp_);
+    if(vo_->stage() != svo::FrameHandlerMono::PAUSED)
+      visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map());
+
+    // write pose to tracefile
+    if(vo_->stage() == svo::FrameHandlerMono::DEFAULT_FRAME)
+    {
+      Quaterniond q(vo_->lastFrame()->T_f_w_.inverse().unit_quaternion());
+      Vector3d p(vo_->lastFrame()->T_f_w_.inverse().translation());
+      trace_est_pose_.precision(15);
+      trace_est_pose_.setf(std::ios::fixed, std::ios::floatfield );
+      trace_est_pose_ << it->stamp_<< " ";
+      trace_est_pose_.precision(6);
+      trace_est_pose_ << p.x() << " " << p.y() << " " << p.z() << " "
+                      << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;
+    }
+    if (!ros::ok() || quit_)
+      break;
+  }
+}
+
+void BenchmarkNode::processUserActions()
+{
+  char input = user_input_thread_.getInput();
+  switch(input)
+  {
+    case 'q': quit_ = true;   break;
+    case 'r': vo_->reset(); break;
+    case 's': vo_->start(); break;
+    default: ;
+  }
+}
+
+} // namespace svo
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "svo");
+  ros::NodeHandle nh;
+  {
+    svo::BenchmarkNode benchmark(nh);
+    benchmark.runCamImuBenchmark();
+  }
+  printf("BenchmarkNode finished.\n");
+  return 0;
+}
@@ -0,0 +1,169 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <map>
+#include <set>
+#include <ros/ros.h>
+#include <vikit/user_input_thread.h>
+#include <vikit/ringbuffer.h>
+#include <std_msgs/String.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <svo_msgs/Info.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <vikit/output_helper.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <algorithm>
+
+class RemoteHandler
+{
+public:
+  ros::NodeHandle& nh_;
+  vk::UserInputThread input_thread_;
+
+  ros::Publisher pub_key_;
+  ros::Publisher pub_frames_;
+  tf::TransformBroadcaster br_;
+
+  vk::RingBuffer<double> acc_timing_;
+  vk::RingBuffer<int> acc_obs_;
+  typedef std::map<size_t, std::pair<Eigen::Vector3d, Eigen::Matrix3d> > pose_map_t;
+  pose_map_t poses_;
+  std::set<int> published_kfs_;
+  std::set<int> current_kfs_;
+
+  RemoteHandler(ros::NodeHandle& nh);
+  ~RemoteHandler();
+  void handleUserInput();
+  void poseCb(const geometry_msgs::PoseWithCovarianceStampedConstPtr msg);
+  void infoCb(const svo_msgs::InfoConstPtr msg);
+};
+
+RemoteHandler::RemoteHandler(ros::NodeHandle& nh) :
+    nh_(nh),
+    pub_key_(nh_.advertise<std_msgs::String> ("/svo/remote_key", 10)),
+    pub_frames_(nh_.advertise<visualization_msgs::Marker>("/svo/vis/frame", 100)),
+    acc_timing_(50),
+    acc_obs_(50)
+{}
+
+RemoteHandler::~RemoteHandler()
+{}
+
+void RemoteHandler::handleUserInput()
+{
+  char key = input_thread_.getInput();
+  std_msgs::StringPtr msg(new std_msgs::String);
+  switch(key)
+  {
+    case 'q': msg->data = 'q'; pub_key_.publish(msg); break; // quit
+    case 'r': msg->data = 'r'; pub_key_.publish(msg); break; // reset
+    case 'p': msg->data = 'p'; pub_key_.publish(msg); break; // pause
+    case 's': msg->data = 's'; pub_key_.publish(msg); break; // start
+    default: ;
+  }
+}
+
+void RemoteHandler::poseCb(const geometry_msgs::PoseWithCovarianceStampedConstPtr msg)
+{
+  Eigen::Vector3d p(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
+  Eigen::Quaterniond q(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
+  Eigen::Matrix3d R_world_from_frame(q.inverse());
+  Eigen::Vector3d p_frame_in_world(-R_world_from_frame*p);
+
+  poses_.insert(std::pair<size_t, std::pair<Eigen::Vector3d, Eigen::Matrix3d> >(
+        msg->header.seq,
+        std::pair<Eigen::Vector3d, Eigen::Matrix3d>(p_frame_in_world, R_world_from_frame)));
+
+  // publish trajectory
+  vk::output_helper::publishPointMarker(pub_frames_, p_frame_in_world, "trajectory_points",
+                         msg->header.stamp, msg->header.seq, 0, 0.006, Eigen::Vector3d(0.,0.,0.5));
+
+  // publish camera pose tf
+  tf::StampedTransform transform_msg;
+  transform_msg.frame_id_ = "cam";
+  transform_msg.child_frame_id_ = "world";
+  transform_msg.stamp_ = msg->header.stamp;
+  transform_msg.setOrigin(tf::Vector3(p.x(), p.y(), p.z()));
+  transform_msg.setRotation(tf::Quaternion(q.x(), q.y(), q.z(), q.w()));
+  br_.sendTransform(transform_msg);
+
+  // publish keyframes
+  std::set<int> deleted_kfs;
+  std::set_difference(published_kfs_.begin(), published_kfs_.end(), current_kfs_.begin(), current_kfs_.end(), std::inserter(deleted_kfs, deleted_kfs.end()));
+  for(std::set<int>::iterator it=deleted_kfs.begin(); it!=deleted_kfs.end(); ++it)
+  {
+    pose_map_t::iterator kf = poses_.find(*it);
+    if(kf != poses_.end())
+      vk::output_helper::publishFrameMarker(pub_frames_, kf->second.second, kf->second.first, "kfs", ros::Time::now(), (*it)*10, 2, 0.015);
+  }
+
+  std::set<int> unpublished_kfs;
+  std::set_difference(current_kfs_.begin(), current_kfs_.end(), published_kfs_.begin(), published_kfs_.end(), std::inserter(unpublished_kfs, unpublished_kfs.end()));
+  for(std::set<int>::iterator it=unpublished_kfs.begin(); it!=unpublished_kfs.end(); ++it)
+  {
+    pose_map_t::iterator kf = poses_.find(*it);
+    if(kf != poses_.end())
+      vk::output_helper::publishFrameMarker(pub_frames_, kf->second.second, kf->second.first, "kfs", ros::Time::now(), (*it)*10, 0, 0.015);
+  }
+  published_kfs_.swap(current_kfs_);
+
+
+  // publish hexacopter
+  vk::output_helper::publishHexacopterMarker(pub_frames_, "cam", "hexacopter", msg->header.stamp, 1, 0, 0.3, Eigen::Vector3d(0.,0.,1.));
+}
+
+void RemoteHandler::infoCb(const svo_msgs::InfoConstPtr msg)
+{
+  std::string kfmsg("");
+  current_kfs_.clear();
+  for(std::vector<int32_t>::const_iterator it=msg->keyframes.begin(); it!=msg->keyframes.end(); ++it) {
+    current_kfs_.insert(static_cast<int>(*it));
+    if(*it == static_cast<int32_t>(msg->header.seq)) {
+      kfmsg = "Keyframe!";
+      break;
+    }
+  }
+  acc_timing_.push_back(msg->processing_time);
+  acc_obs_.push_back(msg->num_matches);
+
+  printf("[%i] t=%.2fhz (mean=%.2f) \t n_obs=%i (mean=%i) \t %s\n",
+         msg->header.seq, 1./msg->processing_time, 1./acc_timing_.getMean(),
+         msg->num_matches, acc_obs_.getMean(),  kfmsg.c_str());
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "svo_remote");
+  ros::NodeHandle nh;
+  RemoteHandler node(nh);
+
+  printf("Listening to User-Input...\n");
+
+  ros::Subscriber sub_pose = nh.subscribe("/svo/pose", 10, &RemoteHandler::poseCb, &node);
+  ros::Subscriber sub_info = nh.subscribe("/svo/info", 10, &RemoteHandler::infoCb, &node);
+  ros::Rate r(100);
+  while (ros::ok())
+  {
+    node.handleUserInput();
+    ros::spinOnce();
+    r.sleep();
+  }
+  return 0;
+}
@@ -0,0 +1,268 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <svo_ros/visualizer.h>
+#include <svo/frame_handler_mono.h>
+#include <svo/frame.h>
+#include <svo/point.h>
+#include <svo/map.h>
+#include <svo/feature.h>
+#include <svo_msgs/Info.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <sensor_msgs/image_encodings.h>
+#include <cv_bridge/cv_bridge.h>
+#include <tf/tf.h>
+#include <iostream>
+#include <fstream>
+#include <ros/package.h>
+#include <vikit/timer.h>
+#include <vikit/output_helper.h>
+#include <vikit/params_helper.h>
+#include <deque>
+#include <algorithm>
+#include <svo_msgs/DenseInput.h>
+
+namespace svo {
+
+Visualizer::
+Visualizer() :
+    pnh_("~"),
+    trace_id_(0),
+    img_pub_level_(vk::getParam<int>("svo/image_publishing_level", 0)),
+    img_pub_nth_(vk::getParam<int>("svo/publish_every_nth_img", 1)),
+    dense_pub_nth_(vk::getParam<int>("svo/publish_every_nth_dense_input", 1))
+{
+  // 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_info_ = pnh_.advertise<svo_msgs::Info>("info", 10);
+  pub_dense_ = pnh_.advertise<svo_msgs::DenseInput>("dense_input",10);
+
+  // create video publisher
+  image_transport::ImageTransport it(pnh_);
+  pub_images_ = it.advertise("image", 10);
+}
+
+void Visualizer::
+visualizeMarkers(const FramePtr& frame, const set<FramePtr>& core_kfs, const Map& map)
+{
+  if(frame != NULL)
+  {
+    // publish camera pose tf
+    vk::output_helper::publishTfTransform(frame->T_f_w_, ros::Time(frame->timestamp_), "cam_pos", "world", br_);
+    if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0)
+    {
+      vk::output_helper::publishHexacopterMarker(pub_frames_, "cam_pos", "cams", ros::Time(frame->timestamp_), 1, 0, 0.3, Vector3d(0.,0.,1.));
+      vk::output_helper::publishPointMarker(pub_points_, frame->pos(), "trajectory", ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5));
+      publishMapRegion(core_kfs);
+      removeDeletedPts(map);
+    }
+  }
+}
+
+void Visualizer::
+publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono& slam, const double timestamp)
+{
+  ++trace_id_;
+  std_msgs::Header header_msg;
+  header_msg.frame_id = "/cam";
+  header_msg.seq = trace_id_;
+  header_msg.stamp = ros::Time(timestamp);
+
+  // publish svo info msgs
+  if(pub_info_.getNumSubscribers() > 0)
+  {
+    svo_msgs::Info msg_info;
+    msg_info.header = header_msg;
+    msg_info.processing_time = slam.lastProcessingTime();
+    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);
+  }
+
+  // publish pyramid-image every nth-frame
+  if(frame != NULL && img_pub_nth_ > 0
+      && trace_id_%img_pub_nth_ == 0 && pub_images_.getNumSubscribers() > 0)
+  {
+    const int scale = (1<<img_pub_level_);
+    cv::Mat img_rgb(frame->img_pyr_[img_pub_level_].size(), CV_8UC3);
+    cv::cvtColor(frame->img_pyr_[img_pub_level_], img_rgb, CV_GRAY2RGB);
+
+    // during initialization, draw lines
+    if(slam.stage() == FrameHandlerBase::SECOND_FRAME)
+    {
+      const vector<cv::Point2f>& px_ref(slam.initFeatureTrackRefPx());
+      const vector<cv::Point2f>& px_cur(slam.initFeatureTrackCurPx());
+      for(vector<cv::Point2f>::const_iterator it_ref=px_ref.begin(), it_cur=px_cur.begin();
+          it_ref != px_ref.end(); ++it_ref, ++it_cur)
+        cv::line(img_rgb,
+                 cv::Point2f(it_cur->x/scale, it_cur->y/scale),
+                 cv::Point2f(it_ref->x/scale, it_ref->y/scale), cv::Scalar(0,255,0), 2);
+    }
+
+    // draw features on frame
+    if(img_pub_level_ == 0)
+      for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
+        cv::rectangle(img_rgb,
+                      cv::Point2f((*it)->px[0]-2, (*it)->px[1]-2),
+                      cv::Point2f((*it)->px[0]+2, (*it)->px[1]+2),
+                      cv::Scalar(0,255,0), CV_FILLED);
+    if(img_pub_level_ == 1)
+      for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
+        cv::rectangle(img_rgb,
+                      cv::Point2f((*it)->px[0]/scale-1, (*it)->px[1]/scale-1),
+                      cv::Point2f((*it)->px[0]/scale+1, (*it)->px[1]/scale+1),
+                      cv::Scalar(0,255,0), CV_FILLED);
+    else
+      for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
+        cv::rectangle(img_rgb,
+                      cv::Point2f((*it)->px[0]/scale, (*it)->px[1]/scale),
+                      cv::Point2f((*it)->px[0]/scale, (*it)->px[1]/scale),
+                      cv::Scalar(0,255,0), CV_FILLED);
+
+    // publish image
+    cv_bridge::CvImage img_msg;
+    img_msg.header = header_msg;
+    img_msg.image = img_rgb;
+    img_msg.encoding = sensor_msgs::image_encodings::BGR8;
+    pub_images_.publish(img_msg.toImageMsg());
+  }
+  else if(frame == NULL && pub_images_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::PAUSED)
+  {
+    // display image when slam is not running
+    cv_bridge::CvImage img_msg;
+    img_msg.header.stamp = ros::Time::now();
+    img_msg.header.frame_id = "/image";
+    img_msg.image = img;
+    img_msg.encoding = sensor_msgs::image_encodings::MONO8;
+    pub_images_.publish(img_msg.toImageMsg());
+  }
+
+  // publish pose (world in camera frame)
+  if(pub_pose_.getNumSubscribers() > 0 && frame != NULL)
+  {
+    geometry_msgs::PoseWithCovarianceStampedPtr msg_pose(new geometry_msgs::PoseWithCovarianceStamped);
+    Quaterniond q(frame->T_f_w_.rotation_matrix());
+    Vector3d p(frame->T_f_w_.translation());
+    msg_pose->header = header_msg;
+    msg_pose->pose.pose.position.x = p[0];
+    msg_pose->pose.pose.position.y = p[1];
+    msg_pose->pose.pose.position.z = p[2];
+    msg_pose->pose.pose.orientation.x = q.x();
+    msg_pose->pose.pose.orientation.y = q.y();
+    msg_pose->pose.pose.orientation.z = q.z();
+    msg_pose->pose.pose.orientation.w = q.w();
+    for(size_t i=0; i<36; ++i)
+      msg_pose->pose.covariance[i] = frame->Cov_(i%6, i/6);
+    pub_pose_.publish(msg_pose);
+  }
+}
+
+void Visualizer::
+publishMapRegion(set<FramePtr> frames)
+{
+  if(pub_points_.getNumSubscribers() > 0)
+  {
+    int ts = vk::Timer::getCurrentTime();
+    for(set<FramePtr>::iterator it=frames.begin(); it!=frames.end(); ++it)
+      displayKeyframeWithMps(*it, ts);
+  }
+}
+
+void Visualizer::
+removeDeletedPts(const Map& map)
+{
+  if(pub_points_.getNumSubscribers() > 0)
+  {
+    for(list<Point*>::const_iterator it=map.trash_points_.begin(); it!=map.trash_points_.end(); ++it)
+      vk::output_helper::publishPointMarker(pub_points_, Vector3d(), "pts", ros::Time::now(), (*it)->id_, 2, 0.006, Vector3d());
+  }
+}
+
+void Visualizer::
+displayKeyframeWithMps(const FramePtr& frame, int ts)
+{
+  // publish keyframe
+  vk::output_helper::publishFrameMarker(pub_frames_, frame->T_f_w_.inverse().rotation_matrix(), frame->pos(), "kfs", ros::Time::now(), frame->id_*10, 0, 0.015);
+
+  // publish point cloud and links
+  for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
+  {
+    if((*it)->point == NULL)
+      continue;
+
+    if((*it)->point->last_published_ts_ == ts)
+      continue;
+
+    vk::output_helper::publishPointMarker(pub_points_, (*it)->point->pos_, "pts", ros::Time::now(), (*it)->point->id_, 0, 0.005, Vector3d(1.0, 0., 1.0));
+    (*it)->point->last_published_ts_ = ts;
+  }
+}
+
+void Visualizer::
+exportToDense(const FramePtr& frame)
+{
+  // publish air_ground_msgs
+  if(frame != NULL && dense_pub_nth_ > 0
+      && trace_id_%dense_pub_nth_ == 0 && pub_dense_.getNumSubscribers() > 0)
+  {
+    svo_msgs::DenseInput msg;
+    msg.header.stamp = ros::Time(frame->timestamp_);
+    msg.header.frame_id = "/world";
+    msg.frame_id = frame->id_;
+
+    cv_bridge::CvImage img_msg;
+    img_msg.header.stamp = msg.header.stamp;
+    img_msg.header.frame_id = "camera";
+    img_msg.image = frame->img();
+    img_msg.encoding = sensor_msgs::image_encodings::MONO8;
+    msg.image = *img_msg.toImageMsg();
+
+    double min_z = std::numeric_limits<double>::max();
+    double max_z = std::numeric_limits<double>::min();
+    for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
+    {
+      if((*it)->point==NULL)
+        continue;
+      Vector3d pos = frame->T_f_w_*(*it)->point->pos_;
+      min_z = min(pos[2], min_z);
+      max_z = max(pos[2], max_z);
+    }
+    msg.min_depth = (float) min_z;
+    msg.max_depth = (float) max_z;
+
+    Quaterniond q(frame->T_f_w_.inverse().rotation_matrix());
+    Vector3d p(frame->T_f_w_.inverse().translation());
+    msg.pose.position.x = p[0];
+    msg.pose.position.y = p[1];
+    msg.pose.position.z = p[2];
+    msg.pose.orientation.w = q.w();
+    msg.pose.orientation.x = q.x();
+    msg.pose.orientation.y = q.y();
+    msg.pose.orientation.z = q.z();
+    pub_dense_.publish(msg);
+  }
+}
+
+} // end namespace svo
@@ -0,0 +1,181 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <ros/package.h>
+#include <string>
+#include <svo/frame_handler_mono.h>
+#include <svo/map.h>
+#include <svo/config.h>
+#include <svo_ros/visualizer.h>
+#include <vikit/params_helper.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/Imu.h>
+#include <std_msgs/String.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <image_transport/image_transport.h>
+#include <boost/thread.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <Eigen/Core>
+#include <vikit/abstract_camera.h>
+#include <vikit/camera_loader.h>
+#include <vikit/user_input_thread.h>
+
+namespace svo {
+
+using namespace std;
+using namespace vk;
+
+/// SVO Interface
+class VoNode
+{
+public:
+  svo::FrameHandlerMono* vo_;
+  svo::Visualizer visualizer_;
+  bool publish_markers_;                 //!< publish only the minimal amount of info (choice for embedded devices)
+  bool publish_dense_input_;
+  vk::UserInputThread* user_input_thread_;
+  ros::Subscriber sub_remote_key_;
+  string remote_input_;
+  vk::AbstractCamera* cam_;
+  bool quit_;
+  VoNode();
+  ~VoNode();
+  void imgCb(const sensor_msgs::ImageConstPtr& msg);
+  void processUserActions();
+  void remoteKeyCb(const std_msgs::StringConstPtr& key_input);
+};
+
+VoNode::
+VoNode() :
+  vo_(NULL),
+  publish_markers_(vk::getParam<bool>("svo/publish_markers", true)),
+  publish_dense_input_(vk::getParam<bool>("svo/publish_dense_input", false)),
+  user_input_thread_(new vk::UserInputThread()),
+  remote_input_(""),
+  cam_(NULL),
+  quit_(false)
+{
+  // Create Camera
+  if(!vk::camera_loader::loadFromRosNs("svo", cam_))
+    throw std::runtime_error("Camera model not correctly specified.");
+
+  // Init camera
+  vo_ = new svo::FrameHandlerMono(cam_);
+
+  // Set initial position and orientation
+  Sophus::SE3 T_cam_from_world(
+      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))),
+      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)));
+  vo_->setInitialPose(T_cam_from_world);
+  vo_->start();
+}
+
+VoNode::
+~VoNode()
+{
+  delete vo_;
+  delete user_input_thread_;
+  delete cam_;
+}
+
+void VoNode::
+imgCb(const sensor_msgs::ImageConstPtr& msg)
+{
+  cv::Mat img;
+  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());
+
+  if(publish_markers_ && vo_->stage() != FrameHandlerBase::PAUSED)
+    visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map());
+
+  if(publish_dense_input_)
+    visualizer_.exportToDense(vo_->lastFrame());
+
+  if(vo_->stage() == FrameHandlerMono::PAUSED)
+    usleep(100000); // avoid busy loop when paused
+}
+
+void VoNode::processUserActions()
+{
+  char input = remote_input_.c_str()[0];
+  remote_input_ = "";
+
+  char console_input = user_input_thread_->getInput();
+  if(console_input != 0)
+    input = console_input;
+
+  switch(input)
+  {
+    case 'q':
+      quit_ = true;
+      printf("Svo User Input: QUIT\n");
+      break;
+    case 'r':
+      vo_->reset();
+      printf("Svo User Input: RESET\n");
+      break;
+    case 's':
+      vo_->start();
+      printf("Svo User Input: START\n");
+      break;
+    default: ;
+  }
+}
+
+void VoNode::remoteKeyCb(const std_msgs::StringConstPtr& key_input)
+{
+  remote_input_ = key_input->data;
+}
+
+} // namespace svo
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "svo");
+  ros::NodeHandle nh;
+  std::cout << "create vo_node" << std::endl;
+  svo::VoNode vo_node;
+
+  // 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);
+
+  // start processing callbacks
+  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!
+  }
+
+  printf("Svo terminated.\n");
+  return 0;
+}