initial_homography_estimation.hpp

#ifndef __fovis_initial_homography_estimator_hpp__
#define __fovis_initial_homography_estimator_hpp__

#include <vector>
#include <stdint.h>
#include <Eigen/Dense>

namespace fovis
{

class InitialHomographyEstimator {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
  void setTemplateImage(const uint8_t * grayData, int width, int height, int stride, int downsampleFactor);

  void setTestImage(const uint8_t * grayData, int width, int height, int stride, int downsampleFactor);

  Eigen::Matrix3f track(const Eigen::Matrix3f &init_H, int nIters, double *finalRMS);

private:
  int template_rows, template_cols; //size of the template
  Eigen::MatrixXf templateImage, testImage; //the images
  Eigen::MatrixXf warpedTestImage; //storage for the warped testImage on the current iteration
  Eigen::MatrixXf errorIm; //the difference between the templateImage and the warpedTestImage
  Eigen::ArrayXf templateDxRow, templateDyRow; //the gradient of the template flattened into a row vector
  Eigen::MatrixXf templatePoints; //nx3 matrix storing the x,y,1 for each pixel
  Eigen::ArrayXf xx, yy; //convenience array storing the original x/y for each pixel in the flattened image

  //internal functions

  static void computeGradient(const Eigen::MatrixXf &image, Eigen::MatrixXf * dxp, Eigen::MatrixXf *dyp);

  static double computeError(const Eigen::MatrixXf &error);

  Eigen::MatrixXf computeJacobian(const Eigen::ArrayXf &dx, const Eigen::ArrayXf &dy) const;

  Eigen::Matrix3f lieToH(const Eigen::VectorXf &lie) const;

  Eigen::MatrixXf constructWarpedImage(const Eigen::MatrixXf &srcImage, const Eigen::MatrixXf &warpedPoints) const;

  static Eigen::ArrayXf flattenMatrix(Eigen::MatrixXf &m);

};

}

#endif