MTF
imgUtils.h
1 #ifndef MTF_IMG_UTILS_H
2 #define MTF_IMG_UTILS_H
3 
4 #include "mtf/Macros/common.h"
5 #include "mtf/Utilities/excpUtils.h"
6 
7 #ifndef PIX_INTERP_TYPE
8 #define PIX_INTERP_TYPE utils::InterpType::Linear
9 #define DEFAULT_PIX_INTERP_TYPE
10 #endif
11 
12 #ifndef GRAD_INTERP_TYPE
13 #define GRAD_INTERP_TYPE utils::InterpType::Linear
14 #define DEFAULT_GRAD_INTERP_TYPE
15 #endif
16 
17 #ifndef HESS_INTERP_TYPE
18 #define HESS_INTERP_TYPE utils::InterpType::Linear
19 #define DEFAULT_HESS_INTERP_TYPE
20 #endif
21 
22 #ifndef PIX_BORDER_TYPE
23 #define PIX_BORDER_TYPE utils::BorderType::Constant
24 #define DEFAULT_PIX_BORDER_TYPE
25 #endif
26 
27 #ifndef GRAD_EPS
28 #define GRAD_EPS 1e-8
29 #endif
30 
31 #ifndef HESS_EPS
32 #define HESS_EPS 1
33 #endif
34 
35 _MTF_BEGIN_NAMESPACE
36 
37 namespace utils{
38  // types of interpolation
39  enum class InterpType { Nearest, Linear, Cubic, Cubic2, CubicBSpl };
40  enum class BorderType{ Constant, Replicate };
41  const char* toString(InterpType interp_type);
42  const char* toString(BorderType border_type);
43  const char* typeToString(int img_type);
44  inline const char* getType(const cv::Mat& mat){
45  return typeToString(mat.type());
46  }
47 
48  /******* functions for extracting pixel values from image ***********/
49 
50  // check if the given x, y coordinates are outside the extents of an image with the given height and width
51  inline bool checkOverflow(double x, double y, unsigned int h, unsigned int w){
52  return ((x < 0) || (x >= w) || (y < 0) || (y >= h));
53  }
54  // computes the pixel value at the given location expressed in (real) x, y, coordinates
55  template<InterpType interp_type, BorderType border_type>
56  inline double getPixVal(const EigImgT &img, double x, double y,
57  unsigned int h, unsigned int w, double overflow_val = 128.0){
58  throw InvalidArgument(
59  cv::format("getPixVal :: Invalid interpolation type specified: %d",
60  interp_type));
61  }
62  // using nearest neighbor interpolation with constant border
63  template<>
64  inline double getPixVal<InterpType::Nearest, BorderType::Constant>(
65  const EigImgT &img, double x, double y,
66  unsigned int h, unsigned int w, double overflow_val){
67  assert(img.rows() == h && img.cols() == w);
68 
69  if(checkOverflow(x, y, h, w)){
70  return overflow_val;
71  }
72  int nx = static_cast<int>(rint(x));
73  int ny = static_cast<int>(rint(y));
74  return checkOverflow(nx, ny, h, w) ? overflow_val : img(ny, nx);
75  }
76  // using nearest neighbor interpolation with replicated border
77  template<>
78  inline double getPixVal<InterpType::Nearest, BorderType::Replicate>(
79  const EigImgT &img, double x, double y,
80  unsigned int h, unsigned int w, double overflow_val){
81  assert(img.rows() == h && img.cols() == w);
82 
83  if(x > w - 1){ x = w - 1; } else if(x < 0){ x = 0; }
84  if(y > h - 1){ y = h - 1; } else if(y < 0){ y = 0; }
85 
86  int nx = static_cast<int>(rint(x));
87  int ny = static_cast<int>(rint(y));
88  return checkOverflow(nx, ny, h, w) ? overflow_val : img(ny, nx);
89  }
90  // using bilinear interpolation with constant border
91  template<>
92  inline double getPixVal<InterpType::Linear, BorderType::Constant>(
93  const EigImgT &img, double x, double y,
94  unsigned int h, unsigned int w, double overflow_val){
95  assert(img.rows() == h && img.cols() == w);
96  if(checkOverflow(x, y, h, w)){
97  return overflow_val;
98  }
99  int lx = static_cast<int>(x);
100  int ly = static_cast<int>(y);
101  double dx = x - lx;
102  double dy = y - ly;
103  int ux = dx == 0 ? lx : lx + 1;
104  int uy = dy == 0 ? ly : ly + 1;
105 
106  if(checkOverflow(lx, ly, h, w) || checkOverflow(ux, uy, h, w)){
107  return overflow_val;
108  }
109  return img(ly, lx) * (1 - dx)*(1 - dy) +
110  img(ly, ux) * dx*(1 - dy) +
111  img(uy, lx) * (1 - dx)*dy +
112  img(uy, ux) * dx*dy;
113  }
114  // using bilinear interpolation with replicated border
115  template<>
116  inline double getPixVal<InterpType::Linear, BorderType::Replicate>(
117  const EigImgT &img, double x, double y,
118  unsigned int h, unsigned int w, double overflow_val){
119  assert(img.rows() == h && img.cols() == w);
120 
121  if(x > w - 1){ x = w - 1; } else if(x < 0){ x = 0; }
122  if(y > h - 1){ y = h - 1; } else if(y < 0){ y = 0; }
123 
124  int lx = static_cast<int>(x);
125  int ly = static_cast<int>(y);
126  double dx = x - lx;
127  double dy = y - ly;
128  int ux = dx == 0 ? lx : lx + 1;
129  int uy = dy == 0 ? ly : ly + 1;
130 
131  return img(ly, lx) * (1 - dx)*(1 - dy) +
132  img(ly, ux) * dx*(1 - dy) +
133  img(uy, lx) * (1 - dx)*dy +
134  img(uy, ux) * dx*dy;
135  }
136  // using bi cubic interpolation
137  template<>
138  double getPixVal<InterpType::Cubic, BorderType::Constant>(
139  const EigImgT &img, double x, double y,
140  unsigned int h, unsigned int w, double overflow_val);
141  // using cubic BSpline interpolation - an alternate formulation that uses polynomial coefficients
142  template<>
143  double getPixVal<InterpType::Cubic2, BorderType::Constant>(
144  const EigImgT &img, double x, double y,
145  unsigned int h, unsigned int w, double overflow_val);
146  // using cubic BSpline interpolation
147  template<>
148  double getPixVal<InterpType::CubicBSpl, BorderType::Constant>(
149  const EigImgT &img, double x, double y,
150  unsigned int h, unsigned int w, double overflow_val);
151 
152  template<typename PtsT>
153  void getPixVals(VectorXd &pix_vals,
154  const EigImgT &img, const PtsT &pts, unsigned int n_pix, unsigned int h, unsigned int w,
155  double norm_mult = 1, double norm_add = 0);
156 
159  void getWeightedPixVals(VectorXd &pix_vals, const EigImgT &img, const PtsT &pts,
160  unsigned int frame_count, double alpha, bool use_running_avg, unsigned int n_pix,
161  unsigned int h, unsigned int w, double norm_mult, double norm_add);
162 
163  /************ functions for image gradient and Hessian ************/
164  void getWarpedImgGrad(PixGradT &warped_img_grad,
165  const EigImgT &img, const Matrix8Xd &warped_offset_pts,
166  double grad_eps, unsigned int n_pix,
167  unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
168  template<InterpType mapping_type>
169  void getWarpedImgGrad(PixGradT &warped_img_grad,
170  const EigImgT &img, const VectorXd &intensity_map,
171  const Matrix8Xd &warped_offset_pts,
172  double grad_eps, unsigned int n_pix, unsigned int h, unsigned int w,
173  double pix_mult_factor = 1.0);
174  void getWarpedImgGrad(PixGradT &warped_img_grad, const EigImgT &img, bool weighted_mapping,
175  const VectorXd &intensity_map, const Matrix8Xd &warped_offset_pts,
176  double grad_eps, unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
177  void getImgGrad(PixGradT &img_grad,
178  const EigImgT &img, const PtsT &pts,
179  double grad_eps, unsigned int n_pix, unsigned int h, unsigned int w,
180  double pix_mult_factor = 1.0);
181  // mapping enabled version
182  template<InterpType mapping_type>
183  void getImgGrad(PixGradT &img_grad, const EigImgT &img,
184  const VectorXd &intensity_map, const PtsT &pts,
185  double grad_eps, unsigned int n_pix, unsigned int h, unsigned int w,
186  double pix_mult_factor = 1.0);
187  void getImgGrad(PixGradT &img_grad, const EigImgT &img,
188  bool weighted_mapping, const VectorXd &intensity_map,
189  const PtsT &pts, double grad_eps, unsigned int n_pix, unsigned int h, unsigned int w,
190  double pix_mult_factor = 1.0);
191  void getWarpedImgHess(PixHessT &warped_img_hess,
192  const EigImgT &img, const PtsT &warped_pts,
193  const Matrix16Xd &warped_offset_pts, double hess_eps, unsigned int n_pix,
194  unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
195  // mapped version
196  template<InterpType mapping_type>
197  void getWarpedImgHess(PixHessT &warped_img_hess,
198  const EigImgT &img, const VectorXd &intensity_map,
199  const PtsT &warped_pts, const Matrix16Xd &warped_offset_pts,
200  double hess_eps, unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
201  void getWarpedImgHess(PixHessT &warped_img_hess, const EigImgT &img,
202  bool weighted_mapping, const VectorXd &intensity_map,
203  const PtsT &warped_pts, const Matrix16Xd &warped_offset_pts,
204  double hess_eps, unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
205  void getImgHess(PixHessT &img_hess, const EigImgT &img,
206  const PtsT &pts, double hess_eps,
207  unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
208  template<InterpType mapping_type>
209  void getImgHess(PixHessT &img_hess, const EigImgT &img, const VectorXd &intensity_map,
210  const PtsT &pts, double hess_eps, unsigned int n_pix, unsigned int h, unsigned int w,
211  double pix_mult_factor = 1.0);
212  void getImgHess(PixHessT &img_hess, const EigImgT &img, bool weighted_mapping,
213  const VectorXd &intensity_map, const PtsT &pts, double hess_eps, unsigned int n_pix,
214  unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
215  // fits a bi cubic polynomial surface at each pixel location and computes the analytical hessian
216  // of this surface thus eliminating the need for finite difference approximations
217  void getImgHess(PixHessT &img_hess, const EigImgT &img,
218  const PtsT &pts, unsigned int n_pix, unsigned int h, unsigned int w);
219 
220  namespace sc{
221 
222  // *************************************************************************************** //
223  // ***** functions for single channel images that work directly on OpenCV Mat images ***** //
224  // *************************************************************************************** //
225 
226  template<typename ScalarType, InterpType interp_type, BorderType border_type>
227  struct PixVal{
228  static inline double get(const cv::Mat &img, double x, double y,
229  unsigned int h, unsigned int w, double overflow_val = 128.0){
230  throw InvalidArgument(
231  cv::format("get :: Invalid interpolation type specified: %d",
232  interp_type));
233  }
234  };
235  // using nearest neighbor interpolation with constant border
236  template<typename ScalarType>
237  struct PixVal < ScalarType, InterpType::Nearest, BorderType::Constant > {
238  static inline double get(
239  const cv::Mat &img, double x, double y,
240  unsigned int h, unsigned int w, double overflow_val = 128.0){
241  assert(img.rows == h && img.cols == w);
242 
243  if(checkOverflow(x, y, h, w)){
244  return overflow_val;
245  }
246  int nx = static_cast<int>(rint(x));
247  int ny = static_cast<int>(rint(y));
248  return checkOverflow(nx, ny, h, w) ? overflow_val : img.at<ScalarType>(ny, nx);
249  }
250  };
251  // using nearest neighbor interpolation with replicated border
252  template<typename ScalarType>
253  struct PixVal < ScalarType, InterpType::Nearest, BorderType::Replicate > {
254  static inline double get(
255  const cv::Mat &img, double x, double y,
256  unsigned int h, unsigned int w, double overflow_val = 128.0){
257  assert(img.rows == h && img.cols == w);
258 
259  if(x > w - 1){ x = w - 1; } else if(x < 0){ x = 0; }
260  if(y > h - 1){ y = h - 1; } else if(y < 0){ y = 0; }
261 
262  int nx = static_cast<int>(rint(x));
263  int ny = static_cast<int>(rint(y));
264  return checkOverflow(nx, ny, h, w) ? overflow_val : img.at<ScalarType>(ny, nx);
265  }
266  };
267  // using bilinear interpolation with constant border
268  template<typename ScalarType>
269  struct PixVal < ScalarType, InterpType::Linear, BorderType::Constant > {
270  static inline double get(
271  const cv::Mat &img, double x, double y,
272  unsigned int h, unsigned int w, double overflow_val = 128.0){
273  assert(img.rows == h && img.cols == w);
274  if(checkOverflow(x, y, h, w)){
275  return overflow_val;
276  }
277  int lx = static_cast<int>(x);
278  int ly = static_cast<int>(y);
279  double dx = x - lx;
280  double dy = y - ly;
281  int ux = dx == 0 ? lx : lx + 1;
282  int uy = dy == 0 ? ly : ly + 1;
283 
284  if(checkOverflow(lx, ly, h, w) || checkOverflow(ux, uy, h, w)){
285  return overflow_val;
286  }
287  return img.at<ScalarType>(ly, lx) * (1 - dx)*(1 - dy) +
288  img.at<ScalarType>(ly, ux) * dx*(1 - dy) +
289  img.at<ScalarType>(uy, lx) * (1 - dx)*dy +
290  img.at<ScalarType>(uy, ux) * dx*dy;
291  }
292  };
293  // using bilinear interpolation with replicated border
294  template<typename ScalarType>
295  struct PixVal < ScalarType, InterpType::Linear, BorderType::Replicate > {
296  static inline double get(
297  const cv::Mat &img, double x, double y,
298  unsigned int h, unsigned int w, double overflow_val = 128.0){
299  assert(img.rows == h && img.cols == w);
300 
301  if(x > w - 1){ x = w - 1; } else if(x < 0){ x = 0; }
302  if(y > h - 1){ y = h - 1; } else if(y < 0){ y = 0; }
303 
304  int lx = static_cast<int>(x);
305  int ly = static_cast<int>(y);
306  double dx = x - lx;
307  double dy = y - ly;
308  int ux = dx == 0 ? lx : lx + 1;
309  int uy = dy == 0 ? ly : ly + 1;
310 
311  return img.at<ScalarType>(ly, lx) * (1 - dx)*(1 - dy) +
312  img.at<ScalarType>(ly, ux) * dx*(1 - dy) +
313  img.at<ScalarType>(uy, lx) * (1 - dx)*dy +
314  img.at<ScalarType>(uy, ux) * dx*dy;
315  }
316  };
318  template<typename ScalarType>
319  void getPixVals(VectorXd &val, const cv::Mat &img, const VectorXd &win_x, const VectorXd &win_y,
320  unsigned int win_h, unsigned int win_w, unsigned int img_h, unsigned int img_w){
321  assert(val.size() == win_h*win_w);
322  assert(win_x.size() == win_w);
323  assert(win_y.size() == win_h);
324  int win_id = 0;
325  for(unsigned int y_id = 0; y_id < win_h; ++y_id){
326  double y = win_y(y_id);
327  for(unsigned int x_id = 0; x_id < win_w; ++x_id){
328  double x = win_x(x_id);
329  val(win_id) = PixVal<ScalarType, PIX_INTERP_TYPE, PIX_BORDER_TYPE>::get(
330  img, x, y, img_h, img_w);
331  ++win_id;
332  }
333  }
334  }
336  template<typename ScalarType>
337  void getPixValsWithGrad(VectorXd &val, PixGradT &grad, const cv::Mat &img,
338  const VectorXd &win_x, const VectorXd &win_y, unsigned int win_h, unsigned int win_w,
339  unsigned int h, unsigned int w, double grad_eps, double grad_mult_factor){
340  //printf("n_pix: %d\t pix_vals.size(): %l\t pts.cols(): %l", n_pix, pix_vals.size(), pts.cols());
341  assert(val.size() == win_h*win_w);
342  assert(win_x.size() == win_w);
343  assert(win_y.size() == win_h);
344  int win_id = 0;
345  for(unsigned int y_id = 0; y_id < win_h; ++y_id){
346  double y = win_y(y_id);
347  for(unsigned int x_id = 0; x_id < win_w; ++x_id){
348  double x = win_x(x_id);
349  val(win_id) = utils::sc::PixVal<ScalarType, PIX_INTERP_TYPE, PIX_BORDER_TYPE>::get(
350  img, x, y, h, w);
351 
352  double pix_val_inc = utils::sc::PixVal<ScalarType, PIX_INTERP_TYPE, PIX_BORDER_TYPE>::get(
353  img, x + grad_eps, y, h, w);
354  double pix_val_dec = utils::sc::PixVal<ScalarType, PIX_INTERP_TYPE, PIX_BORDER_TYPE>::get(
355  img, x - grad_eps, y, h, w);
356  grad(win_id, 0) = (pix_val_inc - pix_val_dec)*grad_mult_factor;
357 
358  pix_val_inc = utils::sc::PixVal<ScalarType, PIX_INTERP_TYPE, PIX_BORDER_TYPE>::get(
359  img, x, y + grad_eps, h, w);
360  pix_val_dec = utils::sc::PixVal<ScalarType, PIX_INTERP_TYPE, PIX_BORDER_TYPE>::get(
361  img, x, y - grad_eps, h, w);
362  grad(win_id, 1) = (pix_val_inc - pix_val_dec)*grad_mult_factor;
363  ++win_id;
364  }
365  }
366  }
367 
368  template<typename ScalarType, typename PtsT>
369  void getPixVals(VectorXd &pix_vals,
370  const cv::Mat &img, const PtsT &pts, unsigned int n_pix, unsigned int h, unsigned int w,
371  double norm_mult = 1, double norm_add = 0);
372  template<typename ScalarType>
373  void getWeightedPixVals(VectorXd &pix_vals, const cv::Mat &img, const PtsT &pts,
374  unsigned int frame_count, double alpha, bool use_running_avg, unsigned int n_pix,
375  unsigned int h, unsigned int w, double norm_mult = 1, double norm_add = 0);
376 
377  /************ functions for image gradient and Hessian ************/
378 
379  template<typename ScalarType>
380  void getWarpedImgGrad(PixGradT &warped_img_grad,
381  const cv::Mat &img, const Matrix8Xd &warped_offset_pts,
382  double grad_eps, unsigned int n_pix,
383  unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
384  template<typename ScalarType>
385  void getImgGrad(PixGradT &img_grad,
386  const cv::Mat &img, const PtsT &pts,
387  double grad_eps, unsigned int n_pix, unsigned int h, unsigned int w,
388  double pix_mult_factor = 1.0);
389 
390  template<typename ScalarType>
391  void getWarpedImgHess(PixHessT &warped_img_hess,
392  const cv::Mat &img, const PtsT &warped_pts,
393  const Matrix16Xd &warped_offset_pts, double hess_eps, unsigned int n_pix,
394  unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
395  template<typename ScalarType>
396  void getImgHess(PixHessT &img_hess, const cv::Mat &img,
397  const PtsT &pts, double hess_eps,
398  unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
399  // mapped versions
400  template<typename ScalarType, InterpType mapping_type>
401  void getWarpedImgGrad(PixGradT &warped_img_grad,
402  const cv::Mat &img, const VectorXd &intensity_map,
403  const Matrix8Xd &warped_offset_pts, double grad_eps,
404  unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
405  template<typename ScalarType, InterpType mapping_type>
406  void getImgGrad(PixGradT &img_grad, const cv::Mat &img,
407  const VectorXd &intensity_map, const PtsT &pts,
408  double grad_eps, unsigned int n_pix, unsigned int h, unsigned int w,
409  double pix_mult_factor = 1.0);
410  template<typename ScalarType, InterpType mapping_type>
411  void getWarpedImgHess(PixHessT &warped_img_hess,
412  const cv::Mat &img, const VectorXd &intensity_map,
413  const PtsT &warped_pts, const HessPtsT &warped_offset_pts,
414  double hess_eps, unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
415  template<typename ScalarType, InterpType mapping_type>
416  void getImgHess(PixHessT &img_hess, const cv::Mat &img,
417  const VectorXd &intensity_map, const PtsT &pts, double hess_eps,
418  unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
419 
421  template<typename ScalarType>
422  void getWarpedImgGrad(PixGradT &warped_img_grad, const cv::Mat &img,
423  bool weighted_mapping, const VectorXd &intensity_map,
424  const Matrix8Xd &warped_offset_pts, double grad_eps,
425  unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
426  template<typename ScalarType>
427  void getImgGrad(PixGradT &img_grad, const cv::Mat &img,
428  bool weighted_mapping, const VectorXd &intensity_map,
429  const PtsT &pts, double grad_eps, unsigned int n_pix, unsigned int h, unsigned int w,
430  double pix_mult_factor = 1.0);
431  template<typename ScalarType>
432  void getWarpedImgHess(PixHessT &warped_img_hess,
433  const cv::Mat &img, bool weighted_mapping, const VectorXd &intensity_map,
434  const PtsT &warped_pts, const HessPtsT &warped_offset_pts,
435  double hess_eps, unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
436  template<typename ScalarType>
437  void getImgHess(PixHessT &img_hess, const cv::Mat &img, bool weighted_mapping,
438  const VectorXd &intensity_map, const PtsT &pts, double hess_eps,
439  unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
440  }
441 
442  namespace mc{
443 
444  // ************************************************************************************** //
445  // ***** functions for multi channel images that work directly on OpenCV Mat images ***** //
446  // ************************************************************************************** //
447 
448  template<typename ScalarType, InterpType interp_type, BorderType border_type>
449  struct PixVal{
450  static inline void get(double *pix_val, const cv::Mat &img, double x, double y,
451  unsigned int h, unsigned int w, double overflow_val = 128.0){
452  throw InvalidArgument(
453  cv::format("get :: Invalid interpolation type specified: %d",
454  interp_type));
455  }
456  };
457  // using nearest neighbor interpolation with constant border
458  template<typename ScalarType>
459  struct PixVal < ScalarType, InterpType::Nearest, BorderType::Constant > {
460  static inline void get(
461  double *pix_val, const cv::Mat &img, double x, double y,
462  unsigned int h, unsigned int w, double overflow_val = 128.0){
463  assert(img.rows == h && img.cols == w && img.type() == CV_32FC3);
464 
465  if(checkOverflow(x, y, h, w)){
466  pix_val[0] = pix_val[1] = pix_val[2] = overflow_val;
467  return;
468  }
469  int nx = static_cast<int>(rint(x));
470  int ny = static_cast<int>(rint(y));
471  if(checkOverflow(nx, ny, h, w)){
472  pix_val[0] = pix_val[1] = pix_val[2] = overflow_val;
473  } else{
474  const ScalarType *pix_data = img.ptr<ScalarType>(ny);
475  pix_val[0] = pix_data[nx];
476  pix_val[1] = pix_data[nx + 1];
477  pix_val[2] = pix_data[nx + 2];
478  }
479  }
480  };
481  // using nearest neighbor interpolation with replicated border
482  template<typename ScalarType>
483  struct PixVal < ScalarType, InterpType::Nearest, BorderType::Replicate > {
484  static inline void get(
485  double *pix_val, const cv::Mat &img, double x, double y,
486  unsigned int h, unsigned int w, double overflow_val = 128.0){
487  assert(img.rows == h && img.cols == w && img.type() == CV_32FC3);
488 
489  if(x > w - 1){ x = w - 1; } else if(x < 0){ x = 0; }
490  if(y > h - 1){ y = h - 1; } else if(y < 0){ y = 0; }
491 
492  int nx = static_cast<int>(rint(x));
493  int ny = static_cast<int>(rint(y));
494 
495  if(checkOverflow(nx, ny, h, w)){
496  pix_val[0] = pix_val[1] = pix_val[2] = overflow_val;
497  } else{
498  const ScalarType *pix_data = img.ptr<ScalarType>(ny);
499  pix_val[0] = pix_data[nx];
500  pix_val[1] = pix_data[nx + 1];
501  pix_val[2] = pix_data[nx + 2];
502  }
503  }
504  };
505  // using bilinear interpolation with constant border
506  template<typename ScalarType>
507  struct PixVal < ScalarType, InterpType::Linear, BorderType::Constant > {
508  static inline void get(
509  double *pix_val, const cv::Mat &img, double x, double y,
510  unsigned int h, unsigned int w, double overflow_val = 128.0){
511  assert(img.rows == h && img.cols == w && img.type() == CV_32FC3);
512 
513  typedef cv::Vec<ScalarType, 3> Vec;
514 
515  if(checkOverflow(x, y, h, w)){
516  pix_val[0] = pix_val[1] = pix_val[2] = overflow_val;
517  return;
518  }
519  int lx = static_cast<int>(x);
520  int ly = static_cast<int>(y);
521  double dx = x - lx;
522  double dy = y - ly;
523  int ux = dx == 0 ? lx : lx + 1;
524  int uy = dy == 0 ? ly : ly + 1;
525 
526  if(checkOverflow(lx, ly, h, w) || checkOverflow(ux, uy, h, w)){
527  pix_val[0] = pix_val[1] = pix_val[2] = overflow_val;
528  } else{
529  double ly_lx = (1 - dx)*(1 - dy), ly_ux = dx*(1 - dy), uy_lx = (1 - dx)*dy, uy_ux = dx*dy;
530  const Vec& pix_ly_lx = img.at<Vec>(ly, lx);
531  const Vec& pix_ly_ux = img.at<Vec>(ly, ux);
532  const Vec& pix_uy_lx = img.at<Vec>(uy, lx);
533  const Vec& pix_uy_ux = img.at<Vec>(uy, ux);
534  pix_val[0] =
535  pix_ly_lx[0] * ly_lx +
536  pix_ly_ux[0] * ly_ux +
537  pix_uy_lx[0] * uy_lx +
538  pix_uy_ux[0] * uy_ux;
539  pix_val[1] =
540  pix_ly_lx[1] * ly_lx +
541  pix_ly_ux[1] * ly_ux +
542  pix_uy_lx[1] * uy_lx +
543  pix_uy_ux[1] * uy_ux;
544  pix_val[2] =
545  pix_ly_lx[2] * ly_lx +
546  pix_ly_ux[2] * ly_ux +
547  pix_uy_lx[2] * uy_lx +
548  pix_uy_ux[2] * uy_ux;
549  }
550  }
551  };
552  // using bilinear interpolation with replicated border
553  template<typename ScalarType>
554  struct PixVal < ScalarType, InterpType::Linear, BorderType::Replicate > {
555  static inline void get(
556  double *pix_val, const cv::Mat &img, double x, double y,
557  unsigned int h, unsigned int w, double overflow_val = 128.0){
558  assert(img.rows == h && img.cols == w && img.type() == CV_32FC3);
559 
560  typedef cv::Vec<ScalarType, 3> Vec;
561 
562  if(x > w - 1){ x = w - 1; } else if(x < 0){ x = 0; }
563  if(y > h - 1){ y = h - 1; } else if(y < 0){ y = 0; }
564 
565  int lx = static_cast<int>(x);
566  int ly = static_cast<int>(y);
567  double dx = x - lx;
568  double dy = y - ly;
569  int ux = dx == 0 ? lx : lx + 1;
570  int uy = dy == 0 ? ly : ly + 1;
571 
572  if(checkOverflow(lx, ly, h, w) || checkOverflow(ux, uy, h, w)){
573  pix_val[0] = pix_val[1] = pix_val[2] = overflow_val;
574  return;
575  } else{
576  const Vec& pix_ly_lx = img.at<Vec>(ly, lx);
577  const Vec& pix_ly_ux = img.at<Vec>(ly, ux);
578  const Vec& pix_uy_lx = img.at<Vec>(uy, lx);
579  const Vec& pix_uy_ux = img.at<Vec>(uy, ux);
580  double ly_lx = (1 - dx)*(1 - dy), ly_ux = dx*(1 - dy), uy_lx = (1 - dx)*dy, uy_ux = dx*dy;
581 
582  pix_val[0] =
583  pix_ly_lx[0] * ly_lx +
584  pix_ly_ux[0] * ly_ux +
585  pix_uy_lx[0] * uy_lx +
586  pix_uy_ux[0] * uy_ux;
587  pix_val[1] =
588  pix_ly_lx[1] * ly_lx +
589  pix_ly_ux[1] * ly_ux +
590  pix_uy_lx[1] * uy_lx +
591  pix_uy_ux[1] * uy_ux;
592  pix_val[2] =
593  pix_ly_lx[2] * ly_lx +
594  pix_ly_ux[2] * ly_ux +
595  pix_uy_lx[2] * uy_lx +
596  pix_uy_ux[2] * uy_ux;
597  }
598  }
599  };
600  template<typename ScalarType, typename PtsT>
601  void getPixVals(VectorXd &pix_vals,
602  const cv::Mat &img, const PtsT &pts, unsigned int n_pix, unsigned int h, unsigned int w,
603  double norm_mult = 1, double norm_add = 0);
604 
607  template<typename ScalarType>
608  void getWeightedPixVals(VectorXd &pix_vals, const cv::Mat &img, const PtsT &pts,
609  unsigned int frame_count, double alpha, bool use_running_avg, unsigned int n_pix,
610  unsigned int h, unsigned int w, double pix_norm_mult, double pix_norm_add);
611 
612  /************ functions for image gradient and Hessian ************/
613 
614  template<typename ScalarType>
615  void getWarpedImgGrad(PixGradT &warped_img_grad,
616  const cv::Mat &img, const Matrix8Xd &warped_offset_pts,
617  double grad_eps, unsigned int n_pix,
618  unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
619  template<typename ScalarType>
620  void getImgGrad(PixGradT &img_grad,
621  const cv::Mat &img, const PtsT &pts,
622  double grad_eps, unsigned int n_pix, unsigned int h, unsigned int w,
623  double pix_mult_factor = 1.0);
624  template<typename ScalarType>
625  void getWarpedImgHess(PixHessT &warped_img_hess,
626  const cv::Mat &img, const PtsT &warped_pts,
627  const Matrix16Xd &warped_offset_pts, double hess_eps, unsigned int n_pix,
628  unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
629  template<typename ScalarType>
630  void getImgHess(PixHessT &img_hess, const cv::Mat &img,
631  const PtsT &pts, double hess_eps,
632  unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
633  // mapped versions
634  template<typename ScalarType, InterpType mapping_type>
635  void getWarpedImgGrad(PixGradT &warped_img_grad,
636  const cv::Mat &img, const VectorXd &intensity_map,
637  const Matrix8Xd &warped_offset_pts, double grad_eps,
638  unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
639  template<typename ScalarType>
640  void getWarpedImgGrad(PixGradT &warped_img_grad, const cv::Mat &img,
641  bool weighted_mapping, const VectorXd &intensity_map,
642  const Matrix8Xd &warped_offset_pts, double grad_eps,
643  unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
644  template<typename ScalarType, InterpType mapping_type>
645  void getImgGrad(PixGradT &img_grad, const cv::Mat &img,
646  const VectorXd &intensity_map, const PtsT &pts,
647  double grad_eps, unsigned int n_pix, unsigned int h, unsigned int w,
648  double pix_mult_factor = 1.0);
649  template<typename ScalarType>
650  void getImgGrad(PixGradT &img_grad, const cv::Mat &img,
651  bool weighted_mapping, const VectorXd &intensity_map,
652  const PtsT &pts, double grad_eps, unsigned int n_pix, unsigned int h, unsigned int w,
653  double pix_mult_factor = 1.0);
654  template<typename ScalarType, InterpType mapping_type>
655  void getWarpedImgHess(PixHessT &warped_img_hess,
656  const cv::Mat &img, const VectorXd &intensity_map,
657  const PtsT &warped_pts, const HessPtsT &warped_offset_pts,
658  double hess_eps, unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
659  template<typename ScalarType>
660  void getWarpedImgHess(PixHessT &warped_img_hess,
661  const cv::Mat &img, bool weighted_mapping, const VectorXd &intensity_map,
662  const PtsT &warped_pts, const HessPtsT &warped_offset_pts,
663  double hess_eps, unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
664  template<typename ScalarType, InterpType mapping_type>
665  void getImgHess(PixHessT &img_hess, const cv::Mat &img,
666  const VectorXd &intensity_map, const PtsT &pts, double hess_eps,
667  unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
668  template<typename ScalarType>
669  void getImgHess(PixHessT &img_hess, const cv::Mat &img, bool weighted_mapping,
670  const VectorXd &intensity_map, const PtsT &pts, double hess_eps,
671  unsigned int n_pix, unsigned int h, unsigned int w, double pix_mult_factor = 1.0);
672  }
673  /*********** functions for mapping pixel values *******************/
674 
676  template<InterpType interp_type>
677  inline double mapPixVal(double x, const VectorXd &intensity_map){
678  printf("mapPixVal:: invalid interpolation type specified: %d\n", interp_type);
679  return 0;
680  }
681  //using nearest neighbor interpolation
682  template<>
683  inline double mapPixVal<InterpType::Nearest>(double x, const VectorXd &intensity_map){
684  return intensity_map(static_cast<int>(rint(x)));
685  }
686  //using linear interpolation
687  template<>
688  inline double mapPixVal<InterpType::Linear>(double x, const VectorXd &intensity_map){
689  int lx = static_cast<int>(x);
690  double dx = x - lx;
691  double mapped_x = dx == 0 ? intensity_map(lx) : (1 - dx)*intensity_map(lx) + dx*intensity_map(lx + 1);;
692  //printf("Weighted mapping: x=%f lx=%d mapped x=%f\n", x, lx, mapped_x);
693  //printMatrix(intensity_map, "intensity_map");
694  return mapped_x;
695  }
696  // maps a vector of pixel values using the given intensity map; templated on the type of mapping to be done
697  template<InterpType interp_type>
698  inline void mapPixVals(VectorXd &dst_pix_vals, const VectorXd &src_pix_vals,
699  const VectorXd &intensity_map, unsigned int n_pix){
700  for(unsigned int i = 0; i < n_pix; ++i){
701  dst_pix_vals(i) = mapPixVal<interp_type>(src_pix_vals(i), intensity_map);
702  }
703  }
704 
705  /*********** functions for bicubic interpolation *******************/
706 
707  // extracts pixel values from a 4x4 grid of integral locations around the given location;
708  // this grid extends in the range [x1-1, x1+2] and [y1-1, y1+2];
709  // uses pixel duplication for points in the grid that lie outside the image extents
710  // these pixels can be used for bicubic interpolation
711  template<typename PixVecT>
712  inline double cubicBSplInterpolate(const PixVecT &pix_vec, double dx) {
713  double dx2 = dx*dx;
714  double dx3 = dx2*dx;
715  double dxi = 1 - dx;
716  double dxi2 = dxi*dxi;
717  double dxi3 = dxi2*dxi;
718  return (pix_vec(0)*dxi3 + pix_vec(1)*(4 - 6 * dx2 + 3 * dx3) + pix_vec(2)*(4 - 6 * dxi2 + 3 * dxi3) + pix_vec(3)*dx3) / 6;
719  }
720  template<typename PixVecT>
721  inline double cubicInterpolate(const PixVecT &pix_vec, double x) {
722  return pix_vec(1) + 0.5 * x*(pix_vec(2) - pix_vec(0) + x*(2.0*pix_vec(0) - 5.0*pix_vec(1) + 4.0*pix_vec(2) - pix_vec(3) + x*(3.0*(pix_vec(1) - pix_vec(2)) + pix_vec(3) - pix_vec(0))));
723  }
724  bool getNeighboringPixGrid(Matrix4d &pix_grid, const EigImgT &img, double x, double y,
725  unsigned int h, unsigned int w);
726  void getBiCubicCoefficients(Matrix4d &bicubic_coeff, const Matrix4d &pix_grid);
727  //evaluates the cubic polynomial surface defines by the given parameters at the given location
728  double biCubic(const Matrix4d &bicubic_coeff, double x, double y);
729  //evaluates the gradient of cubic polynomial surface defines by the given parameters at the given location
730  double biCubicGradX(Vector2d &grad, const Matrix4d &bicubic_coeff, double x, double y);
731  double biCubicGradY(Vector2d &grad, const Matrix4d &bicubic_coeff, double x, double y);
732  //evaluates the hessian of the cubic polynomial surface defines by the given parameters at the given location
733  double biCubicHessXX(const Matrix4d &bicubic_coeff, double x, double y);
734  double biCubicHessYY(const Matrix4d &bicubic_coeff, double x, double y);
735  double biCubicHessYX(const Matrix4d &bicubic_coeff, double x, double y);
736  double biCubicHessXY(const Matrix4d &bicubic_coeff, double x, double y);
737 
739  cv::Mat convertFloatImgToUchar(cv::Mat &img, int nchannels);
740  void generateWarpedImg(cv::Mat &warped_img, const cv::Mat &warped_corners,
741  const mtf::PtsT &warped_pts, const mtf::PixValT orig_patch,
742  const cv::Mat &orig_img, unsigned int img_width, unsigned int img_height,
743  unsigned int n_pts, int background_type, bool show_warped_img = false,
744  const char* win_name = "warped_img");
745  template<typename ScalarType>
746  void generateInverseWarpedImg(cv::Mat &warped_img, const mtf::PtsT &warped_pts,
747  const cv::Mat &orig_img, const mtf::PtsT &orig_pts,
748  int img_width, int img_height, int n_pts, bool show_warped_img = false,
749  const char* win_name = "warped_img");
750  void generateInverseWarpedImg(cv::Mat &warped_img, const PixValT &pix_vals,
751  unsigned int img_width, unsigned int img_height, unsigned int n_pts, bool show_warped_img,
752  const char* win_name = "warped_img");
754  void writePixelsToImage(cv::Mat &img, const PixValT &pix_vals,
755  const mtf::PtsT &pts, unsigned int n_channels, cv::Mat &mask);
756  void writePixelsToImage(cv::Mat &img, const cv::Mat &pix_vals,
757  const mtf::PtsT &pts, int n_channels, cv::Mat &mask);
758  void writePixelsToImage(cv::Mat &img, const cv::Mat &pix_vals,
759  const mtf::CornersT &corners, int n_channels, cv::Mat &mask);
760  void writePixelsToImage(cv::Mat &img, const cv::Mat &pix_vals,
761  const cv::Mat &corners, int n_channels, cv::Mat &mask);
763  bool addGaussianNoise(const cv::Mat mSrc, cv::Mat &mDst,
764  int n_channels, double Mean = 0.0, double StdDev = 10.0);
765  cv::Mat reshapePatch(const VectorXd &curr_patch,
766  int img_height, int img_width, int n_channels = 1);
767  VectorXd reshapePatch(const cv::Mat &cv_patch,
768  int start_x = 0, int start_y = 0,
769  int end_x = -1, int end_y = -1);
770  void anisotropicDiffusion(cv::Mat &output, double lambda = 1.0 / 7.0,
771  double k = 30, unsigned int n_iters = 15);
772 }
773 _MTF_END_NAMESPACE
774 #endif
utils::mapPixVal
double mapPixVal(double x, const VectorXd &intensity_map)
map pixel values using the given intensity map
Definition: imgUtils.h:677
utils::addGaussianNoise
bool addGaussianNoise(const cv::Mat mSrc, cv::Mat &mDst, int n_channels, double Mean=0.0, double StdDev=10.0)
add Gaussian distributed random noise to the image
utils::writePixelsToImage
void writePixelsToImage(cv::Mat &img, const PixValT &pix_vals, const mtf::PtsT &pts, unsigned int n_channels, cv::Mat &mask)
write pixel values in the given image at the given locations
utils::sc::PixVal
Definition: imgUtils.h:227
utils::convertFloatImgToUchar
cv::Mat convertFloatImgToUchar(cv::Mat &img, int nchannels)
Miscellaneous image related utility functions.
utils
basic functions for preprocessing the raw input image using filtering, resizing and histogram equaliz...
Definition: histUtils.h:20
utils::mc::PixVal
Definition: imgUtils.h:449
utils::getWeightedPixVals
void getWeightedPixVals(VectorXd &pix_vals, const EigImgT &img, const PtsT &pts, unsigned int frame_count, double alpha, bool use_running_avg, unsigned int n_pix, unsigned int h, unsigned int w, double norm_mult, double norm_add)
get weighted pixel values using alpha as weighting factor between existing and new pixel values ...
Generated on Sun Jul 29 2018 10:45:28 for MTF by   doxygen 1.8.11