add banchmark image alignment 2

2021-08-23 15:35:30 +02:00
parent 8df5e42a5b
commit 90e452f0c9
34 changed files with 3294 additions and 0 deletions
@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml
+# Editor-based HTTP Client requests
+/httpRequests/
@@ -0,0 +1 @@
+Vio_OpenCL
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module classpath="CMake" type="CPP_MODULE" version="4" />
@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$" />
+</project>
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/Benchmark_openCL.iml" filepath="$PROJECT_DIR$/.idea/Benchmark_openCL.iml" />
+    </modules>
+  </component>
+</project>
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$" vcs="Git" />
+  </component>
+</project>
@@ -0,0 +1,81 @@
+###############################################################################
+# 
+# CMake script for finding the Eigen library.
+# 
+# http://eigen.tuxfamily.org/index.php?title=Main_Page
+# 
+# Copyright (c) 2006, 2007 Montel Laurent, <[email protected]>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <[email protected]>
+# Copyright (c) 2009 Benoit Jacob <[email protected]>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD
+# license.
+# 
+# 
+# Input variables:
+# 
+# - Eigen_ROOT_DIR (optional): When specified, header files and libraries
+#   will be searched for in `${Eigen_ROOT_DIR}/include` and
+#   `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order
+#   will be ignored. When unspecified, the default CMake search order is used.
+#   This variable can be specified either as a CMake or environment variable.
+#   If both are set, preference is given to the CMake variable.
+#   Use this variable for finding packages installed in a nonstandard location,
+#   or for enforcing that one of multiple package installations is picked up.
+# 
+# Cache variables (not intended to be used in CMakeLists.txt files)
+# 
+# - Eigen_INCLUDE_DIR: Absolute path to package headers.
+# 
+# 
+# Output variables:
+# 
+# - Eigen_FOUND: Boolean that indicates if the package was found
+# - Eigen_INCLUDE_DIRS: Paths to the necessary header files
+# - Eigen_VERSION: Version of Eigen library found
+# - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen
+# 
+# 
+# Example usage:
+# 
+#   # Passing the version means Eigen_FOUND will only be TRUE if a
+#   # version >= the provided version is found.
+#   find_package(Eigen 3.1.2)
+#   if(NOT Eigen_FOUND)
+#     # Error handling
+#   endif()
+#   ...
+#   add_definitions(${Eigen_DEFINITIONS})
+#   ...
+#   include_directories(${Eigen_INCLUDE_DIRS} ...)
+# 
+###############################################################################
+
+find_package(PkgConfig)
+pkg_check_modules(PC_EIGEN eigen3)
+set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER})
+
+
+find_path(EIGEN_INCLUDE_DIR Eigen/Core
+    HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS}
+          "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}"
+          "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}"  # Backwards Compatibility
+    PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen"
+          "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0"
+    PATH_SUFFIXES eigen3 include/eigen3 include)
+
+set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR)
+
+mark_as_advanced(EIGEN_INCLUDE_DIR)
+
+if(EIGEN_FOUND)
+  message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})")
+endif(EIGEN_FOUND)
+
+
+set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
+set(Eigen_FOUND ${EIGEN_FOUND})
+set(Eigen_VERSION ${EIGEN_VERSION})
+set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS})
@@ -0,0 +1,127 @@
+/*
+ * aligned_mem.h
+ *
+ *  Created on: May 14, 2013
+ *      Author: cforster
+ *
+ *  Code from libcvd: cvd/internal/aligned_mem.h
+ *  https://github.com/edrosten/libcvd
+ *  Licence: LGPL 2.1
+ */
+
+#ifndef VIKIT_ALIGNED_MEM_H_
+#define VIKIT_ALIGNED_MEM_H_
+
+#include <string>       // memset
+#include <string.h>     // memset
+#include <cassert>
+#include <cstdlib>
+
+namespace vk {
+namespace aligned_mem {
+
+  /// Check if the pointer is aligned to the specified byte granularity
+  inline bool
+  is_aligned8(const void* ptr)
+  {
+    return ((reinterpret_cast<size_t>(ptr)) & 0x7) == 0;
+  }
+
+  inline bool
+  is_aligned16(const void* ptr)
+  {
+    return ((reinterpret_cast<size_t>(ptr)) & 0xF) == 0;
+  }
+
+  template <class T, int N=20> struct placement_delete
+  {
+    enum { Size = (1<<N) };
+
+    struct Array {
+      T data[Size];
+    };
+
+    static inline void destruct(T* buf)
+    {
+      (*(Array*)buf).~Array();
+    }
+
+    static inline void free(T* buf, size_t M)
+    {
+      if (M >= Size) {
+        placement_delete<T,N>::free(buf+Size,M-Size);
+        placement_delete<T,N>::destruct(buf);
+      } else {
+        placement_delete<T,N-1>::free(buf, M);
+      }
+    }
+  };
+
+  template <class T> struct placement_delete<T,-1>
+  {
+    static inline void free(T*, size_t ) {}
+  };
+
+  inline void * aligned_alloc(size_t count, size_t alignment){
+    void * mem = NULL;
+    assert(posix_memalign(&mem, alignment, count) == 0);
+    return mem;
+  }
+
+  inline void aligned_free(void * memory) {
+    free(memory);
+  }
+
+  template <class T>
+  inline T * aligned_alloc(size_t count, size_t alignment){
+    void * data = aligned_alloc(sizeof(T)* count, alignment);
+    return new (data) T[count];
+  }
+
+  template <class T>
+  inline void aligned_free(T * memory, size_t count){
+    placement_delete<T>::free(memory, count);
+    aligned_free(memory);
+  }
+
+  template<class T> inline void memfill(T* data, int n, const T val)
+  {
+    T* de = data + n;
+    for(;data < de; data++)
+      *data=val;
+  }
+
+  template<> inline void memfill(unsigned char* data, int n, const unsigned char val)
+  {
+    memset(data, val, n);
+  }
+
+  template<> inline void memfill(signed char* data, int n, const signed char val)
+  {
+    memset(data, val, n);
+  }
+
+  template<> inline void memfill(char* data, int n, const char val)
+  {
+    memset(data, val, n);
+  }
+
+  template <class T, int N>
+  struct AlignedMem {
+    T* mem;
+    size_t count;
+    AlignedMem(size_t c) : count(c) {
+      mem = aligned_alloc<T>(count, N);
+    }
+    ~AlignedMem() {
+      aligned_free<T>(mem, count);
+    }
+    T* data() { return mem; }
+    const T* data() const { return mem; }
+  };
+
+} // namespace aligned_mem
+} // namespace vikit
+
+
+#endif // VIKIT_ALIGNED_MEM_H_
@@ -0,0 +1,172 @@
+/*
+ * math_utils.h
+ *
+ *  Created on: Jul 20, 2012
+ *      Author: cforster
+ */
+
+#ifndef MATH_UTILS_H_
+#define MATH_UTILS_H_
+
+
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+#include <sophus/se3.h>
+#include <iostream>
+
+namespace vk
+{
+
+using namespace Eigen;
+using namespace std;
+using namespace Sophus;
+
+Vector3d triangulateFeatureNonLin(
+    const Matrix3d& R,
+    const Vector3d& t,
+    const Vector3d& feature1,
+    const Vector3d& feature2);
+
+/// Assumes the bearing vectors f_c and f_r are on the epipolar plane, i.e.
+/// perfect triangulation without noise!
+bool depthFromTriangulationExact(
+    const Matrix3d& R_r_c,
+    const Vector3d& t_r_c,
+    const Vector3d& f_r,
+    const Vector3d& f_c,
+    double& depth_in_r,
+    double& depth_in_c);
+
+double reprojError(
+    const Vector3d& f1,
+    const Vector3d& f2,
+    double error_multiplier2);
+
+double computeInliers(
+    const vector<Vector3d>& features1,
+    const vector<Vector3d>& features2,
+    const Matrix3d& R,
+    const Vector3d& t,
+    const double reproj_thresh,
+    double error_multiplier2,
+    vector<Vector3d>& xyz_vec,
+    vector<int>& inliers,
+    vector<int>& outliers);
+
+void computeInliersOneView(
+    const vector<Vector3d> & feature_sphere_vec,
+    const vector<Vector3d> & xyz_vec,
+    const Matrix3d &R,
+    const Vector3d &t,
+    const double reproj_thresh,
+    const double error_multiplier2,
+    vector<int>& inliers,
+    vector<int>& outliers);
+
+//! Direct Cosine Matrix to Roll Pitch Yaw
+Vector3d dcm2rpy(const Matrix3d &R);
+
+//! Roll Pitch Yaw to Direct Cosine Matrix
+Matrix3d rpy2dcm(const Vector3d &rpy);
+
+//! Angle Axis parametrization to Quaternion
+Quaterniond angax2quat(const Vector3d& n, const double& angle);
+
+//! Angle Axis parametrization to Matrix representation
+Matrix3d angax2dcm(const Vector3d& n, const double& angle);
+
+double sampsonusError(
+    const Vector2d &v2Dash,
+    const Matrix3d& m3Essential,
+    const Vector2d& v2);
+
+inline Matrix3d sqew(const Vector3d& v)
+{
+  Matrix3d v_sqew;
+  v_sqew << 0, -v[2], v[1],
+            v[2], 0, -v[0],
+            -v[1], v[0], 0;
+  return v_sqew;
+}
+
+inline double norm_max(const Eigen::VectorXd & v)
+{
+  double max = -1;
+  for (int i=0; i<v.size(); i++)
+  {
+    double abs = fabs(v[i]);
+    if(abs>max){
+      max = abs;
+    }
+  }
+  return max;
+}
+
+inline Vector2d project2d(const Vector3d& v)
+{
+  return v.head<2>()/v[2];
+}
+
+inline Vector3d unproject2d(const Vector2d& v)
+{
+  return Vector3d(v[0], v[1], 1.0);
+}
+
+inline Vector3d project3d(const Vector4d& v)
+{
+  return v.head<3>()/v[3];
+}
+
+inline Vector4d unproject3d(const Vector3d& v)
+{
+  return Vector4d(v[0], v[1], v[2], 1.0);
+}
+
+template<class T>
+T getMedian(vector<T>& data_vec)
+{
+  assert(!data_vec.empty());
+  typename vector<T>::iterator it = data_vec.begin()+floor(data_vec.size()/2);
+  nth_element(data_vec.begin(), it, data_vec.end());
+  return *it;
+}
+
+inline double pyrFromZero_d(double x_0, int level)
+{
+  return x_0/(1<<level); // = 1 / 2^level
+}
+
+inline Vector2d pyrFromZero_2d(const Vector2d& uv_0, int level)
+{
+  return Vector2d(pyrFromZero_d(uv_0[0], level),
+                  pyrFromZero_d(uv_0[1], level));
+}
+
+inline void
+frameJac_xyz2uv(const Vector3d & xyz,
+                 const double & focal_length,
+                 Matrix<double,2,6> & frame_jac)
+{
+  const double x = xyz[0];
+  const double y = xyz[1];
+  const double z = xyz[2];
+  const double z_2 = z*z;
+
+  frame_jac(0,0) = -1./z *focal_length;
+  frame_jac(0,1) = 0;
+  frame_jac(0,2) = x/z_2 *focal_length;
+  frame_jac(0,3) =  x*y/z_2 * focal_length;
+  frame_jac(0,4) = -(1+(x*x/z_2)) *focal_length;
+  frame_jac(0,5) = y/z *focal_length;
+
+  frame_jac(1,0) = 0;
+  frame_jac(1,1) = -1./z *focal_length;
+  frame_jac(1,2) = y/z_2 *focal_length;
+  frame_jac(1,3) = (1+y*y/z_2) *focal_length;
+  frame_jac(1,4) = -x*y/z_2 *focal_length;
+  frame_jac(1,5) = -x/z *focal_length;
+}
+
+} // end namespace vk
+
+#endif /* MATH_UTILS_H_ */
@@ -0,0 +1,173 @@
+/*
+ * Abstract Nonlinear Least-Squares Solver Class
+ *
+ * nlls_solver.h
+ *
+ *  Created on: Nov 5, 2012
+ *      Author: cforster
+ */
+
+#ifndef LM_SOLVER_H_
+#define LM_SOLVER_H_
+
+#include <stdint.h>
+#include <stdio.h>
+#include <iostream>
+#include <math.h>
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+#include <math_utils.h>
+#include <robust_cost.h>
+
+namespace vk {
+
+using namespace std;
+using namespace Eigen;
+
+/**
+ * \brief Abstract Class for solving nonlinear least-squares (NLLS) problems.
+ *
+ * The function implements two algorithms: Levenberg Marquardt and Gauss Newton
+ *
+ * Example implementations of this function can be found in the rpl_examples
+ * package: img_align_2d.cpp, img_align_3d.cpp
+ *
+ * Template Parameters:
+ * D  : dimension of the residual
+ * T  : type of the model, e.g. SE2, SE3
+ */
+
+template <int D, typename T>
+class NLLSSolver {
+
+public:
+  typedef T ModelType;
+  enum Method{GaussNewton, LevenbergMarquardt};
+  enum ScaleEstimatorType{UnitScale, TDistScale, MADScale, NormalScale};
+  enum WeightFunctionType{UnitWeight, TDistWeight, TukeyWeight, HuberWeight};
+
+protected:
+  Matrix<double, D, D>  H_;       //!< Hessian approximation
+  Matrix<double, D, 1>  Jres_;    //!< Jacobian x Residual
+  Matrix<double, D, 1>  x_;       //!< update step
+  bool                  have_prior_;
+  ModelType prior_;
+  Matrix<double, D, D>  I_prior_; //!< Prior information matrix (inverse covariance)
+  double                chi2_;
+  double                rho_;
+  Method                method_;
+
+  /// If the flag linearize_system is set, the function must also compute the
+  /// Jacobian and set the member variables H_, Jres_
+  virtual double
+  computeResiduals      (const ModelType& model,
+                         bool linearize_system,
+                         bool compute_weight_scale) = 0;
+  virtual double
+  computeResiduals      (bool linearize_system,
+                           bool compute_weight_scale) = 0;
+
+  /// Solve the linear system H*x = Jres. This function must set the update
+  /// step in the member variable x_. Must return true if the system could be
+  /// solved and false if it was singular.
+  virtual int
+  solve                 () = 0;
+
+  virtual void
+  update                (const ModelType& old_model, ModelType& new_model) = 0;
+  virtual void
+  update                () = 0;
+
+  virtual void
+  applyPrior            (const ModelType& current_model) { }
+
+  virtual void
+  startIteration        () { }
+
+  virtual void
+  finishIteration       () { }
+
+  virtual void
+  finishTrial           () { }
+
+public:
+
+  /// Damping parameter. If mu > 0, coefficient matrix is positive definite, this
+  /// ensures that x is a descent direction. If mu is large, x is a short step in
+  /// the steepest direction. This is good if the current iterate is far from the
+  /// solution. If mu is small, LM approximates gauss newton iteration and we
+  /// have (almost) quadratic convergence in the final stages.
+  double                mu_init_, mu_;
+  double                nu_init_, nu_;          //!< Increase factor of mu after fail
+  size_t                n_iter_init_, n_iter_;  //!< Number of Iterations
+  size_t                n_trials_;              //!< Number of trials
+  size_t                n_trials_max_;          //!< Max number of trials
+  size_t                n_meas_;                //!< Number of measurements
+  bool                  stop_;                  //!< Stop flag
+  bool                  verbose_;               //!< Output Statistics
+  double                eps_;                   //!< Stop if update norm is smaller than eps
+  size_t                iter_;                  //!< Current Iteration
+
+  // robust least squares
+  bool                  use_weights_;
+  float                 scale_=2.0;
+  vk::robust_cost::ScaleEstimatorPtr scale_estimator_;
+  vk::robust_cost::WeightFunctionPtr weight_function_;
+
+  NLLSSolver() :
+    have_prior_(false),
+    method_(LevenbergMarquardt),
+    mu_init_(0.01f),
+    mu_(mu_init_),
+    nu_init_(2.0),
+    nu_(nu_init_),
+    n_iter_init_(15),
+    n_iter_(n_iter_init_),
+    n_trials_(0),
+    n_trials_max_(5),
+    n_meas_(0),
+    stop_(false),
+    verbose_(true),
+    eps_(0.0000000001),
+    iter_(0),
+    use_weights_(false),
+    scale_estimator_(NULL),
+    weight_function_(NULL)
+  { }
+
+  virtual ~NLLSSolver() {}
+
+  /// Calls the GaussNewton or LevenbergMarquardt optimization strategy
+  void optimize(ModelType& model);
+
+  /// Gauss Newton optimization strategy
+  void optimizeGaussNewton();
+
+  /// Levenberg Marquardt optimization strategy
+  void optimizeLevenbergMarquardt(ModelType& model);
+
+  /// Specify the robust cost that should be used and the appropriate scale estimator
+  void setRobustCostFunction(
+      ScaleEstimatorType scale_estimator,
+      WeightFunctionType weight_function);
+
+  /// Add prior to optimization.
+  void setPrior(
+      const ModelType&  prior,
+      const Matrix<double, D, D>&  Information);
+
+  /// Reset all parameters to restart the optimization
+  void reset();
+
+  /// Get the squared error
+  const double& getChi2() const;
+
+  /// The Information matrix is equal to the inverse covariance matrix.
+  const Matrix<double, D, D>& getInformationMatrix() const;
+};
+
+} // end namespace vk
+
+#include "nlls_solver_impl.hpp"
+
+#endif /* LM_SOLVER_H_ */
@@ -0,0 +1,315 @@
+/*
+ * Abstract Nonlinear Least-Squares Solver Class
+ *
+ * nlls_solver.h
+ *
+ *  Created on: Nov 5, 2012
+ *      Author: cforster
+ */
+
+#ifndef LM_SOLVER_IMPL_HPP_
+#define LM_SOLVER_IMPL_HPP_
+
+#include <stdexcept>
+
+template <int D, typename T>
+void vk::NLLSSolver<D, T>::optimize(ModelType& model)
+{
+  if(method_ == GaussNewton)
+      optimizeGaussNewton();
+  else if(method_ == LevenbergMarquardt)
+      optimizeLevenbergMarquardt(model);
+}
+
+template <int D, typename T>
+void vk::NLLSSolver<D, T>::optimizeGaussNewton()
+{
+  // Compute weight scale
+  if(use_weights_)
+    computeResiduals(false, true);
+  // perform iterative estimation
+  for (iter_ = 0; iter_<n_iter_; ++iter_)
+  {
+    rho_ = 0;
+    n_meas_ = 0;
+    double new_chi2 = computeResiduals(true, false);
+    // solve the linear system
+    if(!solve())
+    {
+      // matrix was singular and could not be computed
+      std::cout << "Matrix is close to singular! Stop Optimizing." << std::endl;
+      stop_ = true;
+    }
+    // check if error increased since last optimization
+    if((iter_ > 0 && new_chi2 > chi2_) || stop_)
+    {
+      if(verbose_)
+      {
+        std::cerr << "It. " << iter_
+                  << "\t Failure"
+                  << "\t new_chi2 = " << new_chi2
+                  << "\t Error increased. Stop optimizing.\n";
+      }
+      break;
+    }
+    // update the model
+    update();
+    chi2_ = new_chi2;
+    if(verbose_)
+    {
+      std::cerr << "It. " << iter_
+                << "\t Success"
+                << "\t new_chi2 = " << new_chi2
+                << "\t x_norm = " << vk::norm_max(x_)<<'\n';
+    }
+    // stop when converged, i.e. update step too small
+    if(vk::norm_max(x_)<=eps_)
+      break;
+  }
+}
+
+template <int D, typename T>
+void vk::NLLSSolver<D, T>::optimizeLevenbergMarquardt(ModelType& model)
+{
+  // Compute weight scale
+  if(use_weights_)
+    computeResiduals(model, false, true);
+
+  // compute the initial error
+  chi2_ = computeResiduals(model, true, false);
+
+  if(verbose_)
+    cout << "init chi2 = " << chi2_
+         << "\t n_meas = " << n_meas_
+         << endl;
+
+  // TODO: compute initial lambda
+  // Hartley and Zisserman: "A typical init value of lambda is 10^-3 times the
+  // average of the diagonal elements of J'J"
+
+  // Compute Initial Lambda
+  if(mu_ < 0)
+  {
+    double H_max_diag = 0;
+    double tau = 1e-4;
+    for(size_t j=0; j<D; ++j)
+      H_max_diag = max(H_max_diag, fabs(H_(j,j)));
+    mu_ = tau*H_max_diag;
+  }
+
+  // perform iterative estimation
+  for (iter_ = 0; iter_<n_iter_; ++iter_)
+  {
+    rho_ = 0;
+    startIteration();
+    bool sign=false;
+    // try to compute and update, if it fails, try with increased mu
+    n_trials_ = 0;
+    do
+    {
+      // init variables
+      ModelType new_model;
+      double new_chi2 = -1;
+      H_.setZero();
+      //H_ = mu_ * Matrix<double,D,D>::Identity(D,D);
+      Jres_.setZero();
+
+      // compute initial error
+      n_meas_ = 0;
+      computeResiduals(model, true, false);
+
+      // add damping term:
+      H_ += (H_.diagonal()*mu_).asDiagonal();
+
+      // add prior
+      if(have_prior_)
+        applyPrior(model);
+
+      // solve the linear system
+      if(solve())
+      {
+        // update the model
+        update(model, new_model);
+
+        // compute error with new model and compare to old error
+        n_meas_ = 0;
+        new_chi2 = computeResiduals(new_model, true, false);
+        rho_ = chi2_-new_chi2;
+      }
+      else
+      {
+        // matrix was singular and could not be computed
+        cout << "Matrix is close to singular!" << endl;
+        cout << "H = " << H_ << endl;
+        cout << "Jres = " << Jres_ << endl;
+        rho_ = -1;
+      }
+
+      if(rho_>0)
+      {
+        // update decrased the error -> success
+        model = new_model;
+        chi2_ = new_chi2;
+        stop_ = vk::norm_max(x_)<=eps_ ? true : false;
+        mu_ *= max(1./3., min(1.-pow(2*rho_-1,3), 2./3.));
+        nu_ = 2.;
+        if(verbose_)
+        {
+          cout << "It. " << iter_
+               << "\t Trial " << n_trials_
+               << "\t Success"
+               << "\t n_meas = " << n_meas_
+               << "\t new_chi2 = " << new_chi2
+               << "\t mu = " << mu_
+               << "\t nu = " << nu_
+               << "\t x_norm = " << vk::norm_max(x_)
+               << endl;
+        }
+      }
+      else
+      {
+        if(sign){
+            if(verbose_)
+            {
+                cout << "It. " << iter_
+                     << "\t Trial " << n_trials_
+                     << "\t Failure"
+                     << "\t n_meas = " << n_meas_
+                     << "\t new_chi2 = " << new_chi2
+                     << "\t mu = " << mu_
+                     << "\t nu = " << nu_
+                     << "\t x_norm = " << vk::norm_max(x_)
+                     << endl;
+            }
+            mu_ /= nu_;
+            nu_ /= 2.;
+            ++n_trials_;
+            if (n_trials_ >= n_trials_max_)
+                stop_ = true;
+            sign=false;
+        }else{
+            if(verbose_)
+            {
+                cout << "It. " << iter_
+                     << "\t Trial " << n_trials_
+                     << "\t Failure"
+                     << "\t n_meas = " << n_meas_
+                     << "\t new_chi2 = " << new_chi2
+                     << "\t mu = " << mu_
+                     << "\t nu = " << nu_
+                     << "\t x_norm = " << vk::norm_max(x_)
+                     << endl;
+            }
+            mu_ *= nu_;
+            nu_ *= 2.;
+            ++n_trials_;
+            if (n_trials_ >= n_trials_max_)
+                stop_ = true;
+            sign=true;
+        }
+
+      }
+
+      finishTrial();
+
+    } while(!(rho_>0 || stop_));
+    if (stop_)
+      break;
+
+    finishIteration();
+  }
+}
+
+
+template <int D, typename T>
+void vk::NLLSSolver<D, T>::setRobustCostFunction(
+    ScaleEstimatorType scale_estimator,
+    WeightFunctionType weight_function)
+{
+  switch(scale_estimator)
+  {
+    case TDistScale:
+      if(verbose_)
+        printf("Using TDistribution Scale Estimator\n");
+      scale_estimator_.reset(new robust_cost::TDistributionScaleEstimator());
+      use_weights_=true;
+      break;
+    case MADScale:
+      if(verbose_)
+        printf("Using MAD Scale Estimator\n");
+      scale_estimator_.reset(new robust_cost::MADScaleEstimator());
+      use_weights_=true;
+    break;
+    case NormalScale:
+      if(verbose_)
+        printf("Using Normal Scale Estimator\n");
+      scale_estimator_.reset(new robust_cost::NormalDistributionScaleEstimator());
+      use_weights_=true;
+      break;
+    default:
+      if(verbose_)
+        printf("Using Unit Scale Estimator\n");
+      scale_estimator_.reset(new robust_cost::UnitScaleEstimator());
+      use_weights_=false;
+  }
+
+  switch(weight_function)
+  {
+    case TDistWeight:
+      if(verbose_)
+        printf("Using TDistribution Weight Function\n");
+      weight_function_.reset(new robust_cost::TDistributionWeightFunction());
+      break;
+    case TukeyWeight:
+      if(verbose_)
+        printf("Using Tukey Weight Function\n");
+      weight_function_.reset(new robust_cost::TukeyWeightFunction());
+      break;
+    case HuberWeight:
+      if(verbose_)
+        printf("Using Huber Weight Function\n");
+      weight_function_.reset(new robust_cost::HuberWeightFunction());
+      break;
+    default:
+      if(verbose_)
+        printf("Using Unit Weight Function\n");
+      weight_function_.reset(new robust_cost::UnitWeightFunction());
+  }
+}
+
+template <int D, typename T>
+void vk::NLLSSolver<D, T>::setPrior(
+    const T&  prior,
+    const Matrix<double, D, D>&  Information)
+{
+  have_prior_ = true;
+  prior_ = prior;
+  I_prior_ = Information;
+}
+
+template <int D, typename T>
+void vk::NLLSSolver<D, T>::reset()
+{
+  have_prior_ = false;
+  chi2_ = 1e10;
+  mu_ = mu_init_;
+  nu_ = nu_init_;
+  n_meas_ = 0;
+  n_iter_ = n_iter_init_;
+  iter_ = 0;
+  stop_ = false;
+}
+
+template <int D, typename T>
+inline const double& vk::NLLSSolver<D, T>::getChi2() const
+{
+  return chi2_;
+}
+
+template <int D, typename T>
+inline const vk::Matrix<double, D, D>& vk::NLLSSolver<D, T>::getInformationMatrix() const
+{
+  return H_;
+}
+
+#endif /* LM_SOLVER_IMPL_HPP_ */
@@ -0,0 +1,156 @@
+/**
+* This file is part of dvo.
+*
+* Copyright 2012 Christian Kerl <[email protected]> (Technical University of Munich)
+* For more information see <http://vision.in.tum.de/data/software/dvo>.
+*
+* dvo is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* dvo is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with dvo. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef VIKIT_ROBUST_COST_H_
+#define VIKIT_ROBUST_COST_H_
+
+#include <vector>
+#include <stdlib.h>
+#include <memory>
+namespace vk {
+namespace robust_cost {
+
+// interface for scale estimators
+class ScaleEstimator
+{
+public:
+  virtual ~ScaleEstimator() {};
+  virtual float compute(std::vector<float>& errors) const = 0;
+};
+typedef std::shared_ptr<ScaleEstimator> ScaleEstimatorPtr;
+
+class UnitScaleEstimator : public ScaleEstimator
+{
+public:
+  UnitScaleEstimator() {}
+  virtual ~UnitScaleEstimator() {}
+  virtual float compute(std::vector<float>& errors) const { return 1.0f; };
+};
+
+// estimates scale by fitting a t-distribution to the data with the given degrees of freedom
+class TDistributionScaleEstimator : public ScaleEstimator
+{
+public:
+  TDistributionScaleEstimator(const float dof = DEFAULT_DOF);
+  virtual ~TDistributionScaleEstimator() {};
+  virtual float compute(std::vector<float>& errors) const;
+
+  static const float DEFAULT_DOF;
+  static const float INITIAL_SIGMA;
+protected:
+  float dof_;
+  float initial_sigma_;
+};
+
+// estimates scale by computing the median absolute deviation
+class MADScaleEstimator : public ScaleEstimator
+{
+public:
+  MADScaleEstimator() {};
+  virtual ~MADScaleEstimator() {};
+  virtual float compute(std::vector<float>& errors) const;
+
+private:
+  static const float NORMALIZER;;
+};
+
+// estimates scale by computing the standard deviation
+class NormalDistributionScaleEstimator : public ScaleEstimator
+{
+public:
+  NormalDistributionScaleEstimator() {};
+  virtual ~NormalDistributionScaleEstimator() {};
+  virtual float compute(std::vector<float>& errors) const;
+private:
+};
+
+/**
+ * Interface for weight functions. A weight function is the first derivative of a symmetric robust function p(sqrt(t)).
+ * The errors are assumed to be normalized to unit variance.
+ *
+ * See:
+ *   "Lucas-Kanade 20 Years On: A Unifying Framework: Part 2" - Page 23, Equation (54)
+ */
+class WeightFunction
+{
+public:
+  virtual ~WeightFunction() {};
+  virtual float value(const float& x) const = 0;
+  virtual void configure(const float& param) {};
+};
+typedef std::shared_ptr<WeightFunction> WeightFunctionPtr;
+
+class UnitWeightFunction : public WeightFunction
+{
+public:
+  UnitWeightFunction() {};
+  virtual ~UnitWeightFunction() {};
+  virtual float value(const float& x) const { return 1.0f; };
+};
+
+/**
+ * Tukey's hard re-descending function.
+ *
+ * See:
+ *   http://en.wikipedia.org/wiki/Redescending_M-estimator
+ */
+class TukeyWeightFunction : public WeightFunction
+{
+public:
+  TukeyWeightFunction(const float b = DEFAULT_B);
+  virtual ~TukeyWeightFunction() {};
+  virtual float value(const float& x) const;
+  virtual void configure(const float& param);
+
+  static const float DEFAULT_B;
+private:
+  float b_square;
+};
+
+class TDistributionWeightFunction : public WeightFunction
+{
+public:
+  TDistributionWeightFunction(const float dof = DEFAULT_DOF);
+  virtual ~TDistributionWeightFunction() {};
+  virtual float value(const float& x) const;
+  virtual void configure(const float& param);
+
+  static const float DEFAULT_DOF;
+private:
+  float dof_;
+  float normalizer_;
+};
+
+class HuberWeightFunction : public WeightFunction
+{
+public:
+  HuberWeightFunction(const float k = DEFAULT_K);
+  virtual ~HuberWeightFunction() {};
+  virtual float value(const float& x) const;
+  virtual void configure(const float& param);
+
+  static const float DEFAULT_K;
+private:
+  float k;
+};
+
+} // namespace robust_cost
+} // namespace vk
+#endif // VIKIT_ROBUST_COST_H_
@@ -0,0 +1,75 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef VIO_SPARSE_IMG_ALIGN_GPU_H_
+#define VIO_SPARSE_IMG_ALIGN_GPU_H_
+
+#include <nlls_solver.h>
+#include <cl_class.h>
+#include <math_utils.h>
+#include <algorithm>
+#include <sophus/se2.h>
+
+namespace vio {
+
+class Feature;
+
+/// Optimize the pose of the frame by minimizing the photometric error of feature patches.
+class SparseImgAlignGpu : public vk::NLLSSolver<3, Sophus::SE2>
+{
+  static const int patch_halfsize_ = 12;
+  static const int patch_size_ = 12*patch_halfsize_;
+  static const int patch_area_ = patch_size_*patch_size_;
+public:
+
+  SparseImgAlignGpu(
+      int max_level,
+      int min_level,
+      int n_iter,
+      Method method,
+      bool verbose,
+      opencl* residual);
+
+  size_t run();
+  void data_reader(std::string filename, float *rp, float *cp, float *c, float *fts, float* fts_px);
+  int feature_counter(std::string filename);
+   // FILE* data= nullptr;
+
+protected:
+  int level_;                     //!< current pyramid level on which the optimization runs.
+  int max_level_;                 //!< coarsest pyramid level for the alignment.
+  int min_level_;                 //!< finest pyramid level for the alignment.
+  size_t feature_counter_=0;
+  opencl* residual_= nullptr;
+
+  // cache:
+  std::vector<bool> visible_fts_;
+  virtual double computeResiduals(const ModelType& model,
+                           bool linearize_system,
+                           bool compute_weight_scale) {
+        return 0.0;
+    };
+  virtual double computeResiduals(bool linearize_system, bool compute_weight_scale = false);
+  virtual int solve();
+  virtual void update();
+  virtual void update(const ModelType& old_model, ModelType& new_model) { };
+  virtual void startIteration();
+  virtual void finishIteration();
+};
+
+} // namespace vio
+
+#endif // VIO_SPARSE_IMG_ALIGN_GPU_H_
@@ -0,0 +1,80 @@
+/*
+ * vision.h
+ *
+ *  Created on: May 14, 2013
+ *      Author: cforster
+ */
+
+#ifndef VIKIT_VISION_H_
+#define VIKIT_VISION_H_
+
+#include <aligned_mem.h>
+#include <Eigen/Core>
+#include <opencv2/opencv.hpp>
+
+namespace vk
+{
+
+//! Return value between 0 and 1
+//! WARNING This function does not check whether the x/y is within the border
+inline float
+interpolateMat_32f(const cv::Mat& mat, float u, float v)
+{
+  assert(mat.type()==CV_32F);
+  float x = floor(u);
+  float y = floor(v);
+  float subpix_x = u-x;
+  float subpix_y = v-y;
+  float wx0 = 1.0-subpix_x;
+  float wx1 =  subpix_x;
+  float wy0 = 1.0-subpix_y;
+  float wy1 =  subpix_y;
+
+  float val00 = mat.at<float>(y,x);
+  float val10 = mat.at<float>(y,x+1);
+  float val01 = mat.at<float>(y+1,x);
+  float val11 = mat.at<float>(y+1,x+1);
+  return (wx0*wy0)*val00 + (wx1*wy0)*val10 + (wx0*wy1)*val01 + (wx1*wy1)*val11;
+}
+
+//! Return value between 0 and 255
+//! WARNING This function does not check whether the x/y is within the border
+inline float
+interpolateMat_8u(const cv::Mat& mat, float u, float v)
+{
+  if(mat.empty())return 0.0;
+  if(mat.type()!=CV_8U)return 0.0;
+  int x = floor(u);
+  int y = floor(v);
+  float subpix_x = u-x;
+  float subpix_y = v-y;
+
+  float w00 = (1.0f-subpix_x)*(1.0f-subpix_y);
+  float w01 = (1.0f-subpix_x)*subpix_y;
+  float w10 = subpix_x*(1.0f-subpix_y);
+  float w11 = 1.0f - w00 - w01 - w10;
+
+  const int stride = mat.step.p[0];
+  unsigned char* ptr = mat.data + y*stride + x;
+  if(isnan(w00) || isnan(w01 )||isnan( w10) || isnan(w11) || isinf(w00) || isinf(w01)|| isinf(w10)|| isinf(w11))return 0.0;
+  if(ptr[0] == NULL || ptr[1]== NULL || ptr[stride]==NULL ||ptr[stride+1]==NULL)return 0.0;
+  return w00*ptr[0] + w01*ptr[stride] + w10*ptr[1] + w11*ptr[stride+1];
+}
+
+void halfSample(const cv::Mat& in, cv::Mat& out);
+
+float shiTomasiScore(const cv::Mat& img, int u, int v);
+
+void calcSharrDeriv(const cv::Mat& src, cv::Mat& dst);
+
+#ifdef __SSE2__
+
+/// Used to convert a Kinect depthmap
+/// Code by Christian Kerl DVO, GPL Licence
+void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale);
+
+#endif
+
+} // namespace vk
+
+#endif // VIKIT_VISION_H_
@@ -0,0 +1,844 @@
+//  freak.cpp
+//
+//  Copyright (C) 2011-2012  Signal processing laboratory 2, EPFL,
+//  Kirell Benzi ([email protected]),
+//  Raphael Ortiz ([email protected])
+//  Alexandre Alahi ([email protected])
+//  and Pierre Vandergheynst ([email protected])
+//
+//  Redistribution and use in source and binary forms, with or without modification,
+//  are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+//  This software is provided by the copyright holders and contributors "as is" and
+//  any express or implied warranties, including, but not limited to, the implied
+//  warranties of merchantability and fitness for a particular purpose are disclaimed.
+//  In no event shall the Intel Corporation or contributors be liable for any direct,
+//  indirect, incidental, special, exemplary, or consequential damages
+//  (including, but not limited to, procurement of substitute goods or services;
+//  loss of use, data, or profits; or business interruption) however caused
+//  and on any theory of liability, whether in contract, strict liability,
+//  or tort (including negligence or otherwise) arising in any way out of
+//  the use of this software, even if advised of the possibility of such damage.
+
+#include "precomp.hpp"
+#include <fstream>
+#include <stdlib.h>
+#include <algorithm>
+#include <iostream>
+#include <bitset>
+#include <sstream>
+#include <algorithm>
+#include <iomanip>
+#include <string.h>
+
+namespace cv
+{
+namespace xfeatures2d
+{
+
+/*!
+ FREAK implementation
+ */
+class FREAK_Impl CV_FINAL : public FREAK
+{
+public:
+    /** Constructor
+     * @param orientationNormalized enable orientation normalization
+     * @param scaleNormalized enable scale normalization
+     * @param patternScale scaling of the description pattern
+     * @param nbOctave number of octaves covered by the detected keypoints
+     * @param selectedPairs (optional) user defined selected pairs
+     */
+    explicit FREAK_Impl( bool orientationNormalized = true,
+                   bool scaleNormalized = true,
+                   float patternScale = 22.0f,
+                   int nOctaves = 4,
+                   const std::vector<int>& selectedPairs = std::vector<int>());
+
+    virtual ~FREAK_Impl();
+
+    /** returns the descriptor length in bytes */
+    virtual int descriptorSize() const CV_OVERRIDE;
+
+    /** returns the descriptor type */
+    virtual int descriptorType() const CV_OVERRIDE;
+
+    /** returns the default norm type */
+    virtual int defaultNorm() const CV_OVERRIDE;
+
+    /** select the 512 "best description pairs"
+     * @param images grayscale images set
+     * @param keypoints set of detected keypoints
+     * @param corrThresh correlation threshold
+     * @param verbose print construction information
+     * @return list of best pair indexes
+     */
+    std::vector<int> selectPairs( const std::vector<Mat>& images, std::vector<std::vector<KeyPoint> >& keypoints,
+                                 const double corrThresh = 0.7, bool verbose = true );
+    virtual void compute( InputArray image, std::vector<KeyPoint>& keypoints, OutputArray descriptors ) CV_OVERRIDE;
+
+protected:
+
+    void buildPattern();
+
+    template <typename imgType, typename iiType>
+    imgType meanIntensity( InputArray image, InputArray integral, const float kp_x, const float kp_y,
+                          const unsigned int scale, const unsigned int rot, const unsigned int point );
+
+    template <typename srcMatType, typename iiMatType>
+    void computeDescriptors( InputArray image, std::vector<KeyPoint>& keypoints, OutputArray descriptors );
+
+    template <typename srcMatType>
+    void extractDescriptor(srcMatType *pointsValue, void ** ptr);
+
+    bool orientationNormalized; //true if the orientation is normalized, false otherwise
+    bool scaleNormalized; //true if the scale is normalized, false otherwise
+    double patternScale; //scaling of the pattern
+    int nOctaves; //number of octaves
+    bool extAll; // true if all pairs need to be extracted for pairs selection
+
+    double patternScale0;
+    int nOctaves0;
+    std::vector<int> selectedPairs0;
+
+    struct PatternPoint
+    {
+        float x; // x coordinate relative to center
+        float y; // x coordinate relative to center
+        float sigma; // Gaussian smoothing sigma
+    };
+
+    struct DescriptionPair
+    {
+        uchar i; // index of the first point
+        uchar j; // index of the second point
+    };
+
+    struct OrientationPair
+    {
+        uchar i; // index of the first point
+        uchar j; // index of the second point
+        int weight_dx; // dx/(norm_sq))*4096
+        int weight_dy; // dy/(norm_sq))*4096
+    };
+
+    std::vector<PatternPoint> patternLookup; // look-up table for the pattern points (position+sigma of all points at all scales and orientation)
+    int patternSizes[NB_SCALES]; // size of the pattern at a specific scale (used to check if a point is within image boundaries)
+    DescriptionPair descriptionPairs[NB_PAIRS];
+    OrientationPair orientationPairs[NB_ORIENPAIRS];
+};
+
+static const double FREAK_LOG2 = 0.693147180559945;
+static const int FREAK_NB_ORIENTATION = 256;
+static const int FREAK_NB_POINTS = 43;
+static const int FREAK_SMALLEST_KP_SIZE = 7; // smallest size of keypoints
+
+// default pairs
+static const int FREAK_DEF_PAIRS[FREAK_Impl::NB_PAIRS] =
+{
+     404,431,818,511,181,52,311,874,774,543,719,230,417,205,11,
+     560,149,265,39,306,165,857,250,8,61,15,55,717,44,412,
+     592,134,761,695,660,782,625,487,549,516,271,665,762,392,178,
+     796,773,31,672,845,548,794,677,654,241,831,225,238,849,83,
+     691,484,826,707,122,517,583,731,328,339,571,475,394,472,580,
+     381,137,93,380,327,619,729,808,218,213,459,141,806,341,95,
+     382,568,124,750,193,749,706,843,79,199,317,329,768,198,100,
+     466,613,78,562,783,689,136,838,94,142,164,679,219,419,366,
+     418,423,77,89,523,259,683,312,555,20,470,684,123,458,453,833,
+     72,113,253,108,313,25,153,648,411,607,618,128,305,232,301,84,
+     56,264,371,46,407,360,38,99,176,710,114,578,66,372,653,
+     129,359,424,159,821,10,323,393,5,340,891,9,790,47,0,175,346,
+     236,26,172,147,574,561,32,294,429,724,755,398,787,288,299,
+     769,565,767,722,757,224,465,723,498,467,235,127,802,446,233,
+     544,482,800,318,16,532,801,441,554,173,60,530,713,469,30,
+     212,630,899,170,266,799,88,49,512,399,23,500,107,524,90,
+     194,143,135,192,206,345,148,71,119,101,563,870,158,254,214,
+     276,464,332,725,188,385,24,476,40,231,620,171,258,67,109,
+     844,244,187,388,701,690,50,7,850,479,48,522,22,154,12,659,
+     736,655,577,737,830,811,174,21,237,335,353,234,53,270,62,
+     182,45,177,245,812,673,355,556,612,166,204,54,248,365,226,
+     242,452,700,685,573,14,842,481,468,781,564,416,179,405,35,
+     819,608,624,367,98,643,448,2,460,676,440,240,130,146,184,
+     185,430,65,807,377,82,121,708,239,310,138,596,730,575,477,
+     851,797,247,27,85,586,307,779,326,494,856,324,827,96,748,
+     13,397,125,688,702,92,293,716,277,140,112,4,80,855,839,1,
+     413,347,584,493,289,696,19,751,379,76,73,115,6,590,183,734,
+     197,483,217,344,330,400,186,243,587,220,780,200,793,246,824,
+     41,735,579,81,703,322,760,720,139,480,490,91,814,813,163,
+     152,488,763,263,425,410,576,120,319,668,150,160,302,491,515,
+     260,145,428,97,251,395,272,252,18,106,358,854,485,144,550,
+     131,133,378,68,102,104,58,361,275,209,697,582,338,742,589,
+     325,408,229,28,304,191,189,110,126,486,211,547,533,70,215,
+     670,249,36,581,389,605,331,518,442,822
+};
+
+// used to sort pairs during pairs selection
+struct PairStat
+{
+    double mean;
+    int idx;
+};
+
+struct sortMean
+{
+    bool operator()( const PairStat& a, const PairStat& b ) const
+    {
+        return a.mean < b.mean;
+    }
+};
+
+void FREAK_Impl::buildPattern()
+{
+    if( patternScale == patternScale0 && nOctaves == nOctaves0 && !patternLookup.empty() )
+        return;
+
+    nOctaves0 = nOctaves;
+    patternScale0 = patternScale;
+
+patternLookup.resize(FREAK::NB_SCALES*FREAK_NB_ORIENTATION*FREAK_NB_POINTS);
+    double scaleStep = std::pow(2.0, (double)(nOctaves)/FREAK::NB_SCALES ); // 2 ^ ( (nOctaves-1) /nbScales)
+    double scalingFactor, alpha, beta, theta = 0;
+
+    // pattern definition, radius normalized to 1.0 (outer point position+sigma=1.0)
+    const int n[8] = {6,6,6,6,6,6,6,1}; // number of points on each concentric circle (from outer to inner)
+    const double bigR(2.0/3.0); // bigger radius
+    const double smallR(2.0/24.0); // smaller radius
+    const double unitSpace( (bigR-smallR)/21.0 ); // define spaces between concentric circles (from center to outer: 1,2,3,4,5,6)
+    // radii of the concentric cirles (from outer to inner)
+    const double radius[8] = {bigR, bigR-6*unitSpace, bigR-11*unitSpace, bigR-15*unitSpace, bigR-18*unitSpace, bigR-20*unitSpace, smallR, 0.0};
+    // sigma of pattern points (each group of 6 points on a concentric cirle has the same sigma)
+    const double sigma[8] = {radius[0]/2.0, radius[1]/2.0, radius[2]/2.0,
+                             radius[3]/2.0, radius[4]/2.0, radius[5]/2.0,
+                             radius[6]/2.0, radius[6]/2.0
+                            };
+    // fill the lookup table
+    for( int scaleIdx=0; scaleIdx < FREAK::NB_SCALES; ++scaleIdx )
+    {
+        patternSizes[scaleIdx] = 0; // proper initialization
+        scalingFactor = std::pow(scaleStep,scaleIdx); //scale of the pattern, scaleStep ^ scaleIdx
+
+        for( int orientationIdx = 0; orientationIdx < FREAK_NB_ORIENTATION; ++orientationIdx )
+        {
+            theta = double(orientationIdx)* 2*CV_PI/double(FREAK_NB_ORIENTATION); // orientation of the pattern
+            int pointIdx = 0;
+
+            PatternPoint* patternLookupPtr = &patternLookup[0];
+            for( size_t i = 0; i < 8; ++i )
+            {
+                for( int k = 0 ; k < n[i]; ++k )
+                {
+                    beta = CV_PI/n[i] * (i%2); // orientation offset so that groups of points on each circles are staggered
+                    alpha = double(k)* 2*CV_PI/double(n[i])+beta+theta;
+
+                    // add the point to the look-up table
+                    PatternPoint& point = patternLookupPtr[ scaleIdx*FREAK_NB_ORIENTATION*FREAK_NB_POINTS+orientationIdx*FREAK_NB_POINTS+pointIdx ];
+                    point.x = static_cast<float>(radius[i] * cos(alpha) * scalingFactor * patternScale);
+                    point.y = static_cast<float>(radius[i] * sin(alpha) * scalingFactor * patternScale);
+                    point.sigma = static_cast<float>(sigma[i] * scalingFactor * patternScale);
+
+                    // adapt the sizeList if necessary
+                    const int sizeMax = static_cast<int>(ceil((radius[i]+sigma[i])*scalingFactor*patternScale)) + 1;
+                    if( patternSizes[scaleIdx] < sizeMax )
+                        patternSizes[scaleIdx] = sizeMax;
+
+                    ++pointIdx;
+                }
+            }
+        }
+    }
+
+    // build the list of orientation pairs
+    orientationPairs[0].i=0; orientationPairs[0].j=3; orientationPairs[1].i=1; orientationPairs[1].j=4; orientationPairs[2].i=2; orientationPairs[2].j=5;
+    orientationPairs[3].i=0; orientationPairs[3].j=2; orientationPairs[4].i=1; orientationPairs[4].j=3; orientationPairs[5].i=2; orientationPairs[5].j=4;
+    orientationPairs[6].i=3; orientationPairs[6].j=5; orientationPairs[7].i=4; orientationPairs[7].j=0; orientationPairs[8].i=5; orientationPairs[8].j=1;
+
+    orientationPairs[9].i=6; orientationPairs[9].j=9; orientationPairs[10].i=7; orientationPairs[10].j=10; orientationPairs[11].i=8; orientationPairs[11].j=11;
+    orientationPairs[12].i=6; orientationPairs[12].j=8; orientationPairs[13].i=7; orientationPairs[13].j=9; orientationPairs[14].i=8; orientationPairs[14].j=10;
+    orientationPairs[15].i=9; orientationPairs[15].j=11; orientationPairs[16].i=10; orientationPairs[16].j=6; orientationPairs[17].i=11; orientationPairs[17].j=7;
+
+    orientationPairs[18].i=12; orientationPairs[18].j=15; orientationPairs[19].i=13; orientationPairs[19].j=16; orientationPairs[20].i=14; orientationPairs[20].j=17;
+    orientationPairs[21].i=12; orientationPairs[21].j=14; orientationPairs[22].i=13; orientationPairs[22].j=15; orientationPairs[23].i=14; orientationPairs[23].j=16;
+    orientationPairs[24].i=15; orientationPairs[24].j=17; orientationPairs[25].i=16; orientationPairs[25].j=12; orientationPairs[26].i=17; orientationPairs[26].j=13;
+
+    orientationPairs[27].i=18; orientationPairs[27].j=21; orientationPairs[28].i=19; orientationPairs[28].j=22; orientationPairs[29].i=20; orientationPairs[29].j=23;
+    orientationPairs[30].i=18; orientationPairs[30].j=20; orientationPairs[31].i=19; orientationPairs[31].j=21; orientationPairs[32].i=20; orientationPairs[32].j=22;
+    orientationPairs[33].i=21; orientationPairs[33].j=23; orientationPairs[34].i=22; orientationPairs[34].j=18; orientationPairs[35].i=23; orientationPairs[35].j=19;
+
+    orientationPairs[36].i=24; orientationPairs[36].j=27; orientationPairs[37].i=25; orientationPairs[37].j=28; orientationPairs[38].i=26; orientationPairs[38].j=29;
+    orientationPairs[39].i=30; orientationPairs[39].j=33; orientationPairs[40].i=31; orientationPairs[40].j=34; orientationPairs[41].i=32; orientationPairs[41].j=35;
+    orientationPairs[42].i=36; orientationPairs[42].j=39; orientationPairs[43].i=37; orientationPairs[43].j=40; orientationPairs[44].i=38; orientationPairs[44].j=41;
+
+    for( unsigned m = FREAK::NB_ORIENPAIRS; m--; )
+    {
+        const float dx = patternLookup[orientationPairs[m].i].x-patternLookup[orientationPairs[m].j].x;
+        const float dy = patternLookup[orientationPairs[m].i].y-patternLookup[orientationPairs[m].j].y;
+        const float norm_sq = (dx*dx+dy*dy);
+        orientationPairs[m].weight_dx = cvRound((dx/(norm_sq))*4096.0);
+        orientationPairs[m].weight_dy = cvRound((dy/(norm_sq))*4096.0);
+    }
+
+    // build the list of description pairs
+    std::vector<DescriptionPair> allPairs;
+    for( unsigned int i = 1; i < (unsigned int)FREAK_NB_POINTS; ++i )
+    {
+        // (generate all the pairs)
+        for( unsigned int j = 0; (unsigned int)j < i; ++j )
+        {
+            DescriptionPair pair = {(uchar)i,(uchar)j};
+            allPairs.push_back(pair);
+        }
+    }
+    // Input vector provided
+    if( !selectedPairs0.empty() )
+    {
+        if( (int)selectedPairs0.size() == FREAK::NB_PAIRS )
+        {
+            for( int i = 0; i < FREAK::NB_PAIRS; ++i )
+                 descriptionPairs[i] = allPairs[selectedPairs0.at(i)];
+        }
+        else
+        {
+            CV_Error(Error::StsVecLengthErr, "Input vector does not match the required size");
+        }
+    }
+    else // default selected pairs
+    {
+        for( int i = 0; i < FREAK::NB_PAIRS; ++i )
+             descriptionPairs[i] = allPairs[FREAK_DEF_PAIRS[i]];
+    }
+}
+
+void FREAK_Impl::compute( InputArray _image, std::vector<KeyPoint>& keypoints, OutputArray _descriptors )
+{
+    Mat image = _image.getMat();
+    if( image.empty() )
+        return;
+    if( keypoints.empty() )
+        return;
+
+    ((FREAK_Impl*)this)->buildPattern();
+
+    // Convert to gray if not already
+    Mat grayImage;
+    if( image.channels() == 3 || image.channels() == 4 )
+        cvtColor(image, grayImage, COLOR_BGR2GRAY);
+    else {
+        CV_Assert(image.channels() == 1);
+        grayImage = image;
+    }
+
+    // Use 32-bit integers if we won't overflow in the integral image
+    if ((image.depth() == CV_8U || image.depth() == CV_8S) &&
+        (image.rows * image.cols) < 8388608 ) // 8388608 = 2 ^ (32 - 8(bit depth) - 1(sign bit))
+    {
+        // Create the integral image appropriate for our type & usage
+        if (image.depth() == CV_8U)
+            computeDescriptors<uchar, int>(grayImage, keypoints, _descriptors);
+        else if (image.depth() == CV_8S)
+            computeDescriptors<char, int>(grayImage, keypoints, _descriptors);
+        else
+            CV_Error( Error::StsUnsupportedFormat, "" );
+    } else {
+        // Create the integral image appropriate for our type & usage
+        if ( image.depth() == CV_8U )
+            computeDescriptors<uchar, double>(grayImage, keypoints, _descriptors);
+        else if ( image.depth() == CV_8S )
+            computeDescriptors<char, double>(grayImage, keypoints, _descriptors);
+        else if ( image.depth() == CV_16U )
+            computeDescriptors<ushort, double>(grayImage, keypoints, _descriptors);
+        else if ( image.depth() == CV_16S )
+            computeDescriptors<short, double>(grayImage, keypoints, _descriptors);
+        else
+            CV_Error( Error::StsUnsupportedFormat, "" );
+    }
+}
+
+template <typename srcMatType>
+void FREAK_Impl::extractDescriptor(srcMatType *pointsValue, void ** ptr)
+{
+    std::bitset<FREAK::NB_PAIRS>** ptrScalar = (std::bitset<FREAK::NB_PAIRS>**) ptr;
+
+    // extracting descriptor preserving the order of SSE version
+    int cnt = 0;
+    for( int n = 7; n < FREAK::NB_PAIRS; n += 128)
+    {
+        for( int m = 8; m--; )
+        {
+            int nm = n-m;
+            for(int kk = nm+15*8; kk >= nm; kk-=8, ++cnt)
+            {
+                (*ptrScalar)->set(kk, pointsValue[descriptionPairs[cnt].i] >= pointsValue[descriptionPairs[cnt].j]);
+            }
+        }
+    }
+    --(*ptrScalar);
+}
+
+#if CV_SSE2
+template <>
+void FREAK_Impl::extractDescriptor(uchar *pointsValue, void ** ptr)
+{
+    __m128i** ptrSSE = (__m128i**) ptr;
+
+    // note that comparisons order is modified in each block (but first 128 comparisons remain globally the same-->does not affect the 128,384 bits segmanted matching strategy)
+    int cnt = 0;
+    for( int n = FREAK::NB_PAIRS/128; n-- ; )
+    {
+        __m128i result128 = _mm_setzero_si128();
+        for( int m = 128/16; m--; cnt += 16 )
+        {
+            __m128i operand1 = _mm_set_epi8(pointsValue[descriptionPairs[cnt+0].i],
+                                            pointsValue[descriptionPairs[cnt+1].i],
+                                            pointsValue[descriptionPairs[cnt+2].i],
+                                            pointsValue[descriptionPairs[cnt+3].i],
+                                            pointsValue[descriptionPairs[cnt+4].i],
+                                            pointsValue[descriptionPairs[cnt+5].i],
+                                            pointsValue[descriptionPairs[cnt+6].i],
+                                            pointsValue[descriptionPairs[cnt+7].i],
+                                            pointsValue[descriptionPairs[cnt+8].i],
+                                            pointsValue[descriptionPairs[cnt+9].i],
+                                            pointsValue[descriptionPairs[cnt+10].i],
+                                            pointsValue[descriptionPairs[cnt+11].i],
+                                            pointsValue[descriptionPairs[cnt+12].i],
+                                            pointsValue[descriptionPairs[cnt+13].i],
+                                            pointsValue[descriptionPairs[cnt+14].i],
+                                            pointsValue[descriptionPairs[cnt+15].i]);
+
+            __m128i operand2 = _mm_set_epi8(pointsValue[descriptionPairs[cnt+0].j],
+                                            pointsValue[descriptionPairs[cnt+1].j],
+                                            pointsValue[descriptionPairs[cnt+2].j],
+                                            pointsValue[descriptionPairs[cnt+3].j],
+                                            pointsValue[descriptionPairs[cnt+4].j],
+                                            pointsValue[descriptionPairs[cnt+5].j],
+                                            pointsValue[descriptionPairs[cnt+6].j],
+                                            pointsValue[descriptionPairs[cnt+7].j],
+                                            pointsValue[descriptionPairs[cnt+8].j],
+                                            pointsValue[descriptionPairs[cnt+9].j],
+                                            pointsValue[descriptionPairs[cnt+10].j],
+                                            pointsValue[descriptionPairs[cnt+11].j],
+                                            pointsValue[descriptionPairs[cnt+12].j],
+                                            pointsValue[descriptionPairs[cnt+13].j],
+                                            pointsValue[descriptionPairs[cnt+14].j],
+                                            pointsValue[descriptionPairs[cnt+15].j]);
+
+            __m128i workReg = _mm_min_epu8(operand1, operand2); // emulated "not less than" for 8-bit UNSIGNED integers
+            workReg = _mm_cmpeq_epi8(workReg, operand2);        // emulated "not less than" for 8-bit UNSIGNED integers
+
+            workReg = _mm_and_si128(_mm_set1_epi16(short(0x8080 >> m)), workReg); // merge the last 16 bits with the 128bits std::vector until full
+            result128 = _mm_or_si128(result128, workReg);
+        }
+        (**ptrSSE) = result128;
+        ++(*ptrSSE);
+    }
+    (*ptrSSE) -= 8;
+}
+#endif
+
+template <typename srcMatType, typename iiMatType>
+void FREAK_Impl::computeDescriptors( InputArray _image, std::vector<KeyPoint>& keypoints, OutputArray _descriptors ){
+
+    Mat image = _image.getMat();
+    Mat imgIntegral;
+    integral(image, imgIntegral, DataType<iiMatType>::type);
+    std::vector<int> kpScaleIdx(keypoints.size()); // used to save pattern scale index corresponding to each keypoints
+    const std::vector<int>::iterator ScaleIdxBegin = kpScaleIdx.begin(); // used in std::vector erase function
+    const std::vector<cv::KeyPoint>::iterator kpBegin = keypoints.begin(); // used in std::vector erase function
+    const float sizeCst = static_cast<float>(FREAK::NB_SCALES/(FREAK_LOG2* nOctaves));
+    srcMatType pointsValue[FREAK_NB_POINTS];
+    int thetaIdx = 0;
+    int direction0;
+    int direction1;
+
+    // compute the scale index corresponding to the keypoint size and remove keypoints close to the border
+    if( scaleNormalized )
+    {
+        for( size_t k = keypoints.size(); k--; )
+        {
+            //Is k non-zero? If so, decrement it and continue"
+            kpScaleIdx[k] = std::max( (int)(std::log(keypoints[k].size/FREAK_SMALLEST_KP_SIZE)*sizeCst+0.5) ,0);
+            if( kpScaleIdx[k] >= FREAK::NB_SCALES )
+                kpScaleIdx[k] = FREAK::NB_SCALES-1;
+
+            if( keypoints[k].pt.x <= patternSizes[kpScaleIdx[k]] || //check if the description at this specific position and scale fits inside the image
+                 keypoints[k].pt.y <= patternSizes[kpScaleIdx[k]] ||
+                 keypoints[k].pt.x >= image.cols-patternSizes[kpScaleIdx[k]] ||
+                 keypoints[k].pt.y >= image.rows-patternSizes[kpScaleIdx[k]]
+               )
+            {
+                keypoints.erase(kpBegin+k);
+                kpScaleIdx.erase(ScaleIdxBegin+k);
+            }
+        }
+    }
+    else
+    {
+        const int scIdx = std::max( cvRound(1.0986122886681*sizeCst) ,0);
+        for( size_t k = keypoints.size(); k--; )
+        {
+            kpScaleIdx[k] = scIdx; // equivalent to the formule when the scale is normalized with a constant size of keypoints[k].size=3*SMALLEST_KP_SIZE
+            if( kpScaleIdx[k] >= FREAK::NB_SCALES )
+            {
+                kpScaleIdx[k] = FREAK::NB_SCALES-1;
+            }
+            if( keypoints[k].pt.x <= patternSizes[kpScaleIdx[k]] ||
+                keypoints[k].pt.y <= patternSizes[kpScaleIdx[k]] ||
+                keypoints[k].pt.x >= image.cols-patternSizes[kpScaleIdx[k]] ||
+                keypoints[k].pt.y >= image.rows-patternSizes[kpScaleIdx[k]]
+               )
+            {
+                keypoints.erase(kpBegin+k);
+                kpScaleIdx.erase(ScaleIdxBegin+k);
+            }
+        }
+    }
+
+    // allocate descriptor memory, estimate orientations, extract descriptors
+    if( !extAll )
+    {
+        // extract the best comparisons only
+        _descriptors.create((int)keypoints.size(), FREAK::NB_PAIRS/8, CV_8U);
+        _descriptors.setTo(Scalar::all(0));
+        Mat descriptors = _descriptors.getMat();
+
+        void *ptr = descriptors.data+(keypoints.size()-1)*descriptors.step[0];
+
+        for( size_t k = keypoints.size(); k--; ) {
+            // estimate orientation (gradient)
+            if( !orientationNormalized )
+            {
+                thetaIdx = 0; // assign 0° to all keypoints
+                keypoints[k].angle = 0.0;
+            }
+            else
+            {
+                // get the points intensity value in the un-rotated pattern
+                for( int i = FREAK_NB_POINTS; i--; ) {
+                    pointsValue[i] = meanIntensity<srcMatType, iiMatType>(image, imgIntegral,
+                                                                          keypoints[k].pt.x, keypoints[k].pt.y,
+                                                                          kpScaleIdx[k], 0, i);
+                }
+                direction0 = 0;
+                direction1 = 0;
+                for( int m = 45; m--; )
+                {
+                    //iterate through the orientation pairs
+                    const int delta = (pointsValue[ orientationPairs[m].i ]-pointsValue[ orientationPairs[m].j ]);
+                    direction0 += delta*(orientationPairs[m].weight_dx)/2048;
+                    direction1 += delta*(orientationPairs[m].weight_dy)/2048;
+                }
+
+                keypoints[k].angle = static_cast<float>(atan2((float)direction1,(float)direction0)*(180.0/CV_PI));//estimate orientation
+
+                thetaIdx = cvRound(FREAK_NB_ORIENTATION*keypoints[k].angle*(1/360.0));
+
+                if( thetaIdx < 0 )
+                    thetaIdx += FREAK_NB_ORIENTATION;
+
+                if( thetaIdx >= FREAK_NB_ORIENTATION )
+                    thetaIdx -= FREAK_NB_ORIENTATION;
+            }
+            // extract descriptor at the computed orientation
+            for( int i = FREAK_NB_POINTS; i--; ) {
+                pointsValue[i] = meanIntensity<srcMatType, iiMatType>(image, imgIntegral,
+                                                                      keypoints[k].pt.x, keypoints[k].pt.y,
+                                                                      kpScaleIdx[k], thetaIdx, i);
+            }
+
+            // Extract descriptor
+            extractDescriptor<srcMatType>(pointsValue, &ptr);
+        }
+    }
+    else // extract all possible comparisons for selection
+    {
+        _descriptors.create((int)keypoints.size(), 128, CV_8U);
+        _descriptors.setTo(Scalar::all(0));
+        Mat descriptors = _descriptors.getMat();
+        std::bitset<1024>* ptr = (std::bitset<1024>*) (descriptors.data+(keypoints.size()-1)*descriptors.step[0]);
+
+        for( size_t k = keypoints.size(); k--; )
+        {
+            //estimate orientation (gradient)
+            if( !orientationNormalized )
+            {
+                thetaIdx = 0;//assign 0° to all keypoints
+                keypoints[k].angle = 0.0;
+            }
+            else
+            {
+                //get the points intensity value in the un-rotated pattern
+                for( int i = FREAK_NB_POINTS;i--; )
+                    pointsValue[i] = meanIntensity<srcMatType, iiMatType>(image, imgIntegral,
+                                                                          keypoints[k].pt.x,keypoints[k].pt.y,
+                                                                          kpScaleIdx[k], 0, i);
+
+                direction0 = 0;
+                direction1 = 0;
+                for( int m = 45; m--; )
+                {
+                    //iterate through the orientation pairs
+                    const int delta = (pointsValue[ orientationPairs[m].i ]-pointsValue[ orientationPairs[m].j ]);
+                    direction0 += delta*(orientationPairs[m].weight_dx)/2048;
+                    direction1 += delta*(orientationPairs[m].weight_dy)/2048;
+                }
+
+                keypoints[k].angle = static_cast<float>(atan2((float)direction1,(float)direction0)*(180.0/CV_PI)); //estimate orientation
+
+                thetaIdx = cvRound(FREAK_NB_ORIENTATION*keypoints[k].angle*(1/360.0));
+
+                if( thetaIdx < 0 )
+                    thetaIdx += FREAK_NB_ORIENTATION;
+
+                if( thetaIdx >= FREAK_NB_ORIENTATION )
+                    thetaIdx -= FREAK_NB_ORIENTATION;
+            }
+            // get the points intensity value in the rotated pattern
+            for( int i = FREAK_NB_POINTS; i--; ) {
+                pointsValue[i] = meanIntensity<srcMatType, iiMatType>(image, imgIntegral,
+                                                                      keypoints[k].pt.x, keypoints[k].pt.y,
+                                                                      kpScaleIdx[k], thetaIdx, i);
+            }
+
+            int cnt(0);
+            for( int i = 1; i < FREAK_NB_POINTS; ++i )
+            {
+                //(generate all the pairs)
+                for( int j = 0; j < i; ++j )
+                {
+                    ptr->set(cnt, pointsValue[i] >= pointsValue[j] );
+                    ++cnt;
+                }
+            }
+            --ptr;
+        }
+    }
+}
+
+// simply take average on a square patch, not even gaussian approx
+template <typename imgType, typename iiType>
+imgType FREAK_Impl::meanIntensity( InputArray _image, InputArray _integral,
+                              const float kp_x,
+                              const float kp_y,
+                              const unsigned int scale,
+                              const unsigned int rot,
+                              const unsigned int point)
+{
+    Mat image = _image.getMat(), integral = _integral.getMat();
+    // get point position in image
+    const PatternPoint& FreakPoint = patternLookup[scale*FREAK_NB_ORIENTATION*FREAK_NB_POINTS + rot*FREAK_NB_POINTS + point];
+    const float xf = FreakPoint.x+kp_x;
+    const float yf = FreakPoint.y+kp_y;
+    const int x = int(xf);
+    const int y = int(yf);
+
+    // get the sigma:
+    const float radius = FreakPoint.sigma;
+
+    // calculate output:
+    if( radius < 0.5 )
+    {
+        // interpolation multipliers:
+        const int r_x = static_cast<int>((xf-x)*1024);
+        const int r_y = static_cast<int>((yf-y)*1024);
+        const int r_x_1 = (1024-r_x);
+        const int r_y_1 = (1024-r_y);
+        unsigned int ret_val;
+        // linear interpolation:
+        ret_val = r_x_1*r_y_1*int(image.at<imgType>(y  , x  ))
+                + r_x  *r_y_1*int(image.at<imgType>(y  , x+1))
+                + r_x_1*r_y  *int(image.at<imgType>(y+1, x  ))
+                + r_x  *r_y  *int(image.at<imgType>(y+1, x+1));
+        //return the rounded mean
+        ret_val += 2 * 1024 * 1024;
+        return static_cast<imgType>(ret_val / (4 * 1024 * 1024));
+    }
+
+    // expected case:
+
+    // calculate borders
+    const int x_left = cvRound(xf-radius);
+    const int y_top = cvRound(yf-radius);
+    const int x_right = cvRound(xf+radius+1);//integral image is 1px wider
+    const int y_bottom = cvRound(yf+radius+1);//integral image is 1px higher
+    iiType ret_val;
+
+    ret_val = integral.at<iiType>(y_bottom,x_right);//bottom right corner
+    ret_val -= integral.at<iiType>(y_bottom,x_left);
+    ret_val += integral.at<iiType>(y_top,x_left);
+    ret_val -= integral.at<iiType>(y_top,x_right);
+    const int area = (x_right - x_left) * (y_bottom - y_top);
+    ret_val = (ret_val + area/2) / area;
+    //~ std::cout<<integral.step[1]<<std::endl;
+    return static_cast<imgType>(ret_val);
+}
+
+// pair selection algorithm from a set of training images and corresponding keypoints
+std::vector<int> FREAK_Impl::selectPairs(const std::vector<Mat>& images
+                                        , std::vector<std::vector<KeyPoint> >& keypoints
+                                        , const double corrTresh
+                                        , bool verbose )
+{
+    extAll = true;
+    // compute descriptors with all pairs
+    Mat descriptors;
+
+    if( verbose )
+        std::cout << "Number of images: " << images.size() << std::endl;
+
+    for( size_t i = 0;i < images.size(); ++i )
+    {
+        Mat descriptorsTmp;
+        compute(images[i],keypoints[i],descriptorsTmp);
+        descriptors.push_back(descriptorsTmp);
+    }
+
+    if( verbose )
+        std::cout << "number of keypoints: " << descriptors.rows << std::endl;
+
+    //descriptor in floating point format (each bit is a float)
+    Mat descriptorsFloat = Mat::zeros(descriptors.rows, 903, CV_32F);
+
+    std::bitset<1024>* ptr = (std::bitset<1024>*) (descriptors.data+(descriptors.rows-1)*descriptors.step[0]);
+    for( int m = descriptors.rows; m--; )
+    {
+        for( int n = 903; n--; )
+        {
+            if( ptr->test(n) == true )
+                descriptorsFloat.at<float>(m,n)=1.0f;
+        }
+        --ptr;
+    }
+
+    std::vector<PairStat> pairStat;
+    for( int n = 903; n--; )
+    {
+        // the higher the variance, the better --> mean = 0.5
+        PairStat tmp = { fabs( mean(descriptorsFloat.col(n))[0]-0.5 ) ,n};
+        pairStat.push_back(tmp);
+    }
+
+    std::sort( pairStat.begin(),pairStat.end(), sortMean() );
+
+    std::vector<PairStat> bestPairs;
+    for( int m = 0; m < 903; ++m )
+    {
+        if( verbose )
+            std::cout << m << ":" << bestPairs.size() << " " << std::flush;
+        double corrMax(0);
+
+        for( size_t n = 0; n < bestPairs.size(); ++n )
+        {
+            int idxA = bestPairs[n].idx;
+            int idxB = pairStat[m].idx;
+            double corr(0);
+            // compute correlation between 2 pairs
+            corr = fabs(compareHist(descriptorsFloat.col(idxA), descriptorsFloat.col(idxB), HISTCMP_CORREL));
+
+            if( corr > corrMax )
+            {
+                corrMax = corr;
+                if( corrMax >= corrTresh )
+                    break;
+            }
+        }
+
+        if( corrMax < corrTresh/*0.7*/ )
+            bestPairs.push_back(pairStat[m]);
+
+        if( bestPairs.size() >= 512 )
+        {
+            if( verbose )
+                std::cout << m << std::endl;
+            break;
+        }
+    }
+
+    std::vector<int> idxBestPairs;
+    if( (int)bestPairs.size() >= FREAK::NB_PAIRS )
+    {
+        for( int i = 0; i < FREAK::NB_PAIRS; ++i )
+            idxBestPairs.push_back(bestPairs[i].idx);
+    }
+    else
+    {
+        if( verbose )
+            std::cout << "correlation threshold too small (restrictive)" << std::endl;
+        CV_Error(Error::StsError, "correlation threshold too small (restrictive)");
+    }
+    extAll = false;
+    return idxBestPairs;
+}
+
+
+/*
+// create an image showing the brisk pattern
+void FREAKImpl::drawPattern()
+{
+    Mat pattern = Mat::zeros(1000, 1000, CV_8UC3) + Scalar(255,255,255);
+    int sFac = 500 / patternScale;
+    for( int n = 0; n < kNB_POINTS; ++n )
+    {
+        PatternPoint& pt = patternLookup[n];
+        circle(pattern, Point( pt.x*sFac,pt.y*sFac)+Point(500,500), pt.sigma*sFac, Scalar(0,0,255),2);
+        // rectangle(pattern, Point( (pt.x-pt.sigma)*sFac,(pt.y-pt.sigma)*sFac)+Point(500,500), Point( (pt.x+pt.sigma)*sFac,(pt.y+pt.sigma)*sFac)+Point(500,500), Scalar(0,0,255),2);
+
+        circle(pattern, Point( pt.x*sFac,pt.y*sFac)+Point(500,500), 1, Scalar(0,0,0),3);
+        std::ostringstream oss;
+        oss << n;
+        putText( pattern, oss.str(), Point( pt.x*sFac,pt.y*sFac)+Point(500,500), FONT_HERSHEY_SIMPLEX,0.5, Scalar(0,0,0), 1);
+    }
+    imshow( "FreakDescriptorExtractor pattern", pattern );
+    waitKey(0);
+}
+*/
+
+// -------------------------------------------------
+/* FREAK interface implementation */
+FREAK_Impl::FREAK_Impl( bool _orientationNormalized, bool _scaleNormalized
+            , float _patternScale, int _nOctaves, const std::vector<int>& _selectedPairs )
+    : orientationNormalized(_orientationNormalized), scaleNormalized(_scaleNormalized),
+    patternScale(_patternScale), nOctaves(_nOctaves), extAll(false),
+    patternScale0(0.0), nOctaves0(0), selectedPairs0(_selectedPairs)
+{
+}
+
+FREAK_Impl::~FREAK_Impl()
+{
+}
+
+int FREAK_Impl::descriptorSize() const
+{
+    return FREAK::NB_PAIRS / 8; // descriptor length in bytes
+}
+
+int FREAK_Impl::descriptorType() const
+{
+    return CV_8U;
+}
+
+int FREAK_Impl::defaultNorm() const
+{
+    return NORM_HAMMING;
+}
+
+Ptr<FREAK> FREAK::create(bool orientationNormalized,
+                         bool scaleNormalized,
+                         float patternScale,
+                         int nOctaves,
+                         const std::vector<int>& selectedPairs)
+{
+    return makePtr<FREAK_Impl>(orientationNormalized, scaleNormalized,
+                               patternScale, nOctaves, selectedPairs);
+}
+
+}
+} // END NAMESPACE CV
@@ -0,0 +1,172 @@
+static const float FREAK_LOG2 = 0.693147180559945;
+static const int FREAK_NB_ORIENTATION = 256;
+static const int NB_ORIENPAIRS = 45;
+static const int FREAK_NB_POINTS = 43;
+static const int FREAK_SMALLEST_KP_SIZE = 7; // smallest size of keypoints
+static const int NB_PAIRS = 512;
+static const int NB_SCALES = 64;
+
+bool orientationNormalized = true;
+bool scaleNormalized = true;
+float patternScale = 22.0f;
+int nOctaves = 4;
+//float patternScale0 = 0.0;
+//int nOctaves0 = 0;
+bool extAll = false; // true if all pairs need to be extracted for pairs selection
+//int* selectedPairs0;
+
+float3 patternLookup[NB_SCALES * FREAK_NB_ORIENTATION * FREAK_NB_POINTS]; // look-up table for the pattern points (position+sigma of all points at all scales and orientation)
+int patternSizes[NB_SCALES]; // size of the pattern at a specific scale (used to check if a point is within image boundaries)
+uchar2 descriptionPairs[NB_PAIRS];
+int4 orientationPairs[NB_ORIENPAIRS];
+
+// default pairs
+static const int FREAK_DEF_PAIRS[NB_PAIRS] =
+        {
+                404,431,818,511,181,52,311,874,774,543,719,230,417,205,11,
+                560,149,265,39,306,165,857,250,8,61,15,55,717,44,412,
+                592,134,761,695,660,782,625,487,549,516,271,665,762,392,178,
+                796,773,31,672,845,548,794,677,654,241,831,225,238,849,83,
+                691,484,826,707,122,517,583,731,328,339,571,475,394,472,580,
+                381,137,93,380,327,619,729,808,218,213,459,141,806,341,95,
+                382,568,124,750,193,749,706,843,79,199,317,329,768,198,100,
+                466,613,78,562,783,689,136,838,94,142,164,679,219,419,366,
+                418,423,77,89,523,259,683,312,555,20,470,684,123,458,453,833,
+                72,113,253,108,313,25,153,648,411,607,618,128,305,232,301,84,
+                56,264,371,46,407,360,38,99,176,710,114,578,66,372,653,
+                129,359,424,159,821,10,323,393,5,340,891,9,790,47,0,175,346,
+                236,26,172,147,574,561,32,294,429,724,755,398,787,288,299,
+                769,565,767,722,757,224,465,723,498,467,235,127,802,446,233,
+                544,482,800,318,16,532,801,441,554,173,60,530,713,469,30,
+                212,630,899,170,266,799,88,49,512,399,23,500,107,524,90,
+                194,143,135,192,206,345,148,71,119,101,563,870,158,254,214,
+                276,464,332,725,188,385,24,476,40,231,620,171,258,67,109,
+                844,244,187,388,701,690,50,7,850,479,48,522,22,154,12,659,
+                736,655,577,737,830,811,174,21,237,335,353,234,53,270,62,
+                182,45,177,245,812,673,355,556,612,166,204,54,248,365,226,
+                242,452,700,685,573,14,842,481,468,781,564,416,179,405,35,
+                819,608,624,367,98,643,448,2,460,676,440,240,130,146,184,
+                185,430,65,807,377,82,121,708,239,310,138,596,730,575,477,
+                851,797,247,27,85,586,307,779,326,494,856,324,827,96,748,
+                13,397,125,688,702,92,293,716,277,140,112,4,80,855,839,1,
+                413,347,584,493,289,696,19,751,379,76,73,115,6,590,183,734,
+                197,483,217,344,330,400,186,243,587,220,780,200,793,246,824,
+                41,735,579,81,703,322,760,720,139,480,490,91,814,813,163,
+                152,488,763,263,425,410,576,120,319,668,150,160,302,491,515,
+                260,145,428,97,251,395,272,252,18,106,358,854,485,144,550,
+                131,133,378,68,102,104,58,361,275,209,697,582,338,742,589,
+                325,408,229,28,304,191,189,110,126,486,211,547,533,70,215,
+                670,249,36,581,389,605,331,518,442,822
+        };
+
+float scaleStep = pow(2.0, (float)(nOctaves)/FREAK_NB_SCALES ); // 2 ^ ( (nOctaves-1) /nbScales)
+
+const int n[8] = {6, 6, 6, 6, 6, 6, 6, 1}; // number of points on each concentric circle (from outer to inner)
+const float bigR = (float)(2.0 / 3.0); // bigger radius
+const float smallR = (float)(2.0 / 24.0); // smaller radius
+const float unitSpace = (float)((bigR - smallR) / 21.0 ); // define spaces between concentric circles (from center to outer: 1,2,3,4,5,6)
+// radii of the concentric cirles (from outer to inner)
+const float radius[8] = {bigR                 , bigR -  6 * unitSpace, bigR - 11 * unitSpace, bigR - 15 * unitSpace,
+                         bigR - 18 * unitSpace, bigR - 20 * unitSpace, smallR               , 0.0};
+// sigma of pattern points (each group of 6 points on a concentric cirle has the same sigma)
+const float sigma[8] = {radius[0] / 2.0, radius[1] / 2.0, radius[2] / 2.0, radius[3] / 2.0,
+                        radius[4] / 2.0, radius[5] / 2.0, radius[6] / 2.0, radius[6] / 2.0};
+
+for( int scaleIdx=0; scaleIdx < NB_SCALES; ++scaleIdx )
+{
+patternSizes[scaleIdx] = 0; // proper initialization
+float scalingFactor = pow(scaleStep,scaleIdx); //scale of the pattern, scaleStep ^ scaleIdx
+
+for( int orientationIdx = 0; orientationIdx < FREAK_NB_ORIENTATION; ++orientationIdx )
+{
+int pointIdx = 0;
+
+for( size_t i = 0; i < 8; ++i )
+{
+for( int k = 0 ; k < n[i]; ++k )
+{
+float alpha = (float)(k) * 2 * (float)3.141592653589793238462643383279502884197169399375 / (float)(n[i])
+              + ((float)3.141592653589793238462643383279502884197169399375 / n[i] * ( i % 2 )/*beta*/)
+              + ((float)(orientationIdx * 2 * (float)3.141592653589793238462643383279502884197169399375 / (float)(FREAK_NB_ORIENTATION)) /* theta*/);
+
+// add the point to the look-up table
+float3* point = patternLookup[ scaleIdx * FREAK_NB_ORIENTATION * FREAK_NB_POINTS + orientationIdx * FREAK_NB_POINTS + pointIdx];
+point->x = (float)(radius[i] * cos(alpha) * scalingFactor * patternScale); //x
+point->y = (float)(radius[i] * sin(alpha) * scalingFactor * patternScale); //y
+point->z = (float)(sigma[i] * scalingFactor * patternScale); //sigma
+
+// adapt the sizeList if necessary
+const int sizeMax = (int)(ceil((radius[i] + sigma[i]) * scalingFactor * patternScale)) + 1;
+if( patternSizes[scaleIdx] < sizeMax )
+patternSizes[scaleIdx] = sizeMax;
+
++pointIdx;
+}
+}
+}
+}
+
+// build the list of orientation pairs
+orientationPairs[0].w=0;   orientationPairs[0].x=3;   orientationPairs[1].w=1;   orientationPairs[1].x=4;    orientationPairs[2].w=2;   orientationPairs[2].x=5;
+orientationPairs[3].w=0;   orientationPairs[3].x=2;   orientationPairs[4].w=1;   orientationPairs[4].x=3;    orientationPairs[5].w=2;   orientationPairs[5].x=4;
+orientationPairs[6].w=3;   orientationPairs[6].x=5;   orientationPairs[7].w=4;   orientationPairs[7].x=0;    orientationPairs[8].w=5;   orientationPairs[8].x=1;
+
+orientationPairs[9].w=6;   orientationPairs[9].x=9;   orientationPairs[10].w=7;  orientationPairs[10].x=10;  orientationPairs[11].w=8;  orientationPairs[11].x=11;
+orientationPairs[12].w=6;  orientationPairs[12].x=8;  orientationPairs[13].w=7;  orientationPairs[13].x=9;   orientationPairs[14].w=8;  orientationPairs[14].x=10;
+orientationPairs[15].w=9;  orientationPairs[15].x=11; orientationPairs[16].w=10; orientationPairs[16].x=6;   orientationPairs[17].w=11; orientationPairs[17].x=7;
+
+orientationPairs[18].w=12; orientationPairs[18].x=15; orientationPairs[19].w=13; orientationPairs[19].x=16;  orientationPairs[20].w=14; orientationPairs[20].x=17;
+orientationPairs[21].w=12; orientationPairs[21].x=14; orientationPairs[22].w=13; orientationPairs[22].x=15;  orientationPairs[23].w=14; orientationPairs[23].x=16;
+orientationPairs[24].w=15; orientationPairs[24].x=17; orientationPairs[25].w=16; orientationPairs[25].x=12;  orientationPairs[26].w=17; orientationPairs[26].x=13;
+
+orientationPairs[27].w=18; orientationPairs[27].x=21; orientationPairs[28].w=19; orientationPairs[28].x=22;  orientationPairs[29].w=20; orientationPairs[29].x=23;
+orientationPairs[30].w=18; orientationPairs[30].x=20; orientationPairs[31].w=19; orientationPairs[31].x=21;  orientationPairs[32].w=20; orientationPairs[32].x=22;
+orientationPairs[33].w=21; orientationPairs[33].x=23; orientationPairs[34].w=22; orientationPairs[34].x=18;  orientationPairs[35].w=23; orientationPairs[35].x=19;
+
+orientationPairs[36].w=24; orientationPairs[36].x=27; orientationPairs[37].w=25; orientationPairs[37].x=28;  orientationPairs[38].w=26; orientationPairs[38].x=29;
+orientationPairs[39].w=30; orientationPairs[39].x=33; orientationPairs[40].w=31; orientationPairs[40].x=34;  orientationPairs[41].w=32; orientationPairs[41].x=35;
+orientationPairs[42].w=36; orientationPairs[42].x=39; orientationPairs[43].w=37; orientationPairs[43].x=40;  orientationPairs[44].w=38; orientationPairs[44].x=41;
+
+for( unsigned m = NB_ORIENPAIRS; m--; )
+{
+float dx = patternLookup[orientationPairs[m].w].x - patternLookup[orientationPairs[m].x].x;
+float dy = patternLookup[orientationPairs[m].w].y - patternLookup[orientationPairs[m].x].y;
+float norm_sq = (dx * dx + dy * dy);
+orientationPairs[m].y = round((dx / (norm_sq)) * 4096.0); //weight_dx
+orientationPairs[m].z = round((dy / (norm_sq)) * 4096.0); //weight_dy
+}
+
+// build the list of description pairs
+uchar2 allPairs[(int)((FREAK_NB_POINTS * (FREAK_NB_POINTS + 1)) / 2)];
+for( int i = 1, idx = 0; i < FREAK_NB_POINTS; ++i )
+{
+// (generate all the pairs)
+for( int j = 0; j < i; ++j )
+{
+allPairs[idx++] = (uchar2){(uchar)i, (uchar)j};
+}
+}
+for( int i = 0; i < NB_PAIRS; ++i )
+descriptionPairs[i] = allPairs[FREAK_DEF_PAIRS[i]];
+
+int kpScaleIdx[keypoints.size()];
+
+// kernel running with parameter : 
+// __kernel void freak(
+//		__read_only	image2d_t	image, // grayescale image - height X width
+//		__read_only	image2d_t	imgIntegral, // (height+1) X (width+1) with zero values
+//		__global		float3*		keypoints, // ptx, pty, angle
+//		__global		uchar*		descriptors, // kp_size * (64 = NB_PAIRS/8) - CV_8U with zero values
+//		__global		int*			patternSizes, // array size : NB_SCALES = 64
+//		__global		int4*			orientationPairs, // array size : FREAK_NB_ORIENTATION = 256
+//		__global		float3*		patternLookup, // array size : NB_SCALES(64) * FREAK_NB_ORIENTATION(256) * FREAK_NB_POINTS(43)
+//		__global		uchar2*		descriptionPairs, // array size : NB_PAIRS = 512
+//		__global		int*			nOctaves,
+//		__global		int*			kpScaleIdx //array size = kp_size - used to save pattern scale index corresponding to each keypoints
+//		)
+// 	FREAK_LOG2 = 0.693147180559945;
+//	FREAK_NB_ORIENTATION = 256;
+//	FREAK_NB_POINTS = 43;
+//	FREAK_SMALLEST_KP_SIZE = 7;
+//	NB_PAIRS = 512;
+//	NB_SCALES = 64;
@@ -0,0 +1,165 @@
+// Copyright (C) 2021  Majid Geravand
+// Copyright (C) 2021  Gfuse
+
+// Enable OpenCL 32-bit integer atomic functions.
+#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable
+
+float2 world2cam(float3 feature)
+{
+    float r = sqrt(pow(feature.x/feature.z, 2) + pow(feature.y/feature.z, 2));
+    float factor = 1.0;
+    if((float)S != 0 || r > 0.001)
+        factor = (atan(r * 2.0 * tan(0.5 * (float)S)) / (r * (float)S));
+    return (float2)((float)C_X + (float)F_X * factor * feature.x/feature.z, (float)C_Y + (float)F_Y * factor * feature.y/feature.z);
+}
+
+float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
+{
+    if(ref.z < 0.0)
+        ref.z = 3.141592653589793238462643383279502884197169399375 - ref.z;
+    else
+        ref.z = ref.z - 3.1415926535897932384626433832795028841971693993;
+    float cc_ss = cos(cur.z) * cos(ref.z) - sin(cur.z) * sin(ref.z);
+    float sc_cs = cos(cur.z) * sin(ref.z) + cos(ref.z) * sin(cur.z);
+    return  (float3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur.x - ref.x * cos(cur.z) - ref.z * sin(cur.z),
+                      ref_feature.y,
+                      -1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z));
+}
+
+void jacobian_xyz2uv(float3 xyz_in_f, float* J)
+{
+    *(J + 0) = -(1. / xyz_in_f[2]);                                                         // -1/z
+    *(J + 1) = (xyz_in_f[0]) * ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2]));                   // x/z^2
+    *(J + 2) = -(1.0 + pow((xyz_in_f[0]),2) / ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2])));   // -(1.0 + x^2/z^2)
+    *(J + 3) = 1e-19;                                                                       // 0
+    *(J + 4) = (xyz_in_f[1]) * ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2]));                   // y/z^2
+    *(J + 5) = -(xyz_in_f[0]) * (xyz_in_f[1]) / ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2]));  // -x*y/z^2
+}
+
+void jacobian_xyz2uv_2(float3 xyz_in_f, float3 ref_pose, float3 cur_pose, float* J)
+{   float3 txtytetha = cur_pose - ref_pose;
+    float2 uv = world2cam(xyz_in_f);
+    *(J + 0) = 1.0;
+    *(J + 1) = 0.0;
+    *(J + 2) = ((-1 * sin(txtytetha[2]) * cos(ref_pose[2])) - (cos(txtytetha[2]) * sin(ref_pose[2]))) * uv[0] + ((cos(txtytetha[2]) * cos(ref_pose[2])) - (sin(txtytetha[2]) * sin(ref_pose[2]))) * uv[1];
+    *(J + 3) = 0.0;
+    *(J + 4) = 1.0;
+    *(J + 5) = ((-1 * cos(txtytetha[2]) * cos(ref_pose[2])) + (sin(txtytetha[2]) * sin(ref_pose[2]))) * uv[0] - ((sin(txtytetha[2]) * cos(ref_pose[2])) + (cos(txtytetha[2]) * sin(ref_pose[2]))) * uv[1];
+}
+
+void compute_hessain(float3 j, __global float* H, float w)
+{
+    *(H + 0) += j[0] * j[0] * w;
+    *(H + 1) += j[0] * j[1] * w;
+    *(H + 2) += j[0] * j[2] * w;
+    *(H + 3) += j[1] * j[0] * w;
+    *(H + 4) += j[1] * j[1] * w;
+    *(H + 5) += j[1] * j[2] * w;
+    *(H + 6) += j[2] * j[0] * w;
+    *(H + 7) += j[2] * j[1] * w;
+    *(H + 8) += j[2] * j[2] * w;
+}
+
+__kernel void compute_residual(
+        __read_only  image2d_t   image_cur, // current frame
+        __read_only  image2d_t   image_ref, // reference frame
+        __global     float3      * cur_pose,//[reference frame pose{x,z,pitch}]
+        __global     float3      * ref_pose,//[current frame pose{x,z,pitch}]
+        __global     float3     * ref_feature, // feature on the reference frame, when we applied the distance calculation: xyz_ref((*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0],0.0,ref_pos[1])).norm());
+        __global     float2     * featue_px,
+                        int        level,
+        __global     float       * errors,
+        __global     float      * Hessian,
+        __global     float3      * Jacobian,
+        __global     float      * chi,
+                     float        scale_
+)
+{
+    float scale = pow(2.0, -level);
+    // Prepare a suitable OpenCL image sampler.
+    sampler_t const sampler = CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST;
+    // Use global work feature.
+    int f = get_global_id(0);
+    // check if reference with patch size is within image
+    float2 uv_ref = featue_px[f] * scale;
+    float2 uv_ref_i = floor(uv_ref);
+    if(uv_ref_i.x - (PATCH_HALFSIZE + 1) < 0 || uv_ref_i.y - (PATCH_HALFSIZE + 1) < 0 || uv_ref_i.x + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref)[0] || uv_ref_i.y + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref)[1])return;
+    // evaluate projection jacobian
+    float frame_jac[6]={0.0}; // 2X3
+    jacobian_xyz2uv(sqrt(pow(ref_feature[f] - (float3)(ref_pose[0].x,0.0,ref_pose[0].y), 2.0)), &frame_jac);
+    //jacobian_xyz2uv_2(ref_feature[f], ref_pose[0], cur_pose[0], &frame_jac);
+    //printf("jacobian : %f %f %f %f %f %f\n", frame_jac[0], frame_jac[1], frame_jac[2], frame_jac[3], frame_jac[4], frame_jac[5]);
+    // compute bilateral interpolation weights for reference image
+    float2 subpix=  uv_ref - uv_ref_i;
+    float w_ref_tl = (1.0 - subpix.x) * (1.0 - subpix.y);
+    float w_ref_tr = subpix.x * (1.0 - subpix.y);
+    float w_ref_bl = (1.0 - subpix.x) * subpix.y;
+    float w_ref_br = subpix.x * subpix.y;
+
+    // compute bilateral interpolation weights for the current image
+    float2 uv_cur_pyr = world2cam(xyz_cur(cur_pose[0], ref_pose[0] ,ref_feature[f])) * scale;
+    float2 uv_cur_i = floor(uv_cur_pyr);
+    float2 subpix_uv_cur = uv_cur_pyr - uv_cur_i;
+    float w_cur_tl = (1.0 - subpix_uv_cur.x) * (1.0 - subpix_uv_cur.y);
+    float w_cur_tr = subpix_uv_cur.x * (1.0 - subpix_uv_cur.y);
+    float w_cur_bl = (1.0 - subpix_uv_cur.x) * subpix_uv_cur.y;
+    float w_cur_br = subpix_uv_cur.x * subpix_uv_cur.y;
+    float e = 0.0;
+    float chi_=0.0;
+    for(int y = 0; y < PATCH_SIZE; ++y)
+    {
+        int ref_element_addr = (int)(uv_ref_i.y + y - PATCH_HALFSIZE) * get_image_dim(image_ref)[0] + (uv_ref_i.x - PATCH_HALFSIZE);
+        int cur_element_addr = (int)(uv_cur_i.y + y - PATCH_HALFSIZE) * get_image_dim(image_cur)[0] + (uv_cur_i.x - PATCH_HALFSIZE);
+
+        for(int x = 0; x < PATCH_SIZE; ++x, ++ref_element_addr, ++cur_element_addr)
+        {
+            // precompute interpolated reference patch color
+            int2 px_reftl = (int2)( ref_element_addr                                    % get_image_dim(image_ref)[0],  ref_element_addr                                    / get_image_dim(image_ref)[0]);
+            int2 px_reftr = (int2)((ref_element_addr + 1)                               % get_image_dim(image_ref)[0], (ref_element_addr + 1)                               / get_image_dim(image_ref)[0]);
+            int2 px_refbl = (int2)((ref_element_addr + get_image_dim(image_ref)[0])     % get_image_dim(image_ref)[0], (ref_element_addr + get_image_dim(image_ref)[0])     / get_image_dim(image_ref)[0]);
+            int2 px_refbr = (int2)((ref_element_addr + get_image_dim(image_ref)[0] + 1) % get_image_dim(image_ref)[0], (ref_element_addr + get_image_dim(image_ref)[0] + 1) / get_image_dim(image_ref)[0]);
+            float value = w_ref_tl * read_imageui(image_ref, sampler, px_reftl).x + w_ref_tr * read_imageui(image_ref, sampler, px_reftr).x +
+                           w_ref_bl * read_imageui(image_ref, sampler, px_refbl).x + w_ref_br * read_imageui(image_ref, sampler, px_refbr).x;
+
+            float dx = 0.5f * ((w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 1)                                     % get_image_dim(image_ref)[0], ((ref_element_addr + 1)                                     / get_image_dim(image_ref)[0]))).x +
+                                 w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 2)                                     % get_image_dim(image_ref)[0], ((ref_element_addr + 2)                                     / get_image_dim(image_ref)[0]))).x +
+                                 w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1)     % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1)     / get_image_dim(image_ref)[0]))).x +
+                                 w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) + 2)     % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) + 2)     / get_image_dim(image_ref)[0]))).x)
+                               -(w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr - 1)                                     % get_image_dim(image_ref)[0], ((ref_element_addr - 1)                                     / get_image_dim(image_ref)[0]))).x +
+                                 w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 0)                                     % get_image_dim(image_ref)[0], ((ref_element_addr + 0)                                     / get_image_dim(image_ref)[0]))).x +
+                                 w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) - 1)     % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) - 1)     / get_image_dim(image_ref)[0]))).x +
+                                 w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]))         % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]))         / get_image_dim(image_ref)[0]))).x));
+
+            float dy = 0.5f * ((w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]))         % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]))         / get_image_dim(image_ref)[0]))).x +
+                                 w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1)     % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1)     / get_image_dim(image_ref)[0]))).x +
+                                 w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2)     % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2)     / get_image_dim(image_ref)[0]))).x +
+                                 w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2 + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2 + 1) / get_image_dim(image_ref)[0]))).x)
+                               -(w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (-get_image_dim(image_ref)[0]))        % get_image_dim(image_ref)[0], ((ref_element_addr + (-get_image_dim(image_ref)[0]))        / get_image_dim(image_ref)[0]))).x +
+                                 w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (1 - get_image_dim(image_ref)[0]))     % get_image_dim(image_ref)[0], ((ref_element_addr + (1 - get_image_dim(image_ref)[0]))     / get_image_dim(image_ref)[0]))).x +
+                                 w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 0)                                     % get_image_dim(image_ref)[0], ((ref_element_addr + 0)                                     / get_image_dim(image_ref)[0]))).x +
+                                 w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 1)                                     % get_image_dim(image_ref)[0], ((ref_element_addr + 1)                                     / get_image_dim(image_ref)[0]))).x));
+
+            // compute residual
+            int2 px_curtl = (int2)( cur_element_addr                                      % get_image_dim(image_cur)[0],  cur_element_addr                                      / get_image_dim(image_cur)[0]);
+            int2 px_curtr = (int2)((cur_element_addr + 1)                                 % get_image_dim(image_cur)[0], (cur_element_addr + 1)                                 / get_image_dim(image_cur)[0]);
+            int2 px_curbl = (int2)((cur_element_addr + (get_image_dim(image_cur)[0]))     % get_image_dim(image_cur)[0], (cur_element_addr + (get_image_dim(image_cur)[0]))     / get_image_dim(image_cur)[0]);
+            int2 px_curbr = (int2)((cur_element_addr + (get_image_dim(image_cur)[0]) + 1) % get_image_dim(image_cur)[0], (cur_element_addr + (get_image_dim(image_cur)[0]) + 1) / get_image_dim(image_cur)[0]);
+
+            float res = value - w_cur_tl * read_imageui(image_cur, sampler, px_curtl).x + w_cur_tr * read_imageui(image_cur, sampler, px_curtr).x +
+                                 w_cur_bl * read_imageui(image_cur, sampler, px_curbl).x + w_cur_br * read_imageui(image_cur, sampler, px_curbr).x;
+            // used to compute scale for robust cost
+            e += fabs(res);
+            // robustification
+            float weight = res/scale_; //1.48f * vk::getMedian(errors)
+            chi_ += pow(res,2) * weight;
+            float3 j_row0 = (float3)((dx * frame_jac[0]), (dx * frame_jac[1]), (dx * frame_jac[2]));
+            float3 j_row1 = (float3)((dy * frame_jac[3]), (dy * frame_jac[4]), (dy * frame_jac[5]));
+            float3 J = (j_row0 + j_row1) * ((float)F_X / scale);
+            compute_hessain(J, &Hessian[f*9], weight);
+            Jacobian[f] -= J * res * weight;
+
+}
+}
+errors[f] = e / pow(PATCH_SIZE,2.0);
+chi[f]=chi_;
+}
@@ -0,0 +1,129 @@
+void meanIntensity( image2d_t _image, image2d_t _integral, float kp_x, float kp_y,
+							  int scale, int rot, int* point, float3* patternLookupPtr, uchar* output)
+{
+    // Prepare a suitable OpenCL image sampler.
+    sampler_t const sampler = CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST;
+    // get point position in image
+    float3* FreakPoint = patternLookupPtr + (scale * FREAK_NB_ORIENTATION * FREAK_NB_POINTS + rot * FREAK_NB_POINTS + (*point));
+    float xf = FreakPoint->x + kp_x;
+    float yf = FreakPoint->y + kp_y;
+    int x = (int)xf;
+    int y = (int)yf;
+
+    // get the sigma:
+    float radius = FreakPoint->z;//sigma
+
+    // calculate output:
+    if( radius < 0.5 )
+    {
+        // interpolation multipliers:
+        const int r_x = (int)((xf - x) * 1024);
+        const int r_y = (int)((yf - y) * 1024);
+        const int r_x_1 = (1024 - r_x);
+        const int r_y_1 = (1024 - r_y);
+        uint ret_val;
+        // linear interpolation:
+        ret_val = (r_x_1 * r_y_1 * (int)(read_imageui(_image, sampler, (int2)(y  , x  ))).x)
+                + (r_x   * r_y_1 * (int)(read_imageui(_image, sampler, (int2)(y  , x+1))).x)
+                + (r_x_1 * r_y   * (int)(read_imageui(_image, sampler, (int2)(y+1, x  ))).x)
+                + (r_x   * r_y   * (int)(read_imageui(_image, sampler, (int2)(y+1, x+1))).x);
+        // return the rounded mean
+        ret_val += 2 * 1024 * 1024;
+        output[*point] = (uchar)(ret_val / (4 * 1024 * 1024));
+    }
+
+    // calculate borders
+    const int x_left   = round(xf - radius);
+    const int y_top    = round(yf - radius);
+    const int x_right  = round(xf + radius + 1); // integral image is 1px wider
+    const int y_bottom = round(yf + radius + 1); // integral image is 1px higher
+    int ret_val;
+
+    ret_val  = read_imageui(_integral, sampler, (int2)(y_bottom, x_right)).x; // bottom right corner
+    ret_val -= read_imageui(_integral, sampler, (int2)(y_bottom, x_left)).x; // bottom left corner
+    ret_val += read_imageui(_integral, sampler, (int2)(y_top,    x_left)).x; // top left corner
+    ret_val -= read_imageui(_integral, sampler, (int2)(y_top,    x_right)).x; // top right corner
+    const int area = (x_right - x_left) * (y_bottom - y_top);
+    ret_val = (ret_val + area / 2) / area;
+    output[*point] = (uchar)(ret_val);
+}
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+__kernel void freak(
+    __read_only	image2d_t	image, // grayescale image
+	__read_only	image2d_t	imgIntegral,
+    __global		float3*		keypoints, // ptx, pty, angle
+    __global		uchar*		descriptors, // kp_size * (64 = NB_PAIRS/8) - CV_8U with zero values
+	__global		int*			patternSizes, // array size : NB_SCALES = 64
+	__global		int4*			orientationPairs, // array size : FREAK_NB_ORIENTATION = 256
+	__global		float3*		patternLookup, // array size : NB_SCALES(64) * FREAK_NB_ORIENTATION(256) * FREAK_NB_POINTS(43)
+	__global		uchar2*		descriptionPairs, // array size : NB_PAIRS = 512
+	__global		int*			nOctaves,
+	__global		int*			kpScaleIdx //array size = kp_size - used to save pattern scale index corresponding to each keypoints
+	)
+{
+    const float sizeCst = (float)(NB_SCALES / (FREAK_LOG2 * *nOctaves));
+    uchar pointsValue[FREAK_NB_POINTS];
+    int thetaIdx = 0;
+    int direction0;
+    int direction1;
+
+	int k = get_global_id(0);
+
+    kpScaleIdx[k] = max( (int)(log(7.0 / FREAK_SMALLEST_KP_SIZE) * sizeCst + 0.5) ,0);
+    if( kpScaleIdx[k] >= NB_SCALES )
+        kpScaleIdx[k] = NB_SCALES - 1;
+
+    if( keypoints[k].x <= patternSizes[kpScaleIdx[k]] || // check if the description at this specific position and scale fits inside the image
+        keypoints[k].y <= patternSizes[kpScaleIdx[k]] ||
+        keypoints[k].x >= get_image_dim(image).y - patternSizes[kpScaleIdx[k]] ||
+        keypoints[k].y >= get_image_dim(image).x - patternSizes[kpScaleIdx[k]])
+		return;
+
+        // get the points intensity value in the un-rotated pattern
+        for( int i = FREAK_NB_POINTS; i--; )
+        {
+            meanIntensity(image, imgIntegral, keypoints[k].x, keypoints[k].y,
+                          *(kpScaleIdx + k), 0, &i, patternLookup, pointsValue);
+        }
+        direction0 = 0;
+        direction1 = 0;
+        for( int m = 45; m--; )
+        {
+            // iterate through the orientation pairs
+            const int delta = (pointsValue[ orientationPairs[m].w ] - pointsValue[ orientationPairs[m].x ]);
+            direction0 += delta * (orientationPairs[m].y) / 2048; // weight_dx
+            direction1 += delta * (orientationPairs[m].z) / 2048; // weight_dy
+        }
+
+        thetaIdx = round(FREAK_NB_ORIENTATION *
+						(float)(atan2((float)direction1,(float)direction0) * (180.0 / 3.141592653589793238462643383279502884197169399375)) *
+						(1 / 360.0));
+
+        if( thetaIdx < 0 )
+            thetaIdx += FREAK_NB_ORIENTATION;
+
+        if( thetaIdx >= FREAK_NB_ORIENTATION )
+            thetaIdx -= FREAK_NB_ORIENTATION;
+
+        // extract descriptor at the computed orientation
+        for( int i = FREAK_NB_POINTS; i--; )
+        {
+            meanIntensity(image, imgIntegral, keypoints[k].x, keypoints[k].y,
+                          *(kpScaleIdx + k), thetaIdx, &i, patternLookup, pointsValue);
+        }
+
+        // Extract descriptor
+		int cnt = 0;
+        for( int n = 0; n<4; n++ )
+        {
+			for (int m = 8; m >= 1; m--)
+			{
+				for(int kk = (n*16)+15; kk >= (n*16); --kk, ++cnt)
+				{
+					descriptors[(k * 64) + kk] *= 2;
+					if (pointsValue[(int)descriptionPairs[cnt].x] >= pointsValue[(int)descriptionPairs[cnt].y])
+						descriptors[(k * 64) + kk] += 1;
+				}
+			}
+        }
+}
@@ -0,0 +1,13 @@
+__kernel void integral_image(
+    __read_only	image2d_t	int_img, // grayescale image - height X width
+	__write_only	image2d_t	out_img // (height+1) X (width+1) with zero values
+	)
+{
+	sampler_t const sampler = CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST;
+    int2 coord = (int2)(get_global_id(0)+1, get_global_id(1)+1);
+	int res = 0;
+	for (int i = 0; i < coord.x; i++)
+		for (int j = 0; j < coord.y; j++)
+			res += read_imageui(int_img, sampler, (int2)(i, j)); // bottom right corner
+	write_imageui(out_img, coord, res);
+}
@@ -0,0 +1,42 @@
+//
+// Created by root on 8/5/21.
+//
+
+#include <iostream>
+#include "cl_class.h"
+#include <opencv2/opencv.hpp>
+#include <opencv2/core.hpp>
+#include <opencv2/core/mat.hpp>
+#include <sparse_img_align_gpu.h>
+// Maximum number of corners.
+#define FAST_COUNT (1 << 18)
+
+int main() {
+    opencl gpu;
+    gpu.make_kernel("compute_residual");
+    cv::Mat img=cv::Mat(cv::Size(700, 500),CV_8UC1);
+    gpu.write_buf(0,0,img);
+    gpu.write_buf(0,1,img);
+    cl_float3 pose[1]={0.0};
+    gpu.write_buf(0,2,1,pose);
+    gpu.write_buf(0,3,1,pose);
+    cl_float3 f[300]={0};
+    gpu.write_buf(0,4,300,f);
+    cl_float2 px[300]={0};
+    gpu.write_buf(0,5,300,px);
+    float  e[300]={0};
+    gpu.write_buf(0,7,300,e);
+    float H[300*9]={0};
+    gpu.write_buf(0,8,9*300,H);
+    cl_float3 J[300];
+    gpu.write_buf(0,9,300,J);
+    float chi2[300]={0.01};
+    gpu.write_buf(0,10,300,chi2);
+    for(int i=0;i<10;++i){
+        vio::SparseImgAlignGpu img_align(4, 2,30, vio::SparseImgAlignGpu::GaussNewton, true,&gpu);
+        img_align.run();
+    }
+    gpu.clear_buf();
+    std::cerr<<"finished\n";
+    return 0;
+}
@@ -0,0 +1,202 @@
+/*
+ * math_utils.cpp
+ *
+ *  Created on: Jul 20, 2012
+ *      Author: cforster
+ */
+
+#include <math_utils.h>
+
+namespace vk {
+
+using namespace Eigen;
+
+Vector3d
+triangulateFeatureNonLin(const Matrix3d& R,  const Vector3d& t,
+                         const Vector3d& feature1, const Vector3d& feature2 )
+{
+  Vector3d f2 = R * feature2;
+  Vector2d b;
+  b[0] = t.dot(feature1);
+  b[1] = t.dot(f2);
+  Matrix2d A;
+  A(0,0) = feature1.dot(feature1);
+  A(1,0) = feature1.dot(f2);
+  A(0,1) = -A(1,0);
+  A(1,1) = -f2.dot(f2);
+  Vector2d lambda = A.inverse() * b;
+  Vector3d xm = lambda[0] * feature1;
+  Vector3d xn = t + lambda[1] * f2;
+  return ( xm + xn )/2;
+}
+
+bool
+depthFromTriangulationExact(
+    const Matrix3d& R_r_c,
+    const Vector3d& t_r_c,
+    const Vector3d& f_r,
+    const Vector3d& f_c,
+    double& depth_in_r,
+    double& depth_in_c)
+{
+  // bearing vectors (f_r, f_c) do not need to be unit length
+  const Vector3d f_c_in_r(R_r_c*f_c);
+  const double a = f_c_in_r.dot(f_r) / t_r_c.dot(f_r);
+  const double b = f_c_in_r.dot(t_r_c);
+  const double denom = (a*b - f_c_in_r.dot(f_c_in_r));
+
+  if(abs(denom) < 0.000001)
+    return false;
+
+  depth_in_c = (b-a*t_r_c.dot(t_r_c)) / denom;
+  depth_in_r = (t_r_c + f_c_in_r*depth_in_c).norm();
+  return true;
+}
+
+double
+reprojError(const Vector3d& f1,
+            const Vector3d& f2,
+            double error_multiplier2)
+{
+  Vector2d e = project2d(f1) - project2d(f2);
+  return error_multiplier2 * e.norm();
+}
+
+double
+computeInliers(const vector<Vector3d>& features1, // c1
+               const vector<Vector3d>& features2, // c2
+               const Matrix3d& R,                 // R_c1_c2
+               const Vector3d& t,                 // c1_t
+               const double reproj_thresh,
+               double error_multiplier2,
+               vector<Vector3d>& xyz_vec,         // in frame c1
+               vector<int>& inliers,
+               vector<int>& outliers)
+{
+  inliers.clear(); inliers.reserve(features1.size());
+  outliers.clear(); outliers.reserve(features1.size());
+  xyz_vec.clear(); xyz_vec.reserve(features1.size());
+  double tot_error = 0;
+  //triangulate all features and compute reprojection errors and inliers
+  for(size_t j=0; j<features1.size(); ++j)
+  {
+    xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j], features2[j] ));
+    double e1 = reprojError(features1[j], xyz_vec.back(), error_multiplier2);
+    double e2 = reprojError(features2[j], R.transpose()*(xyz_vec.back()-t), error_multiplier2);
+    if(e1 > reproj_thresh || e2 > reproj_thresh)
+      outliers.push_back(j);
+    else
+    {
+      inliers.push_back(j);
+      tot_error += e1+e2;
+    }
+  }
+  return tot_error;
+}
+
+void
+computeInliersOneView(const vector<Vector3d> & feature_sphere_vec,
+                      const vector<Vector3d> & xyz_vec,
+                      const Matrix3d &R,
+                      const Vector3d &t,
+                      const double reproj_thresh,
+                      const double error_multiplier2,
+                      vector<int>& inliers,
+                      vector<int>& outliers)
+{
+  inliers.clear(); inliers.reserve(xyz_vec.size());
+  outliers.clear(); outliers.reserve(xyz_vec.size());
+  for(size_t j = 0; j < xyz_vec.size(); j++ )
+  {
+    double e = reprojError(feature_sphere_vec[j],
+                           R.transpose() * ( xyz_vec[j] - t ),
+                           error_multiplier2);
+    if(e < reproj_thresh)
+      inliers.push_back(j);
+    else
+      outliers.push_back(j);
+  }
+}
+
+Vector3d
+dcm2rpy(const Matrix3d &R)
+{
+  Vector3d rpy;
+  rpy[1] = atan2( -R(2,0), sqrt( pow( R(0,0), 2 ) + pow( R(1,0), 2 ) ) );
+  if( fabs( rpy[1] - M_PI/2 ) < 0.00001 )
+  {
+    rpy[2] = 0;
+    rpy[0] = -atan2( R(0,1), R(1,1) );
+  }
+  else
+  {
+    if( fabs( rpy[1] + M_PI/2 ) < 0.00001 )
+    {
+      rpy[2] = 0;
+      rpy[0] = -atan2( R(0,1), R(1,1) );
+    }
+    else
+    {
+      rpy[2] = atan2( R(1,0)/cos(rpy[1]), R(0,0)/cos(rpy[1]) );
+      rpy[0] = atan2( R(2,1)/cos(rpy[1]), R(2,2)/cos(rpy[1]) );
+    }
+  }
+  return rpy;
+}
+
+Matrix3d
+rpy2dcm(const Vector3d &rpy)
+{
+  Matrix3d R1;
+  R1(0,0) = 1.0; R1(0,1) = 0.0; R1(0,2) = 0.0;
+  R1(1,0) = 0.0; R1(1,1) = cos(rpy[0]); R1(1,2) = -sin(rpy[0]);
+  R1(2,0) = 0.0; R1(2,1) = -R1(1,2); R1(2,2) = R1(1,1);
+
+  Matrix3d R2;
+  R2(0,0) = cos(rpy[1]); R2(0,1) = 0.0; R2(0,2) = sin(rpy[1]);
+  R2(1,0) = 0.0; R2(1,1) = 1.0; R2(1,2) = 0.0;
+  R2(2,0) = -R2(0,2); R2(2,1) = 0.0; R2(2,2) = R2(0,0);
+
+  Matrix3d R3;
+  R3(0,0) = cos(rpy[2]); R3(0,1) = -sin(rpy[2]); R3(0,2) = 0.0;
+  R3(1,0) = -R3(0,1); R3(1,1) = R3(0,0); R3(1,2) = 0.0;
+  R3(2,0) = 0.0; R3(2,1) = 0.0; R3(2,2) = 1.0;
+
+  return R3 * R2 * R1;
+}
+
+Quaterniond
+angax2quat(const Vector3d& n, const double& angle)
+{
+  // n must be normalized!
+  double s(sin(angle/2));
+  return Quaterniond( cos(angle/2), n[0]*s, n[1]*s, n[2]*s );
+}
+
+
+Matrix3d
+angax2dcm(const Vector3d& n, const double& angle)
+{
+  // n must be normalized
+  Matrix3d sqewn(sqew(n));
+  return Matrix3d(Matrix3d::Identity() + sqewn*sin(angle) + sqewn*sqewn*(1-cos(angle)));
+}
+
+double
+sampsonusError(const Vector2d &v2Dash, const Matrix3d& Essential, const Vector2d& v2)
+{
+  Vector3d v3Dash = unproject2d(v2Dash);
+  Vector3d v3 = unproject2d(v2);
+
+  double dError = v3Dash.transpose() * Essential * v3;
+
+  Vector3d fv3 = Essential * v3;
+  Vector3d fTv3Dash = Essential.transpose() * v3Dash;
+
+  Vector2d fv3Slice = fv3.head<2>();
+  Vector2d fTv3DashSlice = fTv3Dash.head<2>();
+
+  return (dError * dError / (fv3Slice.dot(fv3Slice) + fTv3DashSlice.dot(fTv3DashSlice)));
+}
+
+} // end namespace vk
@@ -0,0 +1,166 @@
+/**
+* This file is part of dvo.
+*
+* Copyright 2012 Christian Kerl <[email protected]> (Technical University of Munich)
+* For more information see <http://vision.in.tum.de/data/software/dvo>.
+*
+* dvo is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* dvo is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with dvo. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <numeric>
+#include <algorithm>
+#include <iostream>
+#include <robust_cost.h>
+#include <math_utils.h>
+
+namespace vk {
+namespace robust_cost {
+
+const float TDistributionScaleEstimator::INITIAL_SIGMA = 5.0f;
+const float TDistributionScaleEstimator::DEFAULT_DOF = 5.0f;
+
+TDistributionScaleEstimator::
+TDistributionScaleEstimator(const float dof) :
+  dof_(dof),
+  initial_sigma_(INITIAL_SIGMA)
+{}
+
+float TDistributionScaleEstimator::
+compute(std::vector<float>& errors) const
+{
+    std::cerr<<"42\n";
+  float initial_lamda = 1.0f / (initial_sigma_ * initial_sigma_);
+  int num = 0;
+  float lambda = initial_lamda;
+  int iterations = 0;
+  do
+  {
+    ++iterations;
+    initial_lamda = lambda;
+    num = 0;
+    lambda = 0.0f;
+
+    for(std::vector<float>::iterator it=errors.begin(); it!=errors.end(); ++it)
+    {
+      if(std::isfinite(*it))
+      {
+        ++num;
+        const float error2 = (*it)*(*it);
+        lambda += error2 * ( (dof_ + 1.0f) / (dof_ + initial_lamda * error2) );
+      }
+    }
+    lambda = float(num) / lambda;
+  } while(std::abs(lambda - initial_lamda) > 1e-3);
+
+  return std::sqrt(1.0f / lambda);
+}
+
+const float MADScaleEstimator::NORMALIZER = 1.48f; // 1 / 0.6745
+
+float MADScaleEstimator::
+compute(std::vector<float>& errors) const
+{
+    std::cerr<<"74\n";
+  // error must be in absolute values!
+  return NORMALIZER * vk::getMedian(errors);
+}
+
+float NormalDistributionScaleEstimator::
+compute(std::vector<float>& errors) const
+{
+    std::cerr<<"82\n";
+  const float mean = std::accumulate(errors.begin(), errors.end(), 0)/errors.size();
+  float var = 0.0;
+  std::for_each(errors.begin(), errors.end(), [&](const float d) {
+      var += (d - mean) * (d - mean);
+  });
+  return std::sqrt(var); // return standard deviation
+}
+
+const float TukeyWeightFunction::DEFAULT_B = 4.6851f;
+
+TukeyWeightFunction::TukeyWeightFunction(const float b)
+{
+  configure(b);
+}
+
+float TukeyWeightFunction::value(const float& x) const
+{
+  const float x_square = x * x;
+  if(x_square <= b_square)
+  {
+    const float tmp = 1.0f - x_square / b_square;
+    return tmp * tmp;
+  }
+  else
+  {
+    return 0.0f;
+  }
+}
+
+void TukeyWeightFunction::
+configure(const float& param)
+{
+  b_square = param * param;
+}
+
+const float TDistributionWeightFunction::DEFAULT_DOF = 5.0f;
+
+TDistributionWeightFunction::
+TDistributionWeightFunction(const float dof)
+{
+  configure(dof);
+}
+
+float TDistributionWeightFunction::
+value(const float & x) const
+{
+  return ((dof_ + 1.0f) / (dof_ + (x * x)));
+}
+
+void TDistributionWeightFunction::
+configure(const float& param)
+{
+  dof_ = param;
+  normalizer_ = dof_ / (dof_ + 1.0f);
+}
+
+const float HuberWeightFunction::DEFAULT_K = 1.345f;
+
+HuberWeightFunction::
+HuberWeightFunction(const float k)
+{
+  configure(k);
+}
+
+void HuberWeightFunction::
+configure(const float& param)
+{
+  k = param;
+}
+
+float HuberWeightFunction::
+value(const float& t) const
+{
+  const float t_abs = std::abs(t);
+  if(t_abs < k)
+    return 1.0f;
+  else
+    return k / t_abs;
+}
+
+} // namespace robust_cost
+} // namespace vk
+
+
@@ -0,0 +1,268 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <sparse_img_align_gpu.h>
+
+namespace vio {
+
+SparseImgAlignGpu::SparseImgAlignGpu(
+    int max_level, int min_level, int n_iter,
+    Method method, bool verbose,opencl* residual) :
+        max_level_(max_level),
+        min_level_(min_level),
+        residual_(residual)
+{
+  n_iter_ = n_iter;
+  n_iter_init_ = n_iter_;
+  method_ = method;
+  verbose_ = verbose;
+  eps_ = 1e-10;
+//  data = fopen((std::string(getenv("HOME"))+"/Project/data.txt").c_str(), "w+");
+}
+int SparseImgAlignGpu::feature_counter(std::string filename) {
+    int counter = 0;
+    std::string line;
+    std::ifstream myfile (filename);
+    if (myfile.is_open()) {
+        getline(myfile, line);
+        while(line.find("features") == std::string::npos)
+            getline(myfile, line);
+        while (!myfile.eof()) {
+            getline(myfile, line);
+            counter ++;
+        }
+        myfile.close();
+    }
+    else
+        std::cout << "Unable to open file";
+    return counter;
+}
+void SparseImgAlignGpu::data_reader(std::string filename, float *rp, float *cp, float *c, float *fts, float *fts_px) {
+    std::string line;
+    std::ifstream myfile (filename);
+    if (myfile.is_open()) {
+        while (true) {
+            getline(myfile, line);
+            if (line.find("ref_frame") != std::string::npos) {
+                getline(myfile, line);
+                std::string substring;
+                std::stringstream ss(line);
+                getline(ss, substring, ',');
+                *(rp) = atof(substring.c_str());
+                getline(ss, substring, ',');
+                *(rp + 1) = atof(substring.c_str());
+                getline(ss, substring, ',');
+                *(rp + 2) = atof(substring.c_str());
+            } else if (line.find("cur_frame") != std::string::npos) {
+                getline(myfile, line);
+                std::string substring;
+                std::stringstream ss(line);
+                getline(ss, substring, ',');
+                *(cp) = atof(substring.c_str());
+                getline(ss, substring, ',');
+                *(cp + 1) = atof(substring.c_str());
+                getline(ss, substring, ',');
+                *(cp + 2) = atof(substring.c_str());
+            } else if (line.find("camera") != std::string::npos) {
+                getline(myfile, line);
+                std::string substring;
+                std::stringstream ss(line);
+                getline(ss, substring, ',');
+                *(c) = atof(substring.c_str());
+                getline(ss, substring, ',');
+                *(c + 1) = atof(substring.c_str());
+                getline(ss, substring, ',');
+                *(c + 2) = atof(substring.c_str());
+                getline(ss, substring, ',');
+                *(c + 3) = atof(substring.c_str());
+                getline(ss, substring, ',');
+                *(c + 4) = atof(substring.c_str());
+            }
+            else{
+                break;
+            }
+        }
+        if (line.find("features") != std::string::npos) {
+            int fts_i = 0;
+            int fts_px_i = 0;
+            while (!myfile.eof()) {
+                getline(myfile, line);
+                std::string substring;
+                std::stringstream ss(line);
+                getline(ss, substring, ',');
+                *(fts + (fts_i)) = atof(substring.c_str());
+                getline(ss, substring, ',');
+                *(fts + (fts_i + 1)) = atof(substring.c_str());
+                getline(ss, substring, ',');
+                *(fts + (fts_i + 2)) = atof(substring.c_str());
+                fts_i += 3;
+                getline(ss, substring, ',');
+                *(fts_px + (fts_px_i)) = atof(substring.c_str());
+                getline(ss, substring, ',');
+                *(fts_px + (fts_px_i + 1)) = atof(substring.c_str());
+                fts_px_i += 2;
+            }
+        }
+        myfile.close();
+    }
+    else
+        std::cout << "Unable to open file";
+    return;
+}
+/*void draw(double* points, int f) {
+    for (int level = 2; level <= 4; level++) {
+        double scale = pow(2.0, -level);
+        cv::Mat ref_img = cv::imread(std::string(TEST_DATA_DIR)+"/fer"+ std::to_string(level)+".png");
+        for (int i = 0; i < f; i++) {
+            cv::Point p;
+            p.x = points[2*i] * scale;
+            p.y = points[2*i+1] * scale;
+            cv::circle(ref_img, p, 1, (0, 0, 255));
+        }
+        cv::imwrite("/root/Projects/p_25_vio_opencl/Vio_OpenCL/test/res/ref"+ std::to_string(level)+".png",ref_img);
+    }
+}*/
+size_t SparseImgAlignGpu::run()
+{
+    std::string datafilename = std::string(TEST_DATA_DIR)+"/data.txt";
+    int fts_size = feature_counter(datafilename);
+    float* fts = new float[3 * fts_size];
+    float* fts_px = new float[2 * fts_size];
+  reset();
+    float camera[5];
+    float ref_pos[3];
+    float cur_pos[3];
+    data_reader(datafilename, ref_pos, cur_pos, camera, fts, fts_px);
+    cl_float3 ref_[1],cur_[1];
+    ref_[0].x=ref_pos[0];
+    ref_[0].y=ref_pos[1];
+    ref_[0].z=ref_pos[2];
+    cur_[0].x=cur_pos[0];
+    cur_[0].y=cur_pos[1];
+    cur_[0].z=cur_pos[2];
+  residual_->reload_buf(0,2,cur_);
+  residual_->reload_buf(0,3,ref_);
+  cl_float3 features[fts_size];
+  cl_float2 featue_px[fts_size];
+
+    for (int i = 0; i < fts_size; ++i) {
+        features[i].x = fts[3*i];
+        features[i].y = fts[3*i+1];
+        features[i].z = fts[3*i+2];
+    }
+    for (int i = 0; i < fts_size; ++i) {
+        featue_px[i].x = fts_px[2*i];
+        featue_px[i].y = fts_px[2*i+1];
+    }
+
+  feature_counter_ = fts_size; // is used to compute the index of the cached jacobian
+  level_=4;
+  residual_->reload_buf(0,4,features);
+  residual_->reload_buf(0,5,featue_px);
+  Sophus::SE2 T_cur(cur_pos[2],Eigen::Vector2d(cur_pos[0],cur_pos[1]));///TODO temporary, we can remove it
+  for(level_ = max_level_; level_ >= min_level_; --level_)
+  {
+      cv::Mat cur_img = cv::imread(std::string(TEST_DATA_DIR)+"/cur"+ std::to_string(level_)+".png");
+      cv::Mat ref_img = cv::imread(std::string(TEST_DATA_DIR)+"/fer"+ std::to_string(level_)+".png");
+      residual_->reload_buf(0,0,cur_img);
+      residual_->reload_buf(0,1,ref_img);
+      residual_->write_buf(0,6,level_);
+      mu_ = 1.0;
+      optimize(T_cur);
+  }
+  cl_float3 pos[1]={0};
+  residual_->read(0,2,pos);
+  std::cerr<<"pos : "<<pos[0].x<<"\t"<<pos[0].y<<"\t"<<pos[0].z<<std::endl;
+  std::cerr<<pos[0].x - ref_pos[0]<<"\t "<<pos[0].y- ref_pos[1]<<"\t "<<pos[0].z- ref_pos[2]<<std::endl;
+  return 1;
+}
+
+double SparseImgAlignGpu::computeResiduals(
+    bool linearize_system,
+    bool compute_weight_scale)
+{
+    float H[9*300]={0};
+    cl_float3 J[300]={0.0};
+    float chi2_[300]={0.0};
+    residual_->write_buf(0,11,scale_);
+    residual_->reload_buf(0,10,chi2_);
+    residual_->reload_buf(0,8,H);
+    residual_->reload_buf(0,9,J);
+    /// TODO GPU compute based on the feature
+    residual_->run(0,feature_counter_);
+    float error[300]={0.0};
+    float chi[300]={0.0};
+    residual_->read(0,10,feature_counter_,chi);
+    residual_->read(0,7,feature_counter_,error);
+    for(int i = 1; i < feature_counter_; ++i){
+        error[0] += error[i];
+        chi[0] += chi[i];
+    }
+    scale_ =error[0];
+    return chi[0];
+}
+
+int SparseImgAlignGpu::solve()
+{
+    float H[300*9]={0.0};
+    cl_float3 J[300]={0.0};
+    residual_->read(0,8,feature_counter_*9,H);
+    residual_->read(0,9,feature_counter_,J);
+    for(int i = 1; i < feature_counter_; ++i){
+        H[0] += H[i*9];
+        H[1] += H[i*9+1];
+        H[2] += H[i*9+2];
+        H[3] += H[i*9+3];
+        H[4] += H[i*9+4];
+        H[5] += H[i*9+5];
+        H[6] += H[i*9+6];
+        H[7] += H[i*9+7];
+        H[8] += H[i*9+8];
+        J[0].x -= J[i].x;
+        J[0].y -= J[i].y;
+        J[0].z -= J[i].z;
+    }
+    double Hd[9]={(double)H[0],(double)H[1],(double)H[2],
+                  (double)H[3],(double)H[4],(double)H[5],
+                  (double)H[6],(double)H[7],(double)H[8]};
+    double Jd[3]={(double)J[0].x,(double)J[0].y,(double)J[0].z};
+    x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d(Jd[0],Jd[1],Jd[2]));
+    if((bool) std::isnan((double) x_[0]))
+        return 0;
+    return 1;
+}
+void SparseImgAlignGpu::update()
+{
+/// TODO the update situation may have a smarter solution
+/// TODO the update situation may have a smarter solution
+    cl_float3 pos[1]={0.0,0.0,0.0};
+    residual_->read(0,2,pos);
+    Sophus::SE2 update =  Sophus::SE2(pos[0].z,Eigen::Vector2d(pos[0].x,pos[0].y)) * Sophus::SE2::exp(-1.0*x_);
+    pos[0].x=(float)update.translation()(0);
+    pos[0].y=(float)update.translation()(1);
+    pos[0].z=(float)atan(update.so2().unit_complex().imag()/update.so2().unit_complex().real());
+    residual_->reload_buf(0,2,pos);
+}
+
+void SparseImgAlignGpu::startIteration()
+{}
+
+void SparseImgAlignGpu::finishIteration()
+{
+}
+
+} // namespace vio
+
@@ -0,0 +1,85 @@
+ref_frame:
+ -0.326890,0.009666,1.254757
+cur_frame:
+-0.326890,0.009666,1.254757
+camera:
+296.724480,395.357280,317.877600,238.752640,1.016870
+ features:
+-0.649845,-0.351804,0.548849,65.626709,56.799358
+-0.648425,-0.352782,0.551745,66.660110,56.643040
+0.538037,-0.545661,0.900806,473.341705,28.676302
+0.538337,-0.542014,0.901982,473.459686,30.038052
+-0.611747,-0.287029,0.511483,60.513573,77.859436
+0.505807,-0.460219,0.892039,470.488708,53.739651
+-0.391810,-0.176149,0.489420,111.763100,115.286110
+-0.255603,-0.058511,0.362289,124.788757,179.859940
+-0.254965,-0.057964,0.363864,125.720444,180.545792
+-0.481319,-0.155208,0.936546,167.409195,174.103531
+-0.097664,-0.138420,1.040516,287.678375,181.723557
+-0.576794,-0.204837,1.112729,166.636932,167.188980
+-0.347883,-0.176067,1.225530,229.154205,178.922424
+-0.353727,-0.149093,1.185631,224.753128,186.454300
+-0.351608,-0.145035,1.177231,224.635208,187.506256
+-0.356308,-0.136734,1.151653,221.485016,189.465897
+-0.347270,-0.084129,1.254798,230.716705,210.618484
+-0.271202,-0.049082,1.219884,247.042603,221.671478
+0.551335,-0.024829,0.900301,492.609039,228.268005
+0.570153,-0.026209,0.918852,494.399292,227.940765
+0.593849,-0.027990,0.943331,496.370331,227.543304
+0.538455,0.042511,0.896662,489.880554,256.846283
+-0.410921,0.199957,1.465923,230.130569,295.644165
+-0.418205,0.205574,1.478655,229.414886,296.692078
+0.706988,0.340967,1.050721,500.369568,356.020905
+-1.773052,0.703438,2.013429,97.468231,355.264709
+-0.507049,0.394948,1.237993,197.115158,364.083618
+-0.505709,0.397308,1.246433,198.122162,364.112518
+-0.046137,0.358122,1.028613,303.971924,382.569611
+-0.042918,0.358798,1.030556,304.965149,382.584106
+-0.026664,0.361984,1.039650,309.922577,382.648041
+-0.023372,0.362622,1.041471,310.916443,382.657440
+-0.046169,0.360681,1.028121,303.965210,383.568085
+-0.042947,0.361373,1.030094,304.959534,383.582489
+-0.023289,0.400521,1.039641,310.996948,396.418610
+-0.023522,0.397142,1.036895,310.905396,395.598389
+-0.520659,0.499070,1.226407,195.651825,394.854034
+-0.518963,0.501603,1.233899,196.654175,394.868164
+-0.522851,0.510476,1.242953,196.763367,396.306335
+-0.618554,-0.289543,0.509116,58.533249,77.001205
+-0.567769,-0.273975,0.557984,81.440971,86.736328
+-0.570194,-0.275719,0.552642,79.710831,85.304115
+-0.614502,-0.284236,0.507294,58.608025,78.964867
+-0.609955,-0.284518,0.510723,60.550095,78.821106
+-0.631422,-0.320436,0.530292,63.085056,66.468994
+-0.320845,-0.398865,1.298285,242.146652,113.311325
+0.489002,-0.308087,0.873590,474.750366,107.064598
+-0.065107,-0.329243,1.251848,301.440216,127.998520
+-0.070308,-0.263461,1.136162,298.211243,140.561432
+-0.404364,-0.200603,1.105117,206.199829,164.933563
+-0.262542,-0.061586,0.381638,128.352280,179.516068
+-0.279101,-0.068491,0.423297,134.297928,178.727234
+-0.268416,-0.063972,0.398420,131.273529,179.495895
+-0.495678,-0.162953,0.959804,166.863388,172.604675
+-0.411479,-0.175127,1.136268,206.882751,175.810242
+-0.420565,-0.122517,0.921132,181.328644,185.751114
+-0.408601,-0.119380,0.905473,182.686615,186.124893
+-0.522424,-0.158378,1.018227,167.462585,177.995071
+-0.436577,-0.125475,0.961936,181.999451,186.719391
+0.763324,-0.179506,1.278473,488.213959,185.380585
+0.735538,-0.175594,1.222357,489.217255,184.252213
+-0.531641,-0.135054,1.152132,179.896881,192.049866
+-0.153214,-0.064019,1.125556,273.996948,214.322815
+-0.250853,-0.022721,1.188315,250.454727,230.615723
+-0.269293,-0.049531,1.232366,248.207672,221.678802
+0.689335,0.138668,1.360214,467.413635,278.832611
+-0.315660,0.228150,1.827959,262.734253,291.856873
+-1.722023,0.597785,1.949498,95.677032,341.527618
+-2.017433,0.720874,2.317486,97.949333,343.460022
+-0.394809,-0.185239,0.488969,111.179558,109.535812
+0.503526,-0.272907,0.866554,480.762085,121.125191
+-0.386096,-0.238207,1.189135,218.027527,156.671371
+-0.358741,-0.249081,1.196880,225.289307,153.097733
+0.392533,-0.493985,0.969992,431.760529,47.796806
+-0.073172,-0.350268,1.270865,299.731842,123.017418
+-0.802860,-0.809967,1.735968,188.040833,64.225998
+0.489783,-0.513333,0.903382,462.721436,36.482620
+-0.414553,-0.168965,0.471132,97.895798,119.288155