diff --git a/svo/include/svo/config.h b/svo/include/svo/config.h
index 3ad547b..d4f50df 100644
--- a/svo/include/svo/config.h
+++ b/svo/include/svo/config.h
@@ -14,8 +14,8 @@
 // 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 NSLAM_CONFIG_H_
-#define NSLAM_CONFIG_H_
+#ifndef SVO_CONFIG_H_
+#define SVO_CONFIG_H_
 
 #include <string>
 #include <stdint.h>
@@ -23,7 +23,7 @@
 
 namespace svo {
 
-using namespace std;
+using std::string;
 
 /// Global configuration file of SVO.
 /// Implements the Singleton design pattern to allow global access and to ensure
@@ -154,6 +154,6 @@ private:
   int quality_max_drop_fts;
 };
 
-} // namespace nslam
+} // namespace svo
 
-#endif // NSLAM_CONFIG_H_
+#endif // SVO_CONFIG_H_
diff --git a/svo/include/svo/depth_filter.h b/svo/include/svo/depth_filter.h
index d943764..955799a 100644
--- a/svo/include/svo/depth_filter.h
+++ b/svo/include/svo/depth_filter.h
@@ -46,7 +46,7 @@ struct Seed
   float z_range;               //!< Max range of the possible depth.
   float sigma2;                //!< Variance of normal distribution.
   Matrix2d patch_cov;          //!< Patch covariance in reference image.
-  Seed(Feature* ftr, float angle, float depth_mean, float depth_min);
+  Seed(Feature* ftr, float depth_mean, float depth_min);
 };
 
 /// Depth filter implements the Bayesian Update proposed in:
@@ -64,7 +64,8 @@ public:
   typedef boost::function<void ( const Seed&, const Vector3d& )> callback_t;
 
   /// Depth-filter config parameters
-  struct Options {
+  struct Options
+  {
     bool check_ftr_angle;                       //!< gradient features are only updated if the epipolar line is orthogonal to the gradient.
     bool epi_search_1d;                         //!< restrict Gauss Newton in the epipolar search to the epipolar line.
     bool verbose;                               //!< display output.
@@ -83,8 +84,11 @@ public:
     {}
   } options_;
 
-  DepthFilter(feature_detection::DetectorPtr feature_detector, callback_t seed_converged_cb);
-  ~DepthFilter();
+  DepthFilter(
+      feature_detection::DetectorPtr feature_detector,
+      callback_t seed_converged_cb);
+
+  virtual ~DepthFilter();
 
   /// Start this thread when seed updating should be in a parallel thread.
   void startThread();
@@ -129,7 +133,7 @@ public:
       const double z,
       const double px_error_angle);
 
-private:
+protected:
   feature_detection::DetectorPtr feature_detector_;
   callback_t seed_converged_cb_;
   std::list<Seed, aligned_allocator<Seed> > seeds_;
@@ -149,7 +153,7 @@ private:
   void initializeSeeds(FramePtr frame);
 
   /// Update all seeds with a new measurement frame.
-  void updateSeeds(FramePtr frame);
+  virtual void updateSeeds(FramePtr frame);
 
   /// When a new keyframe arrives, the frame queue should be cleared.
   void clearFrameQueue();
diff --git a/svo/include/svo/feature.h b/svo/include/svo/feature.h
index a0a30f4..37be2b7 100644
--- a/svo/include/svo/feature.h
+++ b/svo/include/svo/feature.h
@@ -26,34 +26,47 @@ struct Feature
 {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  Frame*   frame;       //!< Pointer to frame in which the feature was detected.
+  enum FeatureType {
+    CORNER,
+    EDGELET
+  };
+
+  FeatureType type;     //!< Type can be corner or edgelet.
+  Frame* frame;         //!< Pointer to frame in which the feature was detected.
   Vector2d px;          //!< Coordinates in pixels on pyramid level 0.
   Vector3d f;           //!< Unit-bearing vector of the feature.
-  int      level;       //!< Image pyramid level where feature was extracted.
-  Point*   point;       //!< Pointer to 3D point which corresponds to the feature.
+  int level;            //!< Image pyramid level where feature was extracted.
+  Point* point;         //!< Pointer to 3D point which corresponds to the feature.
+  Vector2d grad;        //!< Dominant gradient direction for edglets, normalized.
 
-  Feature(Frame* _frame, const Vector2d& _px, int _level)
-   : frame(_frame),
-     px(_px),
-     f(frame->cam_->cam2world(px)),
-     level(_level),
-     point(NULL)
+  Feature(Frame* _frame, const Vector2d& _px, int _level) :
+    type(CORNER),
+    frame(_frame),
+    px(_px),
+    f(frame->cam_->cam2world(px)),
+    level(_level),
+    point(NULL),
+    grad(0.0,0.0)
   {}
 
-  Feature(Frame* _frame, const Vector2d& _px, const Vector3d& _f, int _level)
-  : frame(_frame),
+  Feature(Frame* _frame, const Vector2d& _px, const Vector3d& _f, int _level) :
+    type(CORNER),
+    frame(_frame),
     px(_px),
     f(_f),
     level(_level),
-    point(NULL)
+    point(NULL),
+    grad(0.0,0.0)
   {}
 
-  Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, int _level)
-    : frame(_frame),
-      px(_px),
-      f(_f),
-      level(_level),
-      point(_point)
+  Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, int _level) :
+    type(CORNER),
+    frame(_frame),
+    px(_px),
+    f(_f),
+    level(_level),
+    point(_point),
+    grad(0.0,0.0)
   {}
 };
 
diff --git a/svo/include/svo/feature_detection.h b/svo/include/svo/feature_detection.h
index a84586a..d0990a8 100644
--- a/svo/include/svo/feature_detection.h
+++ b/svo/include/svo/feature_detection.h
@@ -26,7 +26,6 @@ namespace svo {
 namespace feature_detection {
 
 /// Temporary container used for corner detection. Features are initialized from these.
-// TODO: why not use feature struct?
 struct Corner
 {
   int x;        //!< x-coordinate of corner in the image.
@@ -44,14 +43,42 @@ typedef vector<Corner> Corners;
 class AbstractDetector
 {
 public:
-  virtual ~AbstractDetector() {};
-  virtual void detect(
-      const ImgPyr& img_pyr,
-      const Features& fts,
+  AbstractDetector(
+      const int img_width,
+      const int img_height,
       const int cell_size,
-      const int n_levels,
+      const int n_pyr_levels);
+
+  virtual ~AbstractDetector() {};
+
+  virtual void detect(
+      Frame* frame,
+      const ImgPyr& img_pyr,
       const double detection_threshold,
-      Corners* corners) const = 0;
+      Features& fts) = 0;
+
+  /// Flag the grid cell as occupied
+  void setGridOccpuancy(const Vector2d& px);
+
+  /// Set grid cells of existing features as occupied
+  void setExistingFeatures(const Features& fts);
+
+protected:
+
+  static const int border_ = 8; //!< no feature should be within 8px of border.
+  const int cell_size_;
+  const int n_pyr_levels_;
+  const int grid_n_cols_;
+  const int grid_n_rows_;
+  vector<bool> grid_occupancy_;
+
+  void resetGrid();
+
+  inline int getCellIndex(int x, int y, int level)
+  {
+    const int scale = (1<<level);
+    return (scale*y)/cell_size_*grid_n_cols_ + (scale*x)/cell_size_;
+  }
 };
 typedef boost::shared_ptr<AbstractDetector> DetectorPtr;
 
@@ -59,15 +86,19 @@ typedef boost::shared_ptr<AbstractDetector> DetectorPtr;
 class FastDetector : public AbstractDetector
 {
 public:
-  FastDetector() {}
+  FastDetector(
+      const int img_width,
+      const int img_height,
+      const int cell_size,
+      const int n_pyr_levels);
+
   virtual ~FastDetector() {}
+
   virtual void detect(
-        const ImgPyr& img_pyr,
-        const Features& fts,
-        const int cell_size,
-        const int n_levels,
-        const double detection_threshold,
-        Corners* corners) const;
+      Frame* frame,
+      const ImgPyr& img_pyr,
+      const double detection_threshold,
+      Features& fts);
 };
 
 } // namespace feature_detection
diff --git a/svo/include/svo/frame.h b/svo/include/svo/frame.h
index f4413e7..8fcfd2c 100644
--- a/svo/include/svo/frame.h
+++ b/svo/include/svo/frame.h
@@ -79,7 +79,7 @@ public:
   void removeKeyPoint(Feature* ftr);
 
   /// Return number of point observations.
-  size_t nObs() const;
+  inline size_t nObs() const { return fts_.size(); }
 
   /// Check if a point in (w)orld coordinate frame is visible in the image.
   bool isVisible(const Vector3d& xyz_w) const;
diff --git a/svo/include/svo/frame_handler_base.h b/svo/include/svo/frame_handler_base.h
index 66d4595..bbde3ce 100644
--- a/svo/include/svo/frame_handler_base.h
+++ b/svo/include/svo/frame_handler_base.h
@@ -63,9 +63,6 @@ public:
 
   virtual ~FrameHandlerBase();
 
-  /// Set initial pose of the camera.
-  void setInitialPose(const SE3& T_frame_from_world) { T_f_w_init_ = T_frame_from_world; }
-
   /// Get the current map.
   const Map& map() const { return map_; }
 
@@ -92,7 +89,6 @@ protected:
   bool set_reset_;              //!< Flag that the user can set. Will reset the system before the next iteration.
   bool set_start_;              //!< Flag the user can set to start the system when the next image is received.
   Map map_;                     //!< Map of keyframes created by the slam system.
-  SE3 T_f_w_init_;              //!< Initial pose of the camera.
   vk::Timer timer_;             //!< Stopwatch to measure time to process frame.
   vk::RingBuffer<double> acc_frame_timings_;    //!< Total processing time of the last 10 frames, used to give some user feedback on the performance.
   vk::RingBuffer<size_t> acc_num_obs_;          //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance.
diff --git a/svo/include/svo/point.h b/svo/include/svo/point.h
index 933b8bc..72a7221 100644
--- a/svo/include/svo/point.h
+++ b/svo/include/svo/point.h
@@ -56,7 +56,8 @@ public:
   int                         n_succeeded_reproj_;      //!< Number of succeeded reprojections. Used to assess the quality of the point.
   int                         last_structure_optim_;    //!< Timestamp of last point optimization
 
-  Point(Vector3d pos);
+  Point(const Vector3d& pos);
+  Point(const Vector3d& pos, Feature* ftr);
   ~Point();
 
   /// Add a reference to a frame.
diff --git a/svo/include/svo/sparse_img_align.h b/svo/include/svo/sparse_img_align.h
index 17cb3a1..c9da479 100644
--- a/svo/include/svo/sparse_img_align.h
+++ b/svo/include/svo/sparse_img_align.h
@@ -51,6 +51,10 @@ public:
       FramePtr ref_frame,
       FramePtr cur_frame);
 
+  /// Return fisher information matrix, i.e. the Hessian of the log-likelihood
+  /// at the converged state.
+  Matrix<double, 6, 6> getFisherInformation();
+
 protected:
   FramePtr ref_frame_;            //!< reference frame, has depth for gradient pixels.
   FramePtr cur_frame_;            //!< only the image is known!
diff --git a/svo/src/depth_filter.cpp b/svo/src/depth_filter.cpp
index ce34e69..2cbafe2 100644
--- a/svo/src/depth_filter.cpp
+++ b/svo/src/depth_filter.cpp
@@ -34,7 +34,7 @@ namespace svo {
 int Seed::batch_counter = 0;
 int Seed::seed_counter = 0;
 
-Seed::Seed(Feature* ftr, float angle, float depth_mean, float depth_min) :
+Seed::Seed(Feature* ftr, float depth_mean, float depth_min) :
     batch_id(batch_counter),
     id(seed_counter++),
     ftr(ftr),
@@ -113,32 +113,20 @@ void DepthFilter::addKeyframe(FramePtr frame, double depth_mean, double depth_mi
 
 void DepthFilter::initializeSeeds(FramePtr frame)
 {
-  feature_detection::Corners corners;
-  feature_detector_->detect(frame->img_pyr_, frame->fts_, svo::Config::gridSize(),
-                            svo::Config::nPyrLevels(), Config::triangMinCornerScore(),
-                            &corners);
+  Features new_features;
+  feature_detector_->setExistingFeatures(frame->fts_);
+  feature_detector_->detect(frame.get(), frame->img_pyr_,
+                            Config::triangMinCornerScore(), new_features);
 
+  // initialize a seed for every new feature
   seeds_updating_halt_ = true;
   lock_t lock(seeds_mut_); // by locking the updateSeeds function stops
-  size_t new_seeds = 0;
   ++Seed::batch_counter;
+  std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){
+    seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));
+  });
 
-  // now for all maximum corners, initialize a new seed
-  for(feature_detection::Corners::iterator it=corners.begin(); it!=corners.end(); ++it)
-  {
-    // we must check again if the score is above the threshold because the Corners
-    // vector is initialized with dummy corners
-    if(it->score > Config::triangMinCornerScore())
-    {
-      Feature* new_feature(new Feature(frame.get(), Vector2d(it->x, it->y), it->level));
-      seeds_.push_back(Seed(new_feature, it->angle, new_keyframe_mean_depth_, new_keyframe_min_depth_));
-      ++new_seeds;
-    }
-  }
-
-  if(options_.verbose)
-    SVO_INFO_STREAM("DepthFilter: Initialized "<<new_seeds<<" new seeds");
-
+  SVO_DEBUG_STREAM("DepthFilter: Initialized "<<new_features.size()<<" new seeds");
   seeds_updating_halt_ = false;
 }
 
@@ -174,8 +162,7 @@ void DepthFilter::reset()
     frame_queue_.pop();
   seeds_updating_halt_ = false;
 
-  if(options_.verbose)
-    SVO_INFO_STREAM("DepthFilter: RESET.");
+  SVO_DEBUG_STREAM("DepthFilter: RESET.");
 }
 
 void DepthFilter::updateSeedsLoop()
@@ -234,11 +221,8 @@ void DepthFilter::updateSeeds(FramePtr frame)
       ++it; // behind the camera
       continue;
     }
-    const Vector2d px(it->ftr->frame->f2c(xyz_f));
-    if(!it->ftr->frame->cam_->isInFrame(px.cast<int>())) {
-      ++it;
-      if(options_.verbose)
-    	SVO_DEBUG_STREAM("not visible");
+    if(!it->ftr->frame->cam_->isInFrame(it->ftr->frame->f2c(xyz_f).cast<int>())) {
+      ++it; // point does not project in image
       continue;
     }
 
@@ -250,25 +234,12 @@ void DepthFilter::updateSeeds(FramePtr frame)
         *it->ftr->frame, *frame, *it->ftr, 1.0/it->mu, 1.0/z_inv_min, 1.0/z_inv_max,
         options_.epi_search_1d, z, h_inv))
     {
-      if(options_.verbose)
-        SVO_INFO_STREAM("no match found");
       it->b++; // increase outlier probability when no match was found
       ++it;
       ++n_failed_matches;
       continue;
     }
 
-    if(options_.use_photometric_disparity_error && h_inv > 0)
-    {
-      px_noise = fmax(2.0*options_.sigma_i_sq*h_inv, 1.0);
-      px_error_angle = atan(px_noise/(2.0*focal_length))*2.0; // law of chord (sehnensatz)
-      if(options_.verbose)
-    	SVO_DEBUG_STREAM("Photometric disparity error "<<px_noise<<" px");
-    }
-
-    if(options_.verbose)
-      SVO_DEBUG_STREAM("updated seed");
-
     // compute tau
     double tau = computeTau(T_ref_cur, it->ftr->f, z, px_error_angle);
     double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));
@@ -284,8 +255,6 @@ void DepthFilter::updateSeeds(FramePtr frame)
       {
         Vector3d xyz_world(it->ftr->frame->T_f_w_.inverse() * (it->ftr->f * (1.0/it->mu)));
         seed_converged_cb_(*it, xyz_world);
-        if(options_.verbose)
-          SVO_DEBUG_STREAM("seed converged");
       }
       it = seeds_.erase(it);
     }
@@ -299,8 +268,7 @@ void DepthFilter::updateSeeds(FramePtr frame)
   }
 }
 
-void DepthFilter::
-clearFrameQueue()
+void DepthFilter::clearFrameQueue()
 {
   while(!frame_queue_.empty())
     frame_queue_.pop();
diff --git a/svo/src/feature_detection.cpp b/svo/src/feature_detection.cpp
index be0ca02..98183f8 100644
--- a/svo/src/feature_detection.cpp
+++ b/svo/src/feature_detection.cpp
@@ -22,54 +22,95 @@
 namespace svo {
 namespace feature_detection {
 
-void FastDetector::detect(
-    const ImgPyr& img_pyr,
-    const Features& existing_fts,
+AbstractDetector::AbstractDetector(
+    const int img_width,
+    const int img_height,
     const int cell_size,
-    const int n_levels,
-    const double detection_threshold,
-    Corners* corners) const
-{
-  // first, find which parts of the image already have a matched feature
-  const int grid_n_cols = ceil(static_cast<double>(img_pyr[0].cols)/cell_size);
-  const int grid_n_rows = ceil(static_cast<double>(img_pyr[0].rows)/cell_size);
-  vector<bool> occupancy(grid_n_cols*grid_n_rows, false);
-  for(Features::const_iterator it=existing_fts.begin(); it!=existing_fts.end(); ++it)
-  {
-    if((*it)->point == false)
-      continue;
-    const int k = static_cast<int>((*it)->px[1]/cell_size)*grid_n_cols + static_cast<int>((*it)->px[0]/cell_size);
-    occupancy.at(k) = true;
-  }
+    const int n_pyr_levels) :
+        cell_size_(cell_size),
+        n_pyr_levels_(n_pyr_levels),
+        grid_n_cols_(ceil(static_cast<double>(img_width)/cell_size_)),
+        grid_n_rows_(ceil(static_cast<double>(img_height)/cell_size_)),
+        grid_occupancy_(grid_n_cols_*grid_n_rows_, false)
+{}
 
-  // now, detect features and perform nonmax suppression
-  corners->resize(grid_n_cols*grid_n_rows, Corner(0,0,detection_threshold,0,0.0f));
-  for(int L=0; L<n_levels; ++L)
+void AbstractDetector::resetGrid()
+{
+  std::fill(grid_occupancy_.begin(), grid_occupancy_.end(), false);
+}
+
+void AbstractDetector::setExistingFeatures(const Features& fts)
+{
+  std::for_each(fts.begin(), fts.end(), [&](Feature* i){
+    grid_occupancy_.at(
+        static_cast<int>(i->px[1]/cell_size_)*grid_n_cols_
+        + static_cast<int>(i->px[0]/cell_size_)) = true;
+  });
+}
+
+void AbstractDetector::setGridOccpuancy(const Vector2d& px)
+{
+  grid_occupancy_.at(
+      static_cast<int>(px[1]/cell_size_)*grid_n_cols_
+    + static_cast<int>(px[0]/cell_size_)) = true;
+}
+
+FastDetector::FastDetector(
+    const int img_width,
+    const int img_height,
+    const int cell_size,
+    const int n_pyr_levels) :
+        AbstractDetector(img_width, img_height, cell_size, n_pyr_levels)
+{}
+
+void FastDetector::detect(
+    Frame* frame,
+    const ImgPyr& img_pyr,
+    const double detection_threshold,
+    Features& fts)
+{
+  Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,detection_threshold,0,0.0f));
+  for(int L=0; L<n_pyr_levels_; ++L)
   {
     const int scale = (1<<L);
     vector<fast::fast_xy> fast_corners;
 #if __SSE2__
-      fast::fast_corner_detect_10_sse2((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
+      fast::fast_corner_detect_10_sse2(
+          (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
+          img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
 #elif HAVE_FAST_NEON
-      fast::fast_corner_detect_9_neon((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
+      fast::fast_corner_detect_9_neon(
+          (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
+          img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
 #else
-      fast::fast_corner_detect_10((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
+      fast::fast_corner_detect_10(
+          (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
+          img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
 #endif
     vector<int> scores, nm_corners;
     fast::fast_corner_score_10((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, fast_corners, 20, scores);
     fast::fast_nonmax_3x3(fast_corners, scores, nm_corners);
 
-    for(vector<int>::iterator it=nm_corners.begin(); it!=nm_corners.end(); ++it)
+    for(auto it=nm_corners.begin(), ite=nm_corners.end(); it!=ite; ++it)
     {
       fast::fast_xy& xy = fast_corners.at(*it);
-      const int k = static_cast<int>((xy.y*scale)/cell_size)*grid_n_cols + static_cast<int>((xy.x*scale)/cell_size);
-      if(occupancy[k])
+      const int k = static_cast<int>((xy.y*scale)/cell_size_)*grid_n_cols_
+                  + static_cast<int>((xy.x*scale)/cell_size_);
+      if(grid_occupancy_[k])
         continue;
       const float score = vk::shiTomasiScore(img_pyr[L], xy.x, xy.y);
-      if(score > corners->at(k).score)
-        corners->at(k) = Corner(xy.x*scale, xy.y*scale, score, L, 0.0f);
+      if(score > corners.at(k).score)
+        corners.at(k) = Corner(xy.x*scale, xy.y*scale, score, L, 0.0f);
     }
   }
+
+  // Create feature for every corner that has high enough corner score
+  std::for_each(corners.begin(), corners.end(), [&](Corner& c) {
+    if(c.score > detection_threshold)
+      fts.push_back(new Feature(frame, Vector2d(c.x, c.y), c.level));
+  });
+
+  resetGrid();
 }
 
 } // namespace feature_detection
diff --git a/svo/src/frame.cpp b/svo/src/frame.cpp
index e5f4501..6234d0b 100644
--- a/svo/src/frame.cpp
+++ b/svo/src/frame.cpp
@@ -137,13 +137,6 @@ void Frame::removeKeyPoint(Feature* ftr)
     setKeyPoints();
 }
 
-size_t Frame::nObs() const
-{
-  size_t n=0;
-  std::for_each(fts_.begin(), fts_.end(), [&](Feature* i){if(i->point != NULL) ++n; });
-  return n;
-}
-
 bool Frame::isVisible(const Vector3d& xyz_w) const
 {
   Vector3d xyz_f = T_f_w_*xyz_w;
diff --git a/svo/src/frame_handler_base.cpp b/svo/src/frame_handler_base.cpp
index 91bf5f0..f526cb3 100644
--- a/svo/src/frame_handler_base.cpp
+++ b/svo/src/frame_handler_base.cpp
@@ -120,14 +120,14 @@ int FrameHandlerBase::finishFrameProcessingCommon(
   num_obs_last_ = num_observations;
   SVO_STOP_TIMER("tot_time");
 
-  #ifdef SVO_TRACE
-    g_permon->writeToFile();
-    {
-      boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
-      size_t n_candidates = map_.point_candidates_.candidates_.size();
-      SVO_LOG(n_candidates);
-    }
-  #endif
+#ifdef SVO_TRACE
+  g_permon->writeToFile();
+  {
+    boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
+    size_t n_candidates = map_.point_candidates_.candidates_.size();
+    SVO_LOG(n_candidates);
+  }
+#endif
 
   // reset if failure or requested
   if(dropout == RESULT_FAILURE || set_reset_)
diff --git a/svo/src/frame_handler_mono.cpp b/svo/src/frame_handler_mono.cpp
index fbd8638..e588ce9 100644
--- a/svo/src/frame_handler_mono.cpp
+++ b/svo/src/frame_handler_mono.cpp
@@ -37,7 +37,8 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam) :
   depth_filter_(NULL)
 {
   // create depth filter and set callback
-  feature_detection::DetectorPtr feature_detector(new feature_detection::FastDetector());
+  feature_detection::DetectorPtr feature_detector(
+      new feature_detection::FastDetector(cam_->width(), cam_->height(), Config::gridSize(), Config::nPyrLevels()));
   DepthFilter::callback_t depth_filter_cb = boost::bind(&MapPointCandidates::newCandidatePoint, &map_.point_candidates_, _1, _2);
   depth_filter_ = new DepthFilter(feature_detector, depth_filter_cb);
   depth_filter_->startThread();
@@ -90,7 +91,7 @@ void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame)
 
 FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
 {
-  new_frame_->T_f_w_ = T_f_w_init_;
+  new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero());
   if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE)
     return RESULT_NO_KEYFRAME;
   new_frame_->setKeyframe();
@@ -183,6 +184,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
     depth_filter_->addFrame(new_frame_);
     return RESULT_NO_KEYFRAME;
   }
+  new_frame_->setKeyframe();
   SVO_DEBUG_STREAM("New keyframe selected.");
 
   // new keyframe selected
@@ -222,7 +224,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   }
 
   // add keyframe to map
-  new_frame_->setKeyframe();
   map_.addKeyframe(new_frame_);
 
   return RESULT_IS_KEYFRAME;
diff --git a/svo/src/initialization.cpp b/svo/src/initialization.cpp
index 2aa3931..16d1c80 100644
--- a/svo/src/initialization.cpp
+++ b/svo/src/initialization.cpp
@@ -109,25 +109,19 @@ void detectFeatures(
     vector<cv::Point2f>& px_vec,
     vector<Vector3d>& f_vec)
 {
-  feature_detection::Corners corners;
-  feature_detection::FastDetector detector;
-  detector.detect(frame->img_pyr_, frame->fts_, Config::gridSize(),
-                  Config::nPyrLevels(), Config::triangMinCornerScore(),
-                  &corners);
+  Features new_features;
+  feature_detection::FastDetector detector(
+      frame->img().cols, frame->img().rows, Config::gridSize(), Config::nPyrLevels());
+  detector.detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), new_features);
 
   // now for all maximum corners, initialize a new seed
-  px_vec.clear(); px_vec.reserve(corners.size());
-  f_vec.clear(); f_vec.reserve(corners.size());
-  for(feature_detection::Corners::iterator it=corners.begin(); it!=corners.end(); ++it)
-  {
-    // we must check again if the score is above the threshold because the Corners
-    // vector is initialized with dummy corners
-    if(it->score > Config::triangMinCornerScore())
-    {
-      px_vec.push_back(cv::Point2f(it->x, it->y));
-      f_vec.push_back(frame->c2f(it->x, it->y));
-    }
-  }
+  px_vec.clear(); px_vec.reserve(new_features.size());
+  f_vec.clear(); f_vec.reserve(new_features.size());
+  std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){
+    px_vec.push_back(cv::Point2f(ftr->px[0], ftr->px[1]));
+    f_vec.push_back(ftr->f);
+    delete ftr;
+  });
 }
 
 void trackKlt(
diff --git a/svo/src/matcher.cpp b/svo/src/matcher.cpp
index 9587608..550d25b 100644
--- a/svo/src/matcher.cpp
+++ b/svo/src/matcher.cpp
@@ -69,7 +69,9 @@ void getWarpMatrixAffine(
   A_cur_ref.col(1) = (px_dv - px_cur)/halfpatch_size;
 }
 
-int getBestSearchLevel(const Matrix2d& A_cur_ref, const int max_level)
+int getBestSearchLevel(
+    const Matrix2d& A_cur_ref,
+    const int max_level)
 {
   // Compute patch level in other image
   int level_cur = 0;
@@ -99,7 +101,7 @@ void warpAffine(
     return;
   }
 
-  // Perform the Warp on a larger patch
+  // Perform the warp on a larger patch.
   uint8_t* ref_patch_ptr = ref_patch.patch_with_border;
   const Vector2f px_ref_pyr = px_ref.cast<float>() / (1<<level_ref);
   for (int y=0; y<patch_size; ++y)
@@ -116,6 +118,7 @@ void warpAffine(
     }
   }
 
+  // Remove border from large patch.
   ref_patch_ptr = ref_patch.patch;
   for(int y=1; y<g_patch_size+1; ++y, ref_patch_ptr += g_patch_size)
   {
@@ -127,8 +130,7 @@ void warpAffine(
 
 } // namespace warp
 
-bool
-findMatchDirect(
+bool findMatchDirect(
     const Point& pt,
     const Frame& cur_frame,
     Vector2d& px_cur,
@@ -160,8 +162,7 @@ findMatchDirect(
   return success;
 }
 
-bool
-depthFromTriangulation(
+bool depthFromTriangulation(
     const SE3& T_search_ref,
     const Vector3d& f_ref,
     const Vector3d& f_cur,
@@ -176,8 +177,7 @@ depthFromTriangulation(
   return true;
 }
 
-bool
-findEpipolarMatchDirect(
+bool findEpipolarMatchDirect(
     const Frame& ref_frame,
     const Frame& cur_frame,
     const Feature& ref_ftr,
diff --git a/svo/src/point.cpp b/svo/src/point.cpp
index 75f14a0..bdb14c5 100644
--- a/svo/src/point.cpp
+++ b/svo/src/point.cpp
@@ -24,7 +24,7 @@ namespace svo {
 
 int Point::point_counter_ = 0;
 
-Point::Point(Vector3d pos) :
+Point::Point(const Vector3d& pos) :
   id_(point_counter_++),
   pos_(pos),
   normal_(Vector3d::Zero()),
@@ -38,6 +38,22 @@ Point::Point(Vector3d pos) :
   last_structure_optim_(0)
 {}
 
+Point::Point(const Vector3d& pos, Feature* ftr) :
+  id_(point_counter_++),
+  pos_(pos),
+  normal_(Vector3d::Zero()),
+  n_obs_(1),
+  v_pt_(NULL),
+  last_published_ts_(0),
+  last_projected_kf_id_(-1),
+  type_(TYPE_UNKNOWN),
+  n_failed_reproj_(0),
+  n_succeeded_reproj_(0),
+  last_structure_optim_(0)
+{
+  obs_.push_front(ftr);
+}
+
 Point::~Point()
 {}
 
diff --git a/svo/src/sparse_img_align.cpp b/svo/src/sparse_img_align.cpp
index 6dc97c8..fc765cb 100644
--- a/svo/src/sparse_img_align.cpp
+++ b/svo/src/sparse_img_align.cpp
@@ -69,9 +69,19 @@ size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
     optimize(T_cur_from_ref);
   }
   cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_->T_f_w_;
+
+
+
   return n_meas_/patch_area_;
 }
 
+Matrix<double, 6, 6> SparseImgAlign::getFisherInformation()
+{
+  double sigma_i_sq = 5e-4*255*255; // image noise
+  Matrix<double,6,6> I = H_/sigma_i_sq;
+  return I;
+}
+
 // TODO take jacobian from Frame Class!
 void frameJac(const Vector3d & xyz, Matrix<double,2,6> & J)
 {
diff --git a/svo/test/test_depth_filter.cpp b/svo/test/test_depth_filter.cpp
index a53947d..6072218 100644
--- a/svo/test/test_depth_filter.cpp
+++ b/svo/test/test_depth_filter.cpp
@@ -90,6 +90,7 @@ void DepthFilterTest::depthFilterCb(
 
 void DepthFilterTest::testReconstruction(
     std::string dataset_dir,
+
     std::string experiment_name)
 {
   vk::FileReader<vk::blender_utils::file_format::ImageNameAndPose> sequence_file_reader(dataset_dir+"/trajectory.txt");
@@ -103,7 +104,9 @@ void DepthFilterTest::testReconstruction(
   vk::Timer t;
   std::list<size_t> n_converged_per_iteration;
 
-  svo::feature_detection::DetectorPtr feature_detector(new svo::feature_detection::FastDetector());
+  svo::feature_detection::DetectorPtr feature_detector(
+      new svo::feature_detection::FastDetector(
+          cam_->width(), cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels()));
   svo::DepthFilter::callback_t depth_filter_cb = boost::bind(&DepthFilterTest::depthFilterCb, this, _1, _2);
   depth_filter_ = new svo::DepthFilter(feature_detector, depth_filter_cb);
 
diff --git a/svo/test/test_feature_detection.cpp b/svo/test/test_feature_detection.cpp
index 6fa3723..b91e97a 100644
--- a/svo/test/test_feature_detection.cpp
+++ b/svo/test/test_feature_detection.cpp
@@ -17,8 +17,10 @@
 #include <string.h>
 #include <svo/global.h>
 #include <svo/config.h>
+#include <svo/frame.h>
 #include <svo/feature_detection.h>
 #include <svo/depth_filter.h>
+#include <svo/feature.h>
 #include <vikit/timer.h>
 #include <vikit/vision.h>
 #include <vikit/abstract_camera.h>
@@ -37,38 +39,29 @@ void testCornerDetector()
   cv::Mat img(cv::imread(img_name, 0));
   assert(img.type() == CV_8UC1 && !img.empty());
 
-  svo::ImgPyr img_pyr;
-  svo::frame_utils::createImgPyramid(
-      img, max(svo::Config::nPyrLevels(), svo::Config::kltMaxLevel()+1), img_pyr);
-  svo::Features fts;
+  vk::AbstractCamera* cam = new vk::ATANCamera(752, 480, 0.511496, 0.802603, 0.530199, 0.496011, 0.934092);
+  svo::FramePtr frame(new svo::Frame(cam, img, 0.0));
 
   // Corner detection
   vk::Timer t;
-  svo::feature_detection::Corners corners;
-  svo::feature_detection::FastDetector fast_detector;
+  svo::Features fts;
+  svo::feature_detection::FastDetector fast_detector(
+      img.cols, img.rows, svo::Config::gridSize(), svo::Config::nPyrLevels());
   for(int i=0; i<100; ++i)
   {
-    fast_detector.detect(img_pyr, fts, svo::Config::gridSize(), svo::Config::nPyrLevels(),
-                         svo::Config::triangMinCornerScore(), &corners);
+    fast_detector.detect(frame.get(), frame->img_pyr_, svo::Config::triangMinCornerScore(), fts);
   }
-  printf("Fast corner detection took %f ms, %zu corners detected (ref i7-W520: 8.06878ms, 416)\n", t.stop()*10, corners.size());
+  printf("Fast corner detection took %f ms, %zu corners detected (ref i7-W520: 8.06878ms, 416)\n", t.stop()*10, fts.size());
+  printf("Note, in this case, feature detection also contains the cam2world projection of the feature.\n");
+  cv::Mat img_rgb = cv::Mat(img.size(), CV_8UC3);
+  cv::cvtColor(img, img_rgb, CV_GRAY2RGB);
+  std::for_each(fts.begin(), fts.end(), [&](svo::Feature* i){
+    cv::circle(img_rgb, cv::Point2f(i->px[0], i->px[1]), 4*(i->level+1), cv::Scalar(0,255,0), 1);
+  });
+  cv::imshow("ref_img", img_rgb);
+  cv::waitKey(0);
 
-  if(false)
-  {
-    cv::Mat img_rgb = cv::Mat(img.size(), CV_8UC3);
-    cv::cvtColor(img, img_rgb, CV_GRAY2RGB);
-    for(svo::feature_detection::Corners::iterator it=corners.begin(); it!=corners.end(); ++it)
-    {
-      // we must check again if the score is above the threshold because the Corners
-      // vector is initialized with dummy corners
-      if(it->score > svo::Config::triangMinCornerScore())
-      {
-        cv::circle(img_rgb, cv::Point2f(it->x, it->y), 4*(it->level+1), cv::Scalar(0,255,0), 1);
-      }
-    }
-    cv::imshow("ref_img", img_rgb);
-    cv::waitKey(0);
-  }
+  std::for_each(fts.begin(), fts.end(), [&](svo::Feature* i){ delete i; });
 }
 
 } // namespace
diff --git a/svo/test/test_pose_optimizer.cpp b/svo/test/test_pose_optimizer.cpp
index 8e98eac..7dd79f8 100644
--- a/svo/test/test_pose_optimizer.cpp
+++ b/svo/test/test_pose_optimizer.cpp
@@ -58,25 +58,15 @@ class PoseOptimizerTest {
     vk::blender_utils::loadBlenderDepthmap(dataset_dir + "/depth/frame_000001_0.depth", *cam_, depthmap_);
 
     // detect features
-    feature_detection::Corners corners;
-    feature_detection::FastDetector detector;
-    detector.detect(frame_->img_pyr_, frame_->fts_, Config::gridSize(),
-                    Config::nPyrLevels(), Config::triangMinCornerScore(), &corners);
+    feature_detection::FastDetector detector(
+        cam_->width(), cam_->height(), Config::gridSize(), Config::nPyrLevels());
+    detector.detect(frame_.get(), frame_->img_pyr_, Config::triangMinCornerScore(), frame_->fts_);
     size_t n_fts = 0;
-    for(feature_detection::Corners::iterator it=corners.begin(); it!=corners.end(); ++it)
-    {
-      // we must check again if the score is above the threshold because the Corners
-      // vector is initialized with dummy corners
-      if(it->score > Config::triangMinCornerScore())
-      {
-        Feature* ftr(new Feature(frame_.get(), Vector2d(it->x, it->y), it->level));
-        Point* point(new Point(frame_->f2w(ftr->f*depthmap_.at<float>(it->y, it->x))));
-        ftr->point = point;
-        ftr->frame = frame_.get();
-        frame_->addFeature(ftr);
-        ++n_fts;
-      }
-    }
+    std::for_each(frame_->fts_.begin(), frame_->fts_.end(), [&](Feature* i){
+      Point* point(new Point(frame_->f2w(i->f*depthmap_.at<float>(i->px[1], i->px[0])), i));
+      i->point = point;
+      ++n_fts;
+    });
     printf("Added %zu features to frame.\n", n_fts);
   }
 
diff --git a/svo/test/test_sparse_img_align.cpp b/svo/test/test_sparse_img_align.cpp
index f719f72..fe1ece5 100644
--- a/svo/test/test_sparse_img_align.cpp
+++ b/svo/test/test_sparse_img_align.cpp
@@ -97,21 +97,16 @@ void SparseImgAlignTest::testSequence(
           dataset_dir+"/depth/"+iter->image_name_+"_0.depth", *cam_, depthmap);
 
       // extract features, generate features with 3D points
-      svo::feature_detection::Corners corners;
       feature_detector->detect(
-          frame_ref_->img_pyr_, frame_ref_->fts_, svo::Config::gridSize(),
-          svo::Config::nPyrLevels(), svo::Config::triangMinCornerScore(), &corners);
-      for(svo::feature_detection::Corners::iterator corner=corners.begin(); corner!=corners.end(); ++corner)
-      {
-        svo::Feature* ftr = new svo::Feature(frame_ref_.get(), Eigen::Vector2d(corner->x, corner->y), corner->level);
-        Eigen::Vector3d pt_pos_cur = ftr->f*depthmap.at<float>(corner->y, corner->x);
+          frame_ref_.get(), frame_ref_->img_pyr_, svo::Config::triangMinCornerScore(), frame_ref_->fts_);
+      std::for_each(frame_ref_->fts_.begin(), frame_ref_->fts_.end(), [&](svo::Feature* i) {
+        Eigen::Vector3d pt_pos_cur = i->f*depthmap.at<float>(i->px[1], i->px[0]);
         Eigen::Vector3d pt_pos_w = frame_ref_->T_f_w_.inverse()*pt_pos_cur;
-        svo::Point* pt = new svo::Point(pt_pos_w);
-        ftr->point = pt;
-        frame_ref_->addFeature(ftr);
-      }
+        svo::Point* pt = new svo::Point(pt_pos_w, i);
+        i->point = pt;
+      });
 
-      printf("Added %zu 3d pts to the reference frame.\n", corners.size());
+      printf("Added %zu 3d pts to the reference frame.\n", frame_ref_->nObs());
       T_prev_w = frame_ref_->T_f_w_;
       T_prevgt_w = T_gt_w;
       continue;
@@ -158,10 +153,11 @@ int main(int argc, char** argv)
 {
   std::string experiment_name("flying_room_1_rig_1_fast_minlev0");
   std::string dataset_dir(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
-  svo::feature_detection::FastDetector detector;
   svo::Config::triangMinCornerScore() = 20;
   svo::Config::kltMinLevel() = 0;
   SparseImgAlignTest test;
+  svo::feature_detection::FastDetector detector(
+      test.cam_->width(), test.cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels());
   test.testSequence(dataset_dir, experiment_name, &detector);
   return 0;
 }
diff --git a/svo_analysis/README.md b/svo_analysis/README.md
index 0a4deeb..07ae6eb 100644
--- a/svo_analysis/README.md
+++ b/svo_analysis/README.md
@@ -25,3 +25,27 @@ Afterwards run
     rosrun svo_analysis benchmark.py flying_room_1_fast
 
 The script will create a new folder in `svo_analysis/results/`, run svo on the dataset and save the tracefile in this folder and at the end it will run some scripts to generate plots from the tracefile.
+
+
+#### Dataset file
+
+A dataset with motion-capture-system ground-truth should contain the following files
+
+* _images.txt_
+  
+     # timestamp img/imagename
+     1329330952.725747585296631 img/00000.png
+     1329330952.765715122222900 img/00001.png
+     1329330952.805604219436646 img/00002.png
+     ...
+
+* _groundtruth.txt_: groundtruth poses as acquired from a motion capture system
+
+     # timestamp tx ty tz qx qy qz qw
+     
+* _groundtruth_matched.txt_: the groundtruth pose associated to the images in images.txt. This file can be generated by the command:
+
+    cd my_dataset
+    rosrun svo_analysis associate.py images.txt groundtruth.txt
+
+  
diff --git a/svo_analysis/experiments/synthetic.yaml b/svo_analysis/experiments/synthetic.yaml
index 5eb0402..e58ff7a 100644
--- a/svo_analysis/experiments/synthetic.yaml
+++ b/svo_analysis/experiments/synthetic.yaml
@@ -1,7 +1,10 @@
 experiment_label: 'svo fast'
 param_settings: vo_fast
 datasets:
+ - 1404102247_fr1_straight
+ - 1404102220_fr1_straight
+ - 1404102129_fr1_straight
  - 1404091756_fr1_straight
  - 1404091807_fr1_straight_rot
  - 1404091819_fr1_sine_2d
- - 1404091831_fr1_sine_3d
\ No newline at end of file
+ - 1404091831_fr1_sine_3d
diff --git a/svo_analysis/scripts/benchmark.py b/svo_analysis/scripts/benchmark.py
index a544ba2..c66f115 100755
--- a/svo_analysis/scripts/benchmark.py
+++ b/svo_analysis/scripts/benchmark.py
@@ -29,18 +29,28 @@ def run_experiment(dataset, params):
     algo_params = yaml.load(open(algo_params_file,'r'))
     
     # combine all parameters
-    if dataset_params['rig_size'] == 1:
+    if 'rig_size' not in dataset_params or dataset_params['rig_size'] == 1:
         params = dict(params.items() + algo_params.items() + dataset_params['cam0'].items())
     else:
         params = dict(params.items() + algo_params.items() + dataset_params.items())
     if 'dataset_is_blender' in dataset_params:
-        params['dataset_is_blender'] = True
+        params['dataset_is_blender'] = dataset_params['dataset_is_blender']
+    params['dataset_first_frame'] = dataset_params['dataset_first_frame']
     
     # dump experiment params to file and copy the other parameter files:
     params_dump_file = os.path.join(params['trace_dir'],'params.yaml')
     with open(params_dump_file,'w') as outfile:
         outfile.write(yaml.dump(params, default_flow_style=False))
-    shutil.copyfile(dataset_params_file, os.path.join(params['trace_dir'], 'dataset_params.yaml'))
+    shutil.copyfile(dataset_params_file,
+                    os.path.join(params['trace_dir'], 'dataset_params.yaml'))
+    
+    # copy the groundtruth trajectory to the trace dir for later evaluation
+    if params['dataset_is_blender']:
+        shutil.copyfile(os.path.join(params['dataset_directory'], 'trajectory.txt'),
+                        os.path.join(params['trace_dir'], 'groundtruth_matched.txt'))
+    else:
+        shutil.copyfile(os.path.join(params['dataset_directory'], 'groundtruth_matched.txt'),
+                        os.path.join(params['trace_dir'], 'groundtruth_matched.txt'))
     
     # execute ros node
     node = ros_node.RosNode(args.version, args.executable)
diff --git a/svo_analysis/src/svo_analysis/analyse_trajectory.py b/svo_analysis/src/svo_analysis/analyse_trajectory.py
old mode 100644
new mode 100755
index f2a800e..a024b79
--- a/svo_analysis/src/svo_analysis/analyse_trajectory.py
+++ b/svo_analysis/src/svo_analysis/analyse_trajectory.py
@@ -1,179 +1,180 @@
-# -*- coding: utf-8 -*-
+#!/usr/bin/python
 
 import os
 import yaml
-from .tum_benchmark_tools import associate
+import argparse
+import numpy as np
 import matplotlib.pyplot as plt
+import svo_analysis.tum_benchmark_tools.associate as associate
 import vikit_py.transformations as transformations
 import vikit_py.align_trajectory as align_trajectory
-import numpy as np
-
-# plot options
 from matplotlib import rc
 rc('font',**{'family':'serif','serif':['Cardo']})
 rc('text', usetex=True)
 
 def plot_translation_error(timestamps, translation_error, results_dir):
-  fig = plt.figure(figsize=(8, 2.5))
-  ax = fig.add_subplot(111, xlabel='time [s]', ylabel='position drift [mm]', xlim=[0,timestamps[-1]-timestamps[0]+4])
-  ax.plot(timestamps-timestamps[0], translation_error[:,0]*1000, 'r-', label='x')
-  ax.plot(timestamps-timestamps[0], translation_error[:,1]*1000, 'g-', label='y')
-  ax.plot(timestamps-timestamps[0], translation_error[:,2]*1000, 'b-', label='z')
-  ax.legend()
-  fig.tight_layout()
-  fig.savefig(results_dir+'/translation_error.pdf')
+    fig = plt.figure(figsize=(8, 2.5))
+    ax = fig.add_subplot(111, xlabel='time [s]', ylabel='position drift [mm]', xlim=[0,timestamps[-1]-timestamps[0]+4])
+    ax.plot(timestamps-timestamps[0], translation_error[:,0]*1000, 'r-', label='x')
+    ax.plot(timestamps-timestamps[0], translation_error[:,1]*1000, 'g-', label='y')
+    ax.plot(timestamps-timestamps[0], translation_error[:,2]*1000, 'b-', label='z')
+    ax.legend()
+    fig.tight_layout()
+    fig.savefig(results_dir+'/translation_error.pdf')
 
 def plot_rotation_error(timestamps, rotation_error, results_dir):
-  fig = plt.figure(figsize=(8, 2.5))
-  ax = fig.add_subplot(111, xlabel='time [s]', ylabel='orientation drift [rad]', xlim=[0,timestamps[-1]-timestamps[0]+4])
-  ax.plot(timestamps-timestamps[0], rotation_error[:,0], 'r-', label='yaw')
-  ax.plot(timestamps-timestamps[0], rotation_error[:,1], 'g-', label='pitch')
-  ax.plot(timestamps-timestamps[0], rotation_error[:,2], 'b-', label='roll')
-  ax.legend()
-  fig.tight_layout()
-  fig.savefig(results_dir+'/orientation_error.pdf')
+    fig = plt.figure(figsize=(8, 2.5))
+    ax = fig.add_subplot(111, xlabel='time [s]', ylabel='orientation drift [rad]', xlim=[0,timestamps[-1]-timestamps[0]+4])
+    ax.plot(timestamps-timestamps[0], rotation_error[:,0], 'r-', label='yaw')
+    ax.plot(timestamps-timestamps[0], rotation_error[:,1], 'g-', label='pitch')
+    ax.plot(timestamps-timestamps[0], rotation_error[:,2], 'b-', label='roll')
+    ax.legend()
+    fig.tight_layout()
+    fig.savefig(results_dir+'/orientation_error.pdf')
 
-def analyse_trajectory(results_dir):
-  # plot translation error
-  data = np.loadtxt(os.path.join(results_dir, 'translation_error.txt'))
-  timestamps = data[:,0]
-  translation_error = data[:,1:4]
-  plot_translation_error(timestamps, translation_error, results_dir)
+def analyse_trajectory(results_dir, n_align_frames = 20, use_hand_eye_calib = True):
   
-  # plot orientation error
-  data = np.loadtxt(os.path.join(results_dir, 'orientation_error.txt'))
-  timestamps = data[:,0]
-  orientation_error = data[:,1:4]
-  plot_rotation_error(timestamps, orientation_error, results_dir)
+    if not os.path.isfile(os.path.join(results_dir, 'translation_error.txt')):
+                
+        T_cm, T_mc = load_hand_eye_calib(results_dir)
+        print('loading datasets.')        
+        t_es, p_es, q_es, t_gt, p_gt, q_gt = load_dataset(results_dir)
+        
+        # align Sim3 to get scale
+        print('align Sim3 using '+str(n_align_frames)+' first frames.')
+        scale,rot,trans = align_trajectory.align_sim3(p_gt[0:n_align_frames,:], p_es[0:n_align_frames,:])
+        print 'scale = '+str(scale)
   
+        # get trafo between (v)ision and (o)ptitrack frame
+        p_es_aligned = np.zeros(np.shape(p_es))
+        if use_hand_eye_calib:
+            print q_gt[0,:]
+            print p_gt[0,:]
+            T_om = get_rigid_body_trafo(q_gt[0,:], p_gt[0,:])
+            T_vc = get_rigid_body_trafo(q_es[0,:], scale*p_es[0,:])
+            T_cv = transformations.inverse_matrix(T_vc)
+            T_ov = np.dot(T_om, np.dot(T_mc, T_cv))
+            print 'T_ov = ' + str(T_ov)
+      
+            # apply transformation to estimated trajectory
+            q_es_aligned = np.zeros(np.shape(q_es))
+            rpy_es_aligned = np.zeros(np.shape(p_es))
+            rpy_gt = np.zeros(np.shape(p_es))       
+            for i in range(np.shape(p_es)[0]):
+                T_vc = get_rigid_body_trafo(q_es[i,:],p_es[i,:])
+                T_vc[0:3,3] *= scale
+                T_om = np.dot(T_ov, np.dot(T_vc, T_cm))
+                p_es_aligned[i,:] = T_om[0:3,3]
+                q_es_aligned[i,:] = transformations.quaternion_from_matrix(T_om)
+                rpy_es_aligned[i,:] = transformations.euler_from_quaternion(q_es_aligned[i,:], 'rzyx')
+                rpy_gt[i,:] = transformations.euler_from_quaternion(q_gt[i,:], 'rzyx') 
+  
+            # plot position error (drift)
+            translation_error = (p_gt-p_es_aligned)
+            plot_translation_error(t_es, translation_error, results_dir)
+      
+            # plot orientation error (drift)
+            orientation_error = (rpy_gt - rpy_es_aligned)
+            plot_rotation_error(t_es, orientation_error, results_dir)
+    
+            # plot scale drift
+            motion_gt = np.diff(p_gt, 0)
+            motion_es = np.diff(p_es_aligned, 0)
+            dist_gt = np.sqrt(np.sum(np.multiply(motion_gt,motion_gt),1))
+            dist_es = np.sqrt(np.sum(np.multiply(motion_es,motion_es),1))
+            fig = plt.figure(figsize=(8,2.5))
+            ax = fig.add_subplot(111, xlabel='time [s]', ylabel='scale change [\%]', xlim=[0,t_es[-1]+4])
+            scale_drift = np.divide(dist_es,dist_gt)*100-100
+            ax.plot(t_es, scale_drift, 'b-')
+            fig.tight_layout()
+        
+        else:
+            for i in range(np.shape(p_es)[0]):
+                p_es_aligned[i,:] = scale*rot.dot(p_es[i,:]) + trans
+      
+        # plot trajectory
+        fig = plt.figure()
+        ax = fig.add_subplot(111, title='trajectory', aspect='equal', xlabel='x [m]', ylabel='y [m]')
+        ax.plot(p_es_aligned[:,0], p_es_aligned[:,1], 'b-', label='estimate')
+        ax.plot(p_gt[:,0], p_gt[:,1], 'r-', label='groundtruth')
+        #ax.plot(p_es_aligned[0:n_align_frames,0], p_es_aligned[0:n_align_frames,1], 'g-', linewidth=2, label='aligned')
+        ax.legend()
+        fig.tight_layout()
+        fig.savefig(results_dir+'/trajectory.pdf')
+      
+
+
+    else:
+        data = np.loadtxt(os.path.join(results_dir, 'translation_error.txt'))
+        timestamps = data[:,0]
+        translation_error = data[:,1:4]
+        plot_translation_error(timestamps, translation_error, results_dir)
+          
+        # plot orientation error
+        data = np.loadtxt(os.path.join(results_dir, 'orientation_error.txt'))
+        timestamps = data[:,0]
+        orientation_error = data[:,1:4]
+        plot_rotation_error(timestamps, orientation_error, results_dir)
+  
+
 def get_rigid_body_trafo(quat,trans):
-  T = transformations.quaternion_matrix(quat)
-  T[0:3,3] = trans
-  return T
+    T = transformations.quaternion_matrix(quat)
+    T[0:3,3] = trans
+    return T
+    
+def load_hand_eye_calib(results_dir):
+    P = yaml.load(open(os.path.join(results_dir, 'dataset_params.yaml'),'r'))['hand_eye_calib']
+    
+    T_cm_quat = np.array([P['Tcm_qx'], P['Tcm_qy'], P['Tcm_qz'], P['Tcm_qw']])
+    T_cm_tran = np.array([P['Tcm_tx'], P['Tcm_ty'], P['Tcm_tz']])
+    T_cm = get_rigid_body_trafo(T_cm_quat, T_cm_tran)
+    T_mc = transformations.inverse_matrix(T_cm)
+    return T_cm, T_mc
+  
+def load_dataset(results_dir):
+    """
+    Assumes that groundtruth measurements are already matched with the
+    images. 
+    """
 
+    data_gt = open(os.path.join(results_dir, 'groundtruth_matched.txt')).read()
+    lines = data_gt.replace(","," ").replace("\t"," ").split("\n") 
+    data_gt = np.array([[np.float(v.strip()) for i,v in enumerate(line.split(" ")) if i != 1 and v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"])
+    data_gt = [(float(l[0]),l[1:]) for l in data_gt]
+    data_gt = dict(data_gt)    
+    
+    data_es = open(os.path.join(results_dir, 'traj_estimate.txt')).read()
+    lines = data_es.replace(","," ").replace("\t"," ").split("\n") 
+    data_es = np.array([[np.float(v.strip()) for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"])
+    data_es = [(float(l[0]),l[1:]) for l in data_es]
+    data_es = dict(data_es)
+    
+    matches = associate.associate(data_gt, data_es, 0.0, 0.0001)
+    p_gt = np.array([[np.float(value) for value in data_gt[a][0:3]] for a,b in matches])
+    q_gt = np.array([[np.float(value) for value in data_gt[a][3:7]] for a,b in matches])
+    p_es = np.array([[np.float(value) for value in data_es[b][0:3]] for a,b in matches])
+    q_es = np.array([[np.float(value) for value in data_es[b][3:7]] for a,b in matches])
+    t_gt = np.array([np.float(a) for a,b in matches])
+    t_es = np.array([np.float(b) for a,b in matches])
+  
+    # set start time to zero
+    start_time = min(t_es[0], t_gt[0])
+    t_es -= start_time
+    t_gt -= start_time
+    return t_es, p_es, q_es, t_gt, p_gt, q_gt
+  
 if __name__ == '__main__':
   
-  # config
-  dataset='20131003_1937_nslam_i7_asl2_fast'
-  n_align_frames = 100
-  save = True
-  results_dir = '../results/'+dataset
-  
-  # load dataset parameters
-  params_stream = open(results_dir+'/params.yaml')
-  params = yaml.load(params_stream)
-  
-  # set trajectory groundtruth and estimate file
-  traj_groundtruth = params['dataset_directory']+'/groundtruth_filtered.txt'
-  traj_estimate = results_dir+'/traj_estimate.txt'
-  traj_estimate_rotated_filename = results_dir+'/traj_estimate_rotated.txt'
-  translation_error_filename = results_dir+'/translation_error.txt'
-  orientation_error_filename = results_dir+'/orientation_error.txt'
-  scale_drift_filename = results_dir+'/scale_drift.txt'
-  
-  # load hand-eye calib: trafo between (m)arker and (c)amera.
-  dataset_param_file = params['dataset_directory']+'/dataset_params.yaml'
-  dataset_param_file_stream = open(dataset_param_file,'r')
-  P = yaml.load(dataset_param_file_stream)
-  T_cm_quat = np.array([P['calib_Tcm_qx'], P['calib_Tcm_qy'], P['calib_Tcm_qz'], P['calib_Tcm_qw']])
-  T_cm_tran = np.array([P['calib_Tcm_tx'], P['calib_Tcm_ty'], P['calib_Tcm_tz']])
-  T_cm = get_rigid_body_trafo(T_cm_quat, T_cm_tran)
-  T_mc = transformations.inverse_matrix(T_cm)
-  
-  # load data
-  data_gt = associate.read_file_list(traj_groundtruth)
-  data_es = associate.read_file_list(traj_estimate)
-  
-  # find matches
-  matches = associate.associate(data_gt, data_es, -P['cam_delay'], 0.02)
-  p_gt = np.array([[float(value) for value in data_gt[a][0:3]] for a,b in matches])
-  q_gt = np.array([[float(value) for value in data_gt[a][3:7]] for a,b in matches])
-  p_es = np.array([[float(value) for value in data_es[b][0:3]] for a,b in matches])
-  q_es = np.array([[float(value) for value in data_es[b][3:7]] for a,b in matches])
-  t_gt = np.array([float(a) for a,b in matches])
-  t_es = np.array([float(b) for a,b in matches])
-  start_time = min(t_es[0], t_gt[0])
-  t_es -= start_time
-  t_gt -= start_time
-  
-  # align Sim3 to get scale
-  if(n_align_frames > np.shape(matches)[0]):
-    raise NameError('n_matches_for_scale is too large')
-  scale,rot,trans = align_trajectory.align_sim3(p_gt, p_es, n_align_frames)
-  print 'scale = '+str(scale)
-  
-  # get trafo between (v)ision and (o)ptitrack frame
-  T_om = get_rigid_body_trafo(q_gt[0,:], p_gt[0,:])
-  T_vc = get_rigid_body_trafo(q_es[0,:], scale*p_es[0,:])
-  T_cv = transformations.inverse_matrix(T_vc)
-  T_ov = np.dot(T_om, np.dot(T_mc, T_cv))
-  print 'T_ov = ' + str(T_ov)
-  
-  p_es_aligned = np.zeros(np.shape(p_es))
-  q_es_aligned = np.zeros(np.shape(q_es))
-  rpy_es_aligned = np.zeros(np.shape(p_es))
-  rpy_gt = np.zeros(np.shape(p_es))
-  
-  # rotate trajectory
-  for i in range(np.shape(matches)[0]):
-    T_vc = get_rigid_body_trafo(q_es[i,:],p_es[i,:])
-    T_vc[0:3,3] *= scale
-    T_om = np.dot(T_ov, np.dot(T_vc, T_cm))
-    p_es_aligned[i,:] = T_om[0:3,3]
-    q_es_aligned[i,:] = transformations.quaternion_from_matrix(T_om)
-    rpy_es_aligned[i,:] = transformations.euler_from_quaternion(q_es_aligned[i,:], 'rzyx')
-    rpy_gt[i,:] = transformations.euler_from_quaternion(q_gt[i,:], 'rzyx')
-  
-  if save:
-    f = open(traj_estimate_rotated_filename,'w')
-    for i in range(np.shape(p_es_aligned)[0]):
-      f.write('%.7f %.5f %.5f %.5f %.5f %.5f %.5f %.5f\n' % (matches[i][1]-P['cam_delay'], p_es_aligned[i,0], p_es_aligned[i,1], p_es_aligned[i,2], q_es_aligned[i,0], q_es_aligned[i,1], q_es_aligned[i,2], q_es_aligned[i,3]))
-    f.close()
-    
-  # plot trajectory
-  fig = plt.figure()
-  ax = fig.add_subplot(111, title='trajectory', aspect='equal', xlabel='x [m]', ylabel='Y [m]')
-  ax.plot(p_es_aligned[:,0], p_es_aligned[:,1], 'b-', label='estimate')
-  ax.plot(p_gt[:,0], p_gt[:,1], 'r-', label='groundtruth')
-  ax.plot(p_es_aligned[0:n_align_frames,0], p_es_aligned[0:n_align_frames,1], 'g-', linewidth=2, label='aligned')
-  fig.tight_layout()
-  fig.savefig(results_dir+'/trajectory.pdf')
-          
-  # plot position error (drift)
-  translation_error = (p_gt-p_es_aligned)
-  plot_translation_error(t_es, translation_error, results_dir)
-  
-  if save:
-    f = open(translation_error_filename,'w')
-    for i in range(translation_error.shape[0]):
-      f.write('%.7f %.5f %.5f %.5f\n'%(t_es[i], translation_error[i,0],  translation_error[i,1],  translation_error[i,2]))
-    f.close()
-    
-  # plot orientation error (drift)
-  orientation_error = (rpy_gt - rpy_es_aligned)
-  plot_rotation_error(t_es, orientation_error, results_dir)
-    
-  if save:  
-    f = open(orientation_error_filename,'w')
-    for i in range(orientation_error.shape[0]):
-      f.write('%.7f %.5f %.5f %.5f\n'%(t_es[i], orientation_error[i,0],  orientation_error[i,1],  orientation_error[i,2]))
-    f.close()
-    
-  # plot scale drift
-  motion_gt = np.diff(p_gt, 0)
-  motion_es = np.diff(p_es_aligned, 0)
-  dist_gt = np.sqrt(np.sum(np.multiply(motion_gt,motion_gt),1))
-  dist_es = np.sqrt(np.sum(np.multiply(motion_es,motion_es),1))
-  fig = plt.figure(figsize=(8,2.5))
-  ax = fig.add_subplot(111, xlabel='time [s]', ylabel='scale change [\%]', xlim=[0,t_es[-1]+4])
-  scale_drift = np.divide(dist_es,dist_gt)*100-100
-  ax.plot(t_es, scale_drift, 'b-')
-  fig.tight_layout()
-  
-  if save:
-    fig.savefig(results_dir+'/scale_drift.pdf')
-    f = open(scale_drift_filename,'w')
-    for i in range(orientation_error.shape[0]):
-      f.write('%.7f %.5f\n'%(t_es[i], scale_drift[i]))
-    f.close()
+    # parse command line
+    parser = argparse.ArgumentParser(description='''
+    Analyse trajectory
+    ''')
+    parser.add_argument('results_dir', help='folder with the results')
+    parser.add_argument('--use_hand_eye_calib', help='', action='store_true')
+    parser.add_argument('--n_align_frames', help='', default=20)
+    args = parser.parse_args()
     
+    print('analyse trajectory for dataset: '+str(args.results_dir))
+    analyse_trajectory(args.results_dir,
+                       n_align_frames = int(args.n_align_frames),
+                       use_hand_eye_calib = args.use_hand_eye_calib)
\ No newline at end of file
diff --git a/svo_analysis/src/svo_analysis/tum_benchmark_tools/associate.py b/svo_analysis/src/svo_analysis/tum_benchmark_tools/associate.py
index 554314a..01bea59 100755
--- a/svo_analysis/src/svo_analysis/tum_benchmark_tools/associate.py
+++ b/svo_analysis/src/svo_analysis/tum_benchmark_tools/associate.py
@@ -108,7 +108,6 @@ if __name__ == '__main__':
     ''')
     parser.add_argument('first_file', help='first text file (format: timestamp data)')
     parser.add_argument('second_file', help='second text file (format: timestamp data)')
-    parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true')
     parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
     parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
     args = parser.parse_args()
@@ -117,12 +116,15 @@ if __name__ == '__main__':
     second_list = read_file_list(args.second_file)
 
     matches = associate(first_list, second_list,float(args.offset),float(args.max_difference))    
-
-    if args.first_only:
-        for a,b in matches:
-            print("%f %s"%(a," ".join(first_list[a])))
-    else:
-        for a,b in matches:
-            print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b])))
-            
+    
+    filename = 'groundtruth_matched.txt' 
+    associates_file = open(filename, 'w')                             
+    for a,b in matches:
+      img = first_list[a]
+      pos = second_list[b]
+      associates_file.write('%.6f %s %s %s %s %s %s %s %s\n' %
+                           (a, img[0],
+                            pos[0], pos[1], pos[2],
+                            pos[3], pos[4], pos[5], pos[6])) 
+    print('Wrote matches to file: ' + filename)
         
diff --git a/svo_ros/include/svo_ros/visualizer.h b/svo_ros/include/svo_ros/visualizer.h
index 1a7df1f..aa6146a 100644
--- a/svo_ros/include/svo_ros/visualizer.h
+++ b/svo_ros/include/svo_ros/visualizer.h
@@ -42,8 +42,7 @@ class FrameHandlerMono;
 
 typedef boost::shared_ptr<Frame> FramePtr;
 
-/// This class bundles all functions to publish visualisation messages,
-/// trace data and output debugging information.
+/// This class bundles all functions to publish visualisation messages.
 class Visualizer
 {
 public:
@@ -59,14 +58,30 @@ public:
   ros::Publisher pub_dense_;
   image_transport::Publisher pub_images_;
   tf::TransformBroadcaster br_;
+  bool publish_world_in_cam_frame_;
+  SE3 T_world_from_vision_;
 
   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 publishMinimal(
+      const cv::Mat& img,
+      const FramePtr& frame,
+      const FrameHandlerMono& slam,
+      const double timestamp);
+
+  void visualizeMarkers(
+      const FramePtr& frame,
+      const set<FramePtr>& core_kfs,
+      const Map& map);
+
   void publishMapRegion(set<FramePtr> frames);
+
   void removeDeletedPts(const Map& map);
+
   void displayKeyframeWithMps(const FramePtr& frame, int ts);
+
   void exportToDense(const FramePtr& frame);
 };
 
diff --git a/svo_ros/src/benchmark_node.cpp b/svo_ros/src/benchmark_node.cpp
index 4083bf2..0c643eb 100644
--- a/svo_ros/src/benchmark_node.cpp
+++ b/svo_ros/src/benchmark_node.cpp
@@ -124,20 +124,20 @@ void BenchmarkNode::runBenchmark(const std::string& dataset_dir)
   dataset_reader.readAllEntries(dataset);
 
   // process dataset
+  int first_frame = vk::getParam<int>("svo/dataset_first_frame", 0);
   for(auto it = dataset.begin(); it != dataset.end() && ros::ok(); ++it, ++frame_count_)
   {
-    // Read image
+    if(frame_count_ < first_frame)
+      continue;
     std::string img_filename(dataset_dir + "/" + it->image_name_);
     cv::Mat img(cv::imread(img_filename, 0));
     if(img.empty()) {
       SVO_ERROR_STREAM("Reading image "<<img_filename<<" failed.");
       return;
     }
-
     vo_->addImage(img, it->timestamp_);
-
     visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, it->timestamp_);
-
+    visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map());
     if(vo_->stage() == svo::FrameHandlerMono::STAGE_DEFAULT_FRAME)
       tracePose(vo_->lastFrame()->T_f_w_.inverse(), it->timestamp_);
   }
@@ -188,32 +188,26 @@ void BenchmarkNode::runBlenderBenchmark(const std::string& dataset_dir)
           dataset_dir+"/depth/"+it->image_name_+"_0.depth", *cam_, depthmap);
 
       // extract features, generate features with 3D points
-      svo::feature_detection::FastDetector detector;
-      svo::feature_detection::Corners corners;
-      detector.detect(frame_ref->img_pyr_, frame_ref->fts_, svo::Config::gridSize(),
-                      svo::Config::nPyrLevels(), svo::Config::triangMinCornerScore(), &corners);
-      for(auto corner=corners.begin(); corner!=corners.end(); ++corner)
-      {
-        if(corner->score < Config::triangMinCornerScore())
-          continue;
-        svo::Feature* ftr = new svo::Feature(frame_ref.get(), Eigen::Vector2d(corner->x, corner->y), corner->level);
-        Eigen::Vector3d pt_pos_cur = ftr->f*depthmap.at<float>(corner->y, corner->x);
+      svo::feature_detection::FastDetector detector(
+          cam_->width(), cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels());
+      detector.detect(frame_ref.get(), frame_ref->img_pyr_, svo::Config::triangMinCornerScore(), frame_ref->fts_);
+      std::for_each(frame_ref->fts_.begin(), frame_ref->fts_.end(), [&](Feature* ftr) {
+        Eigen::Vector3d pt_pos_cur = ftr->f*depthmap.at<float>(ftr->px[1], ftr->px[0]);
         Eigen::Vector3d pt_pos_world = frame_ref->T_f_w_.inverse()*pt_pos_cur;
-        svo::Point* point = new svo::Point(pt_pos_world);
+        svo::Point* point = new svo::Point(pt_pos_world, ftr);
         ftr->point = point;
-        ftr->point->addFrameRef(ftr);
-        frame_ref->addFeature(ftr);
-      }
-      SVO_INFO_STREAM("Added "<<corners.size()<<" 3d pts to the reference frame.");
+      });
+      SVO_INFO_STREAM("Added "<<frame_ref->nObs()<<" 3d pts to the reference frame.");
       vo_->setFirstFrame(frame_ref);
       SVO_INFO_STREAM("Set reference frame.");
-      continue;
     }
-
-    SVO_DEBUG_STREAM("Processing image "<<it->image_name_<<".");
-    vo_->addImage(img, it->timestamp_);
-    visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, it->timestamp_);
-    visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map());
+    else
+    {
+      SVO_DEBUG_STREAM("Processing image "<<it->image_name_<<".");
+      vo_->addImage(img, it->timestamp_);
+      visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, it->timestamp_);
+      visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map());
+    }
 
     if(vo_->stage() != svo::FrameHandlerMono::STAGE_DEFAULT_FRAME)
     {
diff --git a/svo_ros/src/visualizer.cpp b/svo_ros/src/visualizer.cpp
index eda01c6..32ac3e1 100644
--- a/svo_ros/src/visualizer.cpp
+++ b/svo_ros/src/visualizer.cpp
@@ -43,7 +43,9 @@ Visualizer() :
     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))
+    dense_pub_nth_(vk::getParam<int>("svo/publish_every_nth_dense_input", 1)),
+    T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero()),
+    publish_world_in_cam_frame_(vk::getParam<bool>("svo/publish_world_in_cam_frame", true))
 {
   // Init ROS Marker Publishers
   pub_frames_ = pnh_.advertise<visualization_msgs::Marker>("keyframes", 10);
@@ -57,25 +59,11 @@ Visualizer() :
   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)
+void Visualizer::publishMinimal(
+    const cv::Mat& img,
+    const FramePtr& frame,
+    const FrameHandlerMono& slam,
+    const double timestamp)
 {
   ++trace_id_;
   std_msgs::Header header_msg;
@@ -83,7 +71,7 @@ publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono
   header_msg.seq = trace_id_;
   header_msg.stamp = ros::Time(timestamp);
 
-  // publish svo info msgs
+  // publish info msg.
   if(pub_info_.getNumSubscribers() > 0)
   {
     svo_msgs::Info msg_info;
@@ -101,17 +89,31 @@ publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono
     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)
+  if(frame == 0)
+  {
+    if(pub_images_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_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());
+    }
+    return;
+  }
+
+  // Publish pyramid-image every nth-frame.
+  if(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::STAGE_SECOND_FRAME)
     {
+      // During initialization, draw lines.
       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();
@@ -121,7 +123,6 @@ publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono
                  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,
@@ -141,30 +142,35 @@ publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono
                       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::STAGE_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)
+  if(pub_pose_.getNumSubscribers() > 0)
   {
+    Quaterniond q;
+    Vector3d p;
+    Matrix<double,6,6> Cov;
+    if(publish_world_in_cam_frame_)
+    {
+      // publish world in cam frame
+      SE3 T_cam_from_world(frame->T_f_w_* T_world_from_vision_);
+      q = Quaterniond(T_cam_from_world.rotation_matrix());
+      p = T_cam_from_world.translation();
+      Cov = frame->Cov_;
+    }
+    else
+    {
+      // publish cam in world frame
+      SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse());
+      q = Quaterniond(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose());
+      p = T_world_from_cam.translation();
+      Cov = T_world_from_cam.Adj()*frame->Cov_*T_world_from_cam.inverse().Adj();
+    }
     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];
@@ -174,13 +180,36 @@ publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono
     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);
+      msg_pose->pose.covariance[i] = Cov(i%6, i/6);
     pub_pose_.publish(msg_pose);
   }
 }
 
-void Visualizer::
-publishMapRegion(set<FramePtr> frames)
+void Visualizer::visualizeMarkers(
+    const FramePtr& frame,
+    const set<FramePtr>& core_kfs,
+    const Map& map)
+{
+  if(frame != NULL)
+  {
+    vk::output_helper::publishTfTransform(
+        frame->T_f_w_*T_world_from_vision_.inverse(),
+        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_, T_world_from_vision_*frame->pos(), "trajectory",
+          ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5));
+      publishMapRegion(core_kfs);
+      removeDeletedPts(map);
+    }
+  }
+}
+
+void Visualizer::publishMapRegion(set<FramePtr> frames)
 {
   if(pub_points_.getNumSubscribers() > 0)
   {
@@ -190,8 +219,7 @@ publishMapRegion(set<FramePtr> frames)
   }
 }
 
-void Visualizer::
-removeDeletedPts(const Map& map)
+void Visualizer::removeDeletedPts(const Map& map)
 {
   if(pub_points_.getNumSubscribers() > 0)
   {
@@ -200,11 +228,13 @@ removeDeletedPts(const Map& map)
   }
 }
 
-void Visualizer::
-displayKeyframeWithMps(const FramePtr& frame, int ts)
+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);
+  SE3 T_world_cam(T_world_from_vision_*frame->T_f_w_.inverse());
+  vk::output_helper::publishFrameMarker(
+      pub_frames_, T_world_cam.rotation_matrix(),
+      T_world_cam.translation(), "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)
@@ -215,13 +245,14 @@ displayKeyframeWithMps(const FramePtr& frame, int ts)
     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));
+    vk::output_helper::publishPointMarker(
+        pub_points_, T_world_from_vision_*(*it)->point->pos_, "pts",
+        ros::Time::now(), (*it)->point->id_, 0, 0.005, Vector3d(1.0, 0., 1.0));
     (*it)->point->last_published_ts_ = ts;
   }
 }
 
-void Visualizer::
-exportToDense(const FramePtr& frame)
+void Visualizer::exportToDense(const FramePtr& frame)
 {
   // publish air_ground_msgs
   if(frame != NULL && dense_pub_nth_ > 0
@@ -246,8 +277,8 @@ exportToDense(const FramePtr& frame)
       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);
+      min_z = fmin(pos[2], min_z);
+      max_z = fmax(pos[2], max_z);
     }
     msg.min_depth = (float) min_z;
     msg.max_depth = (float) max_z;
diff --git a/svo_ros/src/vo_node.cpp b/svo_ros/src/vo_node.cpp
index 9dc31dd..3dfdc92 100644
--- a/svo_ros/src/vo_node.cpp
+++ b/svo_ros/src/vo_node.cpp
@@ -74,18 +74,17 @@ VoNode() :
   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(
+  // Get initial position and orientation
+  visualizer_.T_world_from_vision_ = Sophus::SE3(
       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);
+
+  // Init VO and start
+  vo_ = new svo::FrameHandlerMono(cam_);
   vo_->start();
 }