first commit

svo_ros/src/benchmark_node.cpp

@@ -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;
+}

svo_ros/src/remote_node.cpp

@@ -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;
+}

svo_ros/src/visualizer.cpp

@@ -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

svo_ros/src/vo_node.cpp

@@ -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;
+}