add banchmark image alignment 2

include/aligned_mem.h

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

include/math_utils.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_ */

include/nlls_solver.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_ */

include/nlls_solver_impl.hpp

@@ -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_ */

include/robust_cost.h

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

include/sparse_img_align_gpu.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_

include/vision.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_