diff --git a/svo/include/svo/depth_filter.h b/svo/include/svo/depth_filter.h
index 955799a..8effbfd 100644
--- a/svo/include/svo/depth_filter.h
+++ b/svo/include/svo/depth_filter.h
@@ -23,6 +23,7 @@
 #include <vikit/performance_monitor.h>
 #include <svo/global.h>
 #include <svo/feature_detection.h>
+#include <svo/matcher.h>
 
 namespace svo {
 
@@ -148,6 +149,7 @@ protected:
   double new_keyframe_min_depth_;       //!< Minimum depth in the new keyframe. Used for range in new seeds.
   double new_keyframe_mean_depth_;      //!< Maximum depth in the new keyframe. Used for range in new seeds.
   vk::PerformanceMonitor permon_;       //!< Separate performance monitor since the DepthFilter runs in a parallel thread.
+  Matcher matcher_;
 
   /// Initialize new seeds from a frame.
   void initializeSeeds(FramePtr frame);
diff --git a/svo/include/svo/feature.h b/svo/include/svo/feature.h
index 37be2b7..6f90a9a 100644
--- a/svo/include/svo/feature.h
+++ b/svo/include/svo/feature.h
@@ -46,7 +46,7 @@ struct Feature
     f(frame->cam_->cam2world(px)),
     level(_level),
     point(NULL),
-    grad(0.0,0.0)
+    grad(1.0,0.0)
   {}
 
   Feature(Frame* _frame, const Vector2d& _px, const Vector3d& _f, int _level) :
@@ -56,7 +56,7 @@ struct Feature
     f(_f),
     level(_level),
     point(NULL),
-    grad(0.0,0.0)
+    grad(1.0,0.0)
   {}
 
   Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, int _level) :
@@ -66,7 +66,7 @@ struct Feature
     f(_f),
     level(_level),
     point(_point),
-    grad(0.0,0.0)
+    grad(1.0,0.0)
   {}
 };
 
diff --git a/svo/include/svo/matcher.h b/svo/include/svo/matcher.h
index b79340b..ddd6e6c 100644
--- a/svo/include/svo/matcher.h
+++ b/svo/include/svo/matcher.h
@@ -21,6 +21,9 @@
 
 namespace vk {
   class AbstractCamera;
+  namespace patch_score {
+    template<int HALF_PATCH_SIZE> class ZMSSD;
+  }
 }
 
 namespace svo {
@@ -29,22 +32,6 @@ class Point;
 class Frame;
 class Feature;
 
-/// Patch-matcher for reprojection-matching and epipolar search in triangulation.
-namespace matcher {
-
-const int g_halfpatch_size = 4; // do not change, because of sse instructions alignment.
-const int g_patch_size = 8;     // do not change, because of sse instructions alignment.
-
-/// Manages the aligned memory of a patch. Aligned for SIMD instructions.
-struct PatchData
-{
-  const int patch_size;
-  uint8_t*  patch;
-  uint8_t*  patch_with_border;
-  PatchData(int patch_size);
-  ~PatchData();
-};
-
 /// Warp a patch from the reference view to the current view.
 namespace warp {
 
@@ -68,38 +55,67 @@ void warpAffine(
     const Vector2d& px_ref,
     const int level_ref,
     const int level_cur,
-    PatchData& ref_patch);
+    const int halfpatch_size,
+    uint8_t* patch);
 
 } // namespace warp
 
-// TODO take function from vikit
-bool depthFromTriangulation(
-    const SE3& T_cur_ref,
-    const Vector3d& f_ref,
-    const Vector3d& f_cur,
-    double& depth);
+/// Patch-matcher for reprojection-matching and epipolar search in triangulation.
+class Matcher
+{
+public:
+  static const int halfpatch_size_ = 4;
+  static const int patch_size_ = 8;
 
-/// Find a match by directly applying subpix refinement.
-/// IMPORTANT! This function assumes that px_cur is already set to an estimate that is within ~2-3 pixel of the final result!
-bool findMatchDirect(
-    const Point& pt,
-    const Frame& frame,
-    Vector2d& px_cur,
-    int& level_cur);
+  typedef vk::patch_score::ZMSSD<halfpatch_size_> PatchScore;
 
-/// Find a match by searching along the epipolar line without using any features.
-bool findEpipolarMatchDirect(
-    const Frame& ref_frame,
-    const Frame& cur_frame,
-    const Feature& ref_ftr,
-    const double d_estimate,
-    const double d_min,
-    const double d_max,
-    const bool align_1d,
-    double& depth,
-    double& h_inv);
+  struct Options
+  {
+    bool align_1d;              //!< in epipolar search: align patch 1D along epipolar line
+    int align_max_iter;         //!< number of iterations for aligning the feature patches in gauss newton
+    double max_epi_length_optim;//!< max length of epipolar line to skip epipolar search and directly go to img align
+    size_t max_epi_search_steps;//!< max number of evaluations along epipolar line
+    bool subpix_refinement;     //!< do gauss newton feature patch alignment after epipolar search
+    Options() :
+      align_1d(false),
+      align_max_iter(10),
+      max_epi_length_optim(2.0),
+      max_epi_search_steps(1000),
+      subpix_refinement(true)
+    {}
+  } options_;
+
+  uint8_t* patch_;
+  uint8_t* patch_with_border_;
+  Matrix2d A_cur_ref_;          //!< affine warp matrix
+  double epi_length_;           //!< length of epipolar line segment in pixels (only used for epipolar search)
+  double h_inv_;                //!< hessian of 1d image alignment along epipolar line
+  int search_level_;
+  Feature* ref_ftr_;
+
+  Matcher();
+  ~Matcher();
+
+  /// Find a match by directly applying subpix refinement.
+  /// IMPORTANT! This function assumes that px_cur is already set to an estimate that is within ~2-3 pixel of the final result!
+  bool findMatchDirect(
+      const Point& pt,
+      const Frame& frame,
+      Vector2d& px_cur);
+
+  /// Find a match by searching along the epipolar line without using any features.
+  bool findEpipolarMatchDirect(
+      const Frame& ref_frame,
+      const Frame& cur_frame,
+      const Feature& ref_ftr,
+      const double d_estimate,
+      const double d_min,
+      const double d_max,
+      double& depth);
+
+  void createPatchFromPatchWithBorder();
+};
 
-} // namespace matcher
 } // namespace svo
 
 #endif // SVO_MATCHER_H_
diff --git a/svo/include/svo/reprojector.h b/svo/include/svo/reprojector.h
index cc6df72..7207b54 100644
--- a/svo/include/svo/reprojector.h
+++ b/svo/include/svo/reprojector.h
@@ -86,6 +86,7 @@ private:
   };
 
   Grid grid_;
+  Matcher matcher_;
   Map& map_;
 
   static bool pointQualityComparator(Candidate& lhs, Candidate& rhs);
diff --git a/svo/src/depth_filter.cpp b/svo/src/depth_filter.cpp
index 2cbafe2..0aba62a 100644
--- a/svo/src/depth_filter.cpp
+++ b/svo/src/depth_filter.cpp
@@ -229,10 +229,9 @@ void DepthFilter::updateSeeds(FramePtr frame)
     // we are using inverse depth coordinates
     float z_inv_min = it->mu + sqrt(it->sigma2);
     float z_inv_max = max(it->mu - sqrt(it->sigma2), 0.00000001f);
-    double z, h_inv = -1.0;
-    if(!matcher::findEpipolarMatchDirect(
-        *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))
+    double z;
+    if(!matcher_.findEpipolarMatchDirect(
+        *it->ftr->frame, *frame, *it->ftr, 1.0/it->mu, 1.0/z_inv_min, 1.0/z_inv_max, z))
     {
       it->b++; // increase outlier probability when no match was found
       ++it;
diff --git a/svo/src/frame_handler_base.cpp b/svo/src/frame_handler_base.cpp
index f526cb3..41b03ce 100644
--- a/svo/src/frame_handler_base.cpp
+++ b/svo/src/frame_handler_base.cpp
@@ -41,7 +41,7 @@ FrameHandlerBase::FrameHandlerBase() :
   acc_frame_timings_(10),
   acc_num_obs_(10),
   num_obs_last_(0),
-  tracking_quality_(TRACKING_BAD)
+  tracking_quality_(TRACKING_INSUFFICIENT)
 {
 #ifdef SVO_TRACE
   // Initialize Performance Monitor
@@ -141,7 +141,7 @@ void FrameHandlerBase::resetCommon()
   stage_ = STAGE_PAUSED;
   set_reset_ = false;
   set_start_ = false;
-  tracking_quality_ = TRACKING_BAD;
+  tracking_quality_ = TRACKING_INSUFFICIENT;
   num_obs_last_ = 0;
   SVO_INFO_STREAM("RESET");
 }
diff --git a/svo/src/matcher.cpp b/svo/src/matcher.cpp
index 550d25b..9ccc000 100644
--- a/svo/src/matcher.cpp
+++ b/svo/src/matcher.cpp
@@ -28,20 +28,6 @@
 #include <svo/feature_alignment.h>
 
 namespace svo {
-namespace matcher {
-
-PatchData::PatchData(int patch_size) :
-    patch_size(patch_size)
-{
-  patch = vk::aligned_mem::aligned_alloc<uint8_t>(patch_size*patch_size, 16);
-  patch_with_border = vk::aligned_mem::aligned_alloc<uint8_t>((patch_size+2)*(patch_size+2), 16);
-}
-
-PatchData::~PatchData()
-{
-  free(patch);
-  free(patch_with_border);
-}
 
 namespace warp {
 
@@ -74,14 +60,14 @@ int getBestSearchLevel(
     const int max_level)
 {
   // Compute patch level in other image
-  int level_cur = 0;
+  int search_level = 0;
   double D = A_cur_ref.determinant();
-  while(D > 3.0 && level_cur < max_level)
+  while(D > 3.0 && search_level < max_level)
   {
-    level_cur += 1;
+    search_level += 1;
     D *= 0.25;
   }
-  return level_cur;
+  return search_level;
 }
 
 void warpAffine(
@@ -89,12 +75,12 @@ void warpAffine(
     const cv::Mat& img_ref,
     const Vector2d& px_ref,
     const int level_ref,
-    const int level_cur,
-    PatchData& ref_patch)
+    const int search_level,
+    const int halfpatch_size,
+    uint8_t* patch)
 {
-  const int halfpatch_size = ref_patch.patch_size/2+1;
   const int patch_size = halfpatch_size*2 ;
-  Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>();
+  const Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>();
   if(isnan(A_ref_cur(0,0)))
   {
     printf("Affine warp is NaN, probably camera has no translation\n"); // TODO
@@ -102,64 +88,36 @@ void warpAffine(
   }
 
   // Perform the warp on a larger patch.
-  uint8_t* ref_patch_ptr = ref_patch.patch_with_border;
+  uint8_t* patch_ptr = patch;
   const Vector2f px_ref_pyr = px_ref.cast<float>() / (1<<level_ref);
   for (int y=0; y<patch_size; ++y)
   {
-    for (int x=0; x<patch_size; ++x, ++ref_patch_ptr)
+    for (int x=0; x<patch_size; ++x, ++patch_ptr)
     {
       Vector2f px_patch(x-halfpatch_size, y-halfpatch_size);
-      px_patch *= (1<<level_cur);
+      px_patch *= (1<<search_level);
       const Vector2f px(A_ref_cur*px_patch + px_ref_pyr);
       if (px[0]<0 || px[1]<0 || px[0]>=img_ref.cols-1 || px[1]>=img_ref.rows-1)
-        *ref_patch_ptr = 0;
+        *patch_ptr = 0;
       else
-        *ref_patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]);
+        *patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]);
     }
   }
-
-  // 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)
-  {
-    uint8_t* ref_patch_border_ptr = ref_patch.patch_with_border + y*(g_patch_size+2) + 1;
-    for(int x=0; x<g_patch_size; ++x)
-      ref_patch_ptr[x] = ref_patch_border_ptr[x];
-  }
 }
 
 } // namespace warp
 
-bool findMatchDirect(
-    const Point& pt,
-    const Frame& cur_frame,
-    Vector2d& px_cur,
-    int& level_cur)
+
+Matcher::Matcher()
 {
-  Feature* ref_ftr;
-  if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr))
-    return false;
+  patch_ = vk::aligned_mem::aligned_alloc<uint8_t>(patch_size_*patch_size_, 16);
+  patch_with_border_ = vk::aligned_mem::aligned_alloc<uint8_t>((patch_size_+2)*(patch_size_+2), 16);
+}
 
-  if(!ref_ftr->frame->cam_->isInFrame(
-      ref_ftr->px.cast<int>()/(1<<ref_ftr->level), g_halfpatch_size+2, ref_ftr->level))
-    return false;
-
-  // warp affine
-  PatchData ref_patch(g_patch_size);
-  Matrix2d A_cur_ref;
-  warp::getWarpMatrixAffine(
-      *ref_ftr->frame->cam_, *cur_frame.cam_, ref_ftr->px, ref_ftr->f,
-      (ref_ftr->frame->pos() - pt.pos_).norm(), cur_frame.T_f_w_ * ref_ftr->frame->T_f_w_.inverse(), ref_ftr->level, A_cur_ref);
-  level_cur = warp::getBestSearchLevel(A_cur_ref, Config::nPyrLevels()-1);
-  warp::warpAffine(
-      A_cur_ref, ref_ftr->frame->img_pyr_[ref_ftr->level], ref_ftr->px,  ref_ftr->level, level_cur, ref_patch);
-
-  // px_cur should be set
-  Vector2d px_scaled(px_cur/(1<<level_cur));
-  bool success = feature_alignment::align2D(
-      cur_frame.img_pyr_[level_cur], ref_patch.patch_with_border, ref_patch.patch, 10, px_scaled);
-  px_cur = px_scaled * (1<<level_cur);
-  return success;
+Matcher::~Matcher()
+{
+  free(patch_);
+  free(patch_with_border_);
 }
 
 bool depthFromTriangulation(
@@ -177,20 +135,57 @@ bool depthFromTriangulation(
   return true;
 }
 
-bool findEpipolarMatchDirect(
+void Matcher::createPatchFromPatchWithBorder()
+{
+  uint8_t* ref_patch_ptr = patch_;
+  for(int y=1; y<patch_size_+1; ++y, ref_patch_ptr += patch_size_)
+  {
+    uint8_t* ref_patch_border_ptr = patch_with_border_ + y*(patch_size_+2) + 1;
+    for(int x=0; x<patch_size_; ++x)
+      ref_patch_ptr[x] = ref_patch_border_ptr[x];
+  }
+}
+
+bool Matcher::findMatchDirect(
+    const Point& pt,
+    const Frame& cur_frame,
+    Vector2d& px_cur)
+{
+  if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_))
+    return false;
+
+  if(!ref_ftr_->frame->cam_->isInFrame(
+      ref_ftr_->px.cast<int>()/(1<<ref_ftr_->level), halfpatch_size_+2, ref_ftr_->level))
+    return false;
+
+  // warp affine
+  warp::getWarpMatrixAffine(
+      *ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f,
+      (ref_ftr_->frame->pos() - pt.pos_).norm(),
+      cur_frame.T_f_w_ * ref_ftr_->frame->T_f_w_.inverse(), ref_ftr_->level, A_cur_ref_);
+  search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
+  warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,
+                   ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_);
+  createPatchFromPatchWithBorder();
+
+  // px_cur should be set
+  Vector2d px_scaled(px_cur/(1<<search_level_));
+  bool success = feature_alignment::align2D(
+      cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
+      options_.align_max_iter, px_scaled);
+  px_cur = px_scaled * (1<<search_level_);
+  return success;
+}
+
+bool Matcher::findEpipolarMatchDirect(
     const Frame& ref_frame,
     const Frame& cur_frame,
     const Feature& ref_ftr,
     const double d_estimate,
     const double d_min,
     const double d_max,
-    const bool align_1d,
-    double& depth,
-    double& h_inv)
+    double& depth)
 {
-  typedef vk::patch_score::ZMSSD<g_halfpatch_size> PatchScore;
-
-  PatchData ref_patch(g_patch_size);
   SE3 T_cur_ref = cur_frame.T_f_w_ * ref_frame.T_f_w_.inverse();
   int zmssd_best = PatchScore::threshold();
   Vector2d uv_best;
@@ -201,64 +196,57 @@ bool findEpipolarMatchDirect(
   Vector2d epi_dir = A - B;
 
   // Compute affine warp matrix
-  Matrix2d A_cur_ref;
   warp::getWarpMatrixAffine(
       *ref_frame.cam_, *cur_frame.cam_, ref_ftr.px, ref_ftr.f,
-      d_estimate, T_cur_ref, ref_ftr.level, A_cur_ref);
-  int level_cur = warp::getBestSearchLevel(A_cur_ref, Config::nPyrLevels()-1);
+      d_estimate, T_cur_ref, ref_ftr.level, A_cur_ref_);
+  search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
 
   // Find length of search range on epipolar line
   Vector2d px_A(cur_frame.cam_->world2cam(A));
   Vector2d px_B(cur_frame.cam_->world2cam(B));
-  double px_length = (px_A-px_B).norm() / (1<<level_cur);
-
-#ifdef DEBUG_MATCHER_EPISEARCH
-    printf("px_length = %f\n", px_length);
-    cv::Mat img_rgb = cv::Mat(cur_frame.img_pyr_.at(level_cur).size(), CV_8UC3);
-    cv::cvtColor(cur_frame.img_pyr_.at(level_cur), img_rgb, CV_GRAY2RGB);
-#endif
+  epi_length_ = (px_A-px_B).norm() / (1<<search_level_);
 
   // Warp reference patch at ref_level
-  warp::warpAffine(
-      A_cur_ref, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px,  ref_ftr.level, level_cur, ref_patch);
+  warp::warpAffine(A_cur_ref_, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px,
+                   ref_ftr.level, search_level_, halfpatch_size_+1, patch_with_border_);
+  createPatchFromPatchWithBorder();
 
-  //printf("px_length = %f\n", px_length);
-  if(px_length < 2.0)
+  if(epi_length_ < 2.0)
   {
     Vector2d px = (px_A+px_B)/2.0;
-    Vector2d px_scaled(px/(1<<level_cur));
+    Vector2d px_scaled(px/(1<<search_level_));
     bool res;
-    if(align_1d)
+    if(options_.align_1d)
       res = feature_alignment::align1D(
-          cur_frame.img_pyr_[level_cur], (px_A-px_B).cast<float>().normalized(),
-          ref_patch.patch_with_border, ref_patch.patch, 10, px_scaled, h_inv);
+          cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().normalized(),
+          patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
     else
       res = feature_alignment::align2D(
-          cur_frame.img_pyr_[level_cur], ref_patch.patch_with_border,
-          ref_patch.patch, 10, px_scaled);
+          cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
+          options_.align_max_iter, px_scaled);
     if(res)
     {
-      px = px_scaled*(1<<level_cur);
+      px = px_scaled*(1<<search_level_);
       if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px), depth))
         return true;
     }
     return false;
   }
 
-  size_t n_steps = px_length/0.7; // one step per pixel
+  size_t n_steps = epi_length_/0.7; // one step per pixel
   Vector2d step = epi_dir/n_steps;
 
-  if(n_steps > 1000)
+  if(n_steps > options_.max_epi_search_steps)
   {
     printf("WARNING: skip epipolar search: %zu evaluations, px_lenght=%f, d_min=%f, d_max=%f.\n",
-           n_steps, px_length, d_min, d_max);
+           n_steps, epi_length_, d_min, d_max);
     return false;
   }
 
   // for matching, precompute sum and sum2 of warped reference patch
   int pixel_sum = 0;
   int pixel_sum_square = 0;
-  PatchScore patch_score(ref_patch.patch);
+  PatchScore patch_score(patch_);
 
   // now we sample along the epipolar line
   Vector2d uv = B-step;
@@ -267,24 +255,22 @@ bool findEpipolarMatchDirect(
   for(size_t i=0; i<n_steps; ++i, uv+=step)
   {
     Vector2d px(cur_frame.cam_->world2cam(uv));
-    Vector2i pxi(px[0]/(1<<level_cur)+0.5,
-                 px[1]/(1<<level_cur)+0.5); // +0.5 to round to closest int
-
-#ifdef DEBUG_MATCHER_EPISEARCH
-    cv::circle(img_rgb, cv::Point2f(pxi[0], pxi[1]), 1, cv::Scalar(0,0,255), 1);
-#endif
+    Vector2i pxi(px[0]/(1<<search_level_)+0.5,
+                 px[1]/(1<<search_level_)+0.5); // +0.5 to round to closest int
 
     if(pxi == last_checked_pxi)
       continue;
     last_checked_pxi = pxi;
 
     // check if the patch is full within the new frame
-    if(!cur_frame.cam_->isInFrame(pxi, g_patch_size, level_cur))
+    if(!cur_frame.cam_->isInFrame(pxi, patch_size_, search_level_))
       continue;
 
     // TODO interpolation would probably be a good idea
-    uint8_t* cur_patch_ptr = cur_frame.img_pyr_[level_cur].data + (pxi[1]-g_halfpatch_size)*cur_frame.img_pyr_[level_cur].cols + (pxi[0]-g_halfpatch_size);
-    int zmssd = patch_score.computeScore(cur_patch_ptr, cur_frame.img_pyr_[level_cur].cols);
+    uint8_t* cur_patch_ptr = cur_frame.img_pyr_[search_level_].data
+                             + (pxi[1]-halfpatch_size_)*cur_frame.img_pyr_[search_level_].cols
+                             + (pxi[0]-halfpatch_size_);
+    int zmssd = patch_score.computeScore(cur_patch_ptr, cur_frame.img_pyr_[search_level_].cols);
 
     if(zmssd < zmssd_best) {
       zmssd_best = zmssd;
@@ -292,41 +278,24 @@ bool findEpipolarMatchDirect(
     }
   }
 
-#ifdef DEBUG_MATCHER_EPISEARCH
-  if(zmssd_best < g_zmssd_thresh) {
-    Vector2d px(cur_frame.cam_->world2cam(uv_best));
-    Vector2i pxi(px[0]/(1<<level_cur)+0.5, px[1]/(1<<level_cur)+0.5);
-    cv::circle(img_rgb, cv::Point2f(px[0]/(1<<level_cur), px[1]/(1<<level_cur)), 8, cv::Scalar(255,0,0), 2);
-  }
-  cv::imshow("cur_img", img_rgb);
-  cv::Mat ref_img_rgb = cv::Mat(ref_frame.img_pyr_.at(ref_ftr.level).size(), CV_8UC3);
-  cv::cvtColor(ref_frame.img_pyr_.at(ref_ftr.level), ref_img_rgb, CV_GRAY2RGB);
-  cv::circle(ref_img_rgb, cv::Point2f(ref_ftr.px[0]/(1<<ref_ftr.level), ref_ftr.px[1]/(1<<ref_ftr.level)), 8, cv::Scalar(255,0,0), 2);
-  cv::imshow("ref_img", ref_img_rgb);
-  uint8_t* cur_patch_ptr = cur_frame.img_pyr_[level_cur].data + (pxi[1]-g_halfpatch_size)*cur_frame.img_pyr_[level_cur].cols + (pxi[0]-g_halfpatch_size);
-  cv::Mat cur_patch(g_patch_size,g_patch_size,CV_8U,cur_patch_ptr);
-  cv::imshow("cur_patch", cur_patch);
-  cv::waitKey(0);
-#endif
-
   if(zmssd_best < PatchScore::threshold())
   {
-//    if(px_length < 4.0)
+    if(options_.subpix_refinement)
     {
       Vector2d px(cur_frame.cam_->world2cam(uv_best));
-      Vector2d px_scaled(px/(1<<level_cur));
+      Vector2d px_scaled(px/(1<<search_level_));
       bool res;
-      if(align_1d)
+      if(options_.align_1d)
         res = feature_alignment::align1D(
-            cur_frame.img_pyr_[level_cur], (px_A-px_B).cast<float>().normalized(),
-            ref_patch.patch_with_border, ref_patch.patch, 10, px_scaled, h_inv);
+            cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().normalized(),
+            patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
       else
         res = feature_alignment::align2D(
-            cur_frame.img_pyr_[level_cur], ref_patch.patch_with_border,
-            ref_patch.patch, 10, px_scaled);
+            cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
+            options_.align_max_iter, px_scaled);
       if(res)
       {
-        px = px_scaled*(1<<level_cur);
+        px = px_scaled*(1<<search_level_);
         if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px), depth))
           return true;
       }
@@ -339,5 +308,4 @@ bool findEpipolarMatchDirect(
   return false;
 }
 
-} // namespace matcher
 } // namespace svo
diff --git a/svo/src/reprojector.cpp b/svo/src/reprojector.cpp
index c0a0bc1..6081620 100644
--- a/svo/src/reprojector.cpp
+++ b/svo/src/reprojector.cpp
@@ -162,10 +162,9 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
       continue;
     }
 
-    int feature_level;
     bool found_match = true;
     if(options_.find_match_direct)
-      found_match = matcher::findMatchDirect(*it->pt, *frame, it->px, feature_level);
+      found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);
     if(!found_match)
     {
       it->pt->n_failed_reproj_++;
@@ -180,10 +179,18 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
     if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
       it->pt->type_ = Point::TYPE_GOOD;
 
-    Feature* new_feature = new Feature(frame.get(), it->px, feature_level);
+    Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_);
     frame->addFeature(new_feature);
+
+    // Here we add a reference in the feature to the 3D point, the other way
+    // round is only done if this frame is selected as keyframe.
     new_feature->point = it->pt;
-    // note, we add a reference to the point only if the frame is selected as a keyframe
+
+    if(matcher_.ref_ftr_->type == Feature::EDGELET)
+    {
+      new_feature->type = Feature::EDGELET;
+      new_feature->grad = matcher_.A_cur_ref_*matcher_.ref_ftr_->grad;
+    }
 
     // If the keyframe is selected and we reproject the rest, we don't have to
     // check this point anymore.
diff --git a/svo/test/test_matcher.cpp b/svo/test/test_matcher.cpp
index c81c1fe..37ad1f1 100644
--- a/svo/test/test_matcher.cpp
+++ b/svo/test/test_matcher.cpp
@@ -82,6 +82,7 @@ class MatcherTest {
 
 void MatcherTest::testEpipolarSearchFullImg()
 {
+  svo::Matcher matcher;
   cv::Mat depthmap(frame_ref_->cam_->height(), frame_ref_->cam_->width(), CV_32FC1, cv::Scalar(0));
   cv::Mat error_map(depthmap.rows, depthmap.cols, CV_32FC1, cv::Scalar(0));
   cv::Mat mask(depthmap.rows, depthmap.cols, CV_8UC1, cv::Scalar(1));
@@ -95,10 +96,10 @@ void MatcherTest::testEpipolarSearchFullImg()
     {
       svo::Feature ftr(frame_ref_, Eigen::Vector2d(x,y), 0);
       double depth_gt = depth_ref_.at<float>(y, x);
-      double depth_estimate, h_inv;
-      int res = svo::matcher::findEpipolarMatchDirect(
-          *frame_ref_, *frame_cur_, ftr,
-          depth_gt, fmax(depth_gt-depth_range,0.0), depth_gt+depth_range, false, depth_estimate, h_inv);
+      double depth_estimate;
+      int res = matcher.findEpipolarMatchDirect(
+          *frame_ref_, *frame_cur_, ftr, depth_gt, fmax(depth_gt-depth_range,0.0),
+          depth_gt+depth_range, depth_estimate);
       if(res)
       {
         depthmap.at<float>(y,x) = depth_estimate;
@@ -148,21 +149,23 @@ void MatcherTest::testWarpAffine()
 {
   const int halfpatch_size = 15;
   const int patch_size=halfpatch_size*2;
-  svo::matcher::PatchData md(patch_size);
+  svo::Matcher matcher;
   double depth = 1.0;
 
   Sophus::SE3 T_cur_ref(frame_cur_->T_f_w_*frame_ref_->T_f_w_.inverse());
   Eigen::Vector2d px_cur(frame_cur_->cam_->world2cam(T_cur_ref*(ref_ftr_->f*depth)));
 
   // compute reference patch
-  cv::Mat ref_patch(patch_size, patch_size, CV_8U, md.patch_with_border);
+  cv::Mat ref_patch(matcher.halfpatch_size_+1, matcher.halfpatch_size_+1,
+                    CV_8U, matcher.patch_with_border_);
 
   Eigen::Matrix2d A_cur_ref;
-  svo::matcher::warp::getWarpMatrixAffine(
+  svo::warp::getWarpMatrixAffine(
       *cam_, *cam_, ref_ftr_->px, ref_ftr_->f, 1.0, T_cur_ref, ref_ftr_->level, A_cur_ref);
-  int level_cur = svo::matcher::warp::getBestSearchLevel(A_cur_ref, svo::Config::nPyrLevels()-1);
-  svo::matcher::warp::warpAffine(
-      A_cur_ref, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,  ref_ftr_->level, level_cur, md);
+  int level_cur = svo::warp::getBestSearchLevel(A_cur_ref, svo::Config::nPyrLevels()-1);
+  svo::warp::warpAffine(
+      A_cur_ref, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,
+      ref_ftr_->level, level_cur, matcher.halfpatch_size_+1, matcher.patch_with_border_);
 
   // copy current patch
   Eigen::Vector2i pxi(px_cur[0]/(1<<level_cur)+0.5, px_cur[1]/(1<<level_cur)+0.5);