libcvd
esm.h
1 #ifndef CVD_ESM_H_
2 #define CVD_ESM_H_
3 
4 #include <TooN/so3.h>
5 #include <TooN/wls.h>
6 
7 #include <cvd/image.h>
8 #include <cvd/image_io.h>
9 #include <cvd/vector_image_ref.h>
10 #include <cvd/vision.h>
11 
12 #include <algorithm>
13 #include <iomanip>
14 #include <sstream>
15 #include <vector>
16 
17 namespace CVD
18 {
19 
20 static int DEBUG_ESM = 0;
21 
56 
59 struct ESMResult
60 {
61  double error;
62  double delta;
63  int pixels;
64  int iterations;
65 
67  double RMSE() const { return std::sqrt(error / std::max(1, pixels)); }
68 };
69 
70 inline std::ostream& operator<<(std::ostream& out, const ESMResult& r)
71 {
72  out << r.error << " " << r.pixels << " " << r.RMSE() << " " << r.delta << " " << r.iterations;
73  return out;
74 }
75 
76 namespace Internal
77 { // forward declaration of some internal functions
78  // intersects the line AB with the rectangle rect, returns the interval for the parameter t
79  // for X = A + (B-A)*t
80  // A, B in homogeneous coordinates, yielding the correct perspective interpolation for t
81  inline TooN::Vector<2> getBounds(const TooN::Vector<2>& rect, const TooN::Vector<3>& A, const TooN::Vector<3>& B);
82  inline std::vector<TooN::Vector<2, int>> getBounds(const TooN::Vector<2>& rect, const ImageRef& in, const TooN::Matrix<3>& H);
83  inline std::vector<TooN::Vector<2, int>> erode(const std::vector<TooN::Vector<2, int>>& in);
84 
85  // H takes pixels in out to pixels in in as a 2D homography (3x3 matrix)
86  template <typename T>
87  inline std::vector<TooN::Vector<2, int>> transform_perspective(BasicImage<T>& out, const BasicImage<T>& in, const TooN::Matrix<3>& H);
88  // H takes pixels in out to pixels in in as a 2D affine transformation (3x3 matrix)
89  template <typename T>
90  inline std::vector<TooN::Vector<2, int>> transform_affine(BasicImage<T>& out, const BasicImage<T>& in, const TooN::Matrix<3>& H);
91  // H takes pixels in out to pixels in in as a 2D translation only (stored in the 3x3 matrix)
92  // This is extra optimized to use the constant mixing factors in the bi-linear interpolation
93  template <typename T>
94  inline std::vector<TooN::Vector<2, int>> transform_translation(BasicImage<T>& out, const BasicImage<T>& in, const TooN::Matrix<3>& H);
95  // automatically selects the right transform implementation based on the properties of H
96  template <typename T>
97  inline std::vector<TooN::Vector<2, int>> transform(BasicImage<T>& out, const BasicImage<T>& in, const TooN::Matrix<3>& H);
98 
99  // calculates the gradient of a one component image within the given bounds only. The
100  // function uses central differences, but does not divide by 2!
101  template <typename GRADIENT, typename IMAGE>
102  inline Image<GRADIENT> gradient(const BasicImage<IMAGE>& img, const std::vector<TooN::Vector<2, int>>& bounds);
103  // calculates the gradient of a one component image directly. The
104  // function uses central differences, but does not divide by 2!
105  template <typename GRADIENT, typename IMAGE>
106  inline Image<GRADIENT> gradient(const BasicImage<IMAGE>& img);
107 
120  template <typename TRANSFORM, typename APPEARANCE, typename IMAGE, typename GRADIENT>
121  inline ESMResult esm_opt(TRANSFORM& T, APPEARANCE& A, const BasicImage<IMAGE>& templateImage, const BasicImage<GRADIENT>& templateGradient, const BasicImage<IMAGE>& target, const int max_iterations = 40, const double min_delta = 1e-8, const double max_RMSE = 1.0);
122 } // namespace Internal
123 
136 template <int PARAMS>
137 class Homography
138 {
139  public:
140  static const int dimensions = PARAMS;
141 
142  Homography()
143  : H(TooN::Identity)
144  {
145  }
146 
147  template <int R, int C, typename P, typename B>
148  Homography(const TooN::Matrix<R, C, P, B>& h)
149  : H(h)
150  {
151  }
152 
153  const TooN::Matrix<3>& get_matrix() const { return H; }
154  TooN::Matrix<3>& get_matrix() { return H; }
155  const TooN::Vector<PARAMS>& get_jacobian(const TooN::Vector<2>& p, const TooN::Vector<2>& grad) const
156  {
157  switch(PARAMS)
158  {
159  case 8:
160  // 0 0 p[1]
161  J[7] = -p[1] * (grad * p); // TooN::makeVector(-p[1]*p[0], -p[1]*p[1]);
162  case 7:
163  // 0 0 p[0]
164  J[6] = -p[0] * (grad * p); // TooN::makeVector(-p[0]*p[0], -p[0]*p[1]);
165  case 6:
166  // p[1] p[0] 0
167  J[5] = grad[0] * p[1] + grad[1] * p[0]; // TooN::makeVector(p[1], p[0]);
168  case 5:
169  // p[0] -p[1] 0
170  J[4] = grad[0] * p[0] - grad[1] * p[1]; // TooN::makeVector(p[0], -p[1]);
171  case 4:
172  // p[0] p[1] -2
173  J[3] = 3 * (grad * p); //TooN::makeVector(3*p[0], 3*p[1]);
174  case 3:
175  // -p[1] p[0] 0
176  J[2] = -grad[0] * p[1] + grad[1] * p[0]; // TooN::makeVector(-p[1], p[0]);
177  case 2:
178  J[1] = grad[1]; // TooN::makeVector(0, 1);
179  case 1:
180  J[0] = grad[0]; // TooN::makeVector(1, 0);
181  break;
182  default:
183  assert(false);
184  }
185  return J;
186  }
187 
188  void update(const TooN::Vector<PARAMS>& v)
189  {
190  TooN::Matrix<3> G = TooN::Zeros;
191  switch(PARAMS)
192  {
193  case 8:
194  G(2, 1) = v[7];
195  case 7:
196  G(2, 0) = v[6];
197  case 6:
198  G(0, 1) = v[5];
199  G(1, 0) = v[5];
200  case 5:
201  G(0, 0) = v[4];
202  G(1, 1) = -v[4];
203  case 4:
204  G(0, 0) += v[3];
205  G(1, 1) += v[3];
206  G(2, 2) += -2 * v[3];
207  case 3:
208  G(0, 1) -= v[2];
209  G(1, 0) += v[2];
210  case 2:
211  G(1, 2) = v[1];
212  case 1:
213  G(0, 2) = v[0];
214  break;
215  default:
216  assert(false);
217  }
218  H = H * TooN::exp(-G);
219  }
220 
221  protected:
222  TooN::Matrix<3> H;
223  mutable TooN::Vector<PARAMS> J;
224 };
225 
232 template <int PARAMS>
233 class HomographyPrefix : public Homography<PARAMS>
234 {
235  public:
236  static const int dimensions = PARAMS;
237 
238  HomographyPrefix()
239  : Pre(TooN::Identity)
240  {
241  }
242 
243  template <int R, int C, typename P, typename B>
244  HomographyPrefix(const TooN::Matrix<R, C, P, B>& p)
245  {
246  set_prefix(p);
247  }
248 
249  template <int R, int C, typename P, typename B, int R2, int C2, typename P2, typename B2>
250  HomographyPrefix(const TooN::Matrix<R, C, P, B>& h, const TooN::Matrix<R2, C2, P2, B2>& p)
251  {
252  set_prefix(p);
253  H = h * H;
254  }
255 
256  const TooN::Matrix<3> get_matrix() const { return H * Pre; }
257 
258  template <int R, int C, typename P, typename B>
259  void set_prefix(const TooN::Matrix<R, C, P, B>& p)
260  {
261  Pre = p;
262  const TooN::Matrix<3> id = TooN::Identity;
263  H = TooN::gaussian_elimination(Pre, id);
264  PreGrad = H.T().template slice<0, 0, 2, 2>(); // this is an approximation, if Pre is a projective warp !
265  }
266 
267  const TooN::Vector<PARAMS>& get_jacobian(const TooN::Vector<2>& x, const TooN::Vector<2>& grad) const
268  {
269  if(PARAMS > 2)
270  {
271  const TooN::Vector<2> p = TooN::project(Pre * TooN::unproject(x));
272  return Homography<PARAMS>::get_jacobian(p, PreGrad * grad);
273  }
274  return Homography<PARAMS>::get_jacobian(x, grad);
275  }
276 
277  protected:
278  using Homography<PARAMS>::H;
279  TooN::Matrix<3> Pre;
280  TooN::Matrix<2> PreGrad;
281 };
282 
287 class CameraRotation
288 {
289  public:
290  static const int dimensions = 3;
291 
292  CameraRotation() { }
293 
294  template <typename P, typename B>
295  CameraRotation(const TooN::Vector<4, P, B>& camera_params, const TooN::SO3<>& init = TooN::SO3<>())
296  : R(init)
297  {
298  set_camera(camera_params);
299  }
300 
301  template <typename P, typename B>
302  void set_camera(const TooN::Vector<4, P, B>& camera_params)
303  {
304  k = camera_params;
305  K = TooN::Identity;
306  K(0, 0) = camera_params[0];
307  K(1, 1) = camera_params[1];
308  K(0, 2) = camera_params[2];
309  K(1, 2) = camera_params[3];
310 
311  kinv = TooN::makeVector(1 / camera_params[0], 1 / camera_params[1], -camera_params[2] / camera_params[0], -camera_params[3] / camera_params[1]);
312  Kinv = TooN::Identity;
313  Kinv(0, 0) = kinv[0];
314  Kinv(1, 1) = kinv[1];
315  Kinv(0, 2) = kinv[2];
316  Kinv(1, 2) = kinv[3];
317  }
318 
319  const TooN::Matrix<3> get_matrix() const { return K * R * Kinv; }
320 
321  const TooN::Vector<dimensions>& get_jacobian(const TooN::Vector<2>& p, const TooN::Vector<2>& grad) const
322  {
323  // this implements grad * proj * K * G_i * Kinv * p;
324  const TooN::Vector<3> pW = TooN::makeVector(kinv[0] * p[0] + kinv[2], kinv[1] * p[1] + kinv[3], 1);
325  const TooN::Vector<3> J_pK = TooN::makeVector(grad[0] * k[0], grad[1] * k[1], grad * (k.slice<2, 2>() - p));
326  J[0] = J_pK * TooN::SO3<>::generator_field(0, pW);
327  J[1] = J_pK * TooN::SO3<>::generator_field(1, pW);
328  J[2] = J_pK * TooN::SO3<>::generator_field(2, pW);
329  return J;
330  }
331 
332  void update(const TooN::Vector<dimensions>& v)
333  {
334  R = R * TooN::SO3<>::exp(-v);
335  }
336 
337  const TooN::SO3<>& get_rotation() const { return R; }
338  TooN::SO3<>& get_rotation() { return R; }
339 
340  protected:
341  TooN::Matrix<3> K, Kinv;
342  TooN::Vector<4> k, kinv;
343  TooN::SO3<> R;
344  mutable TooN::Vector<dimensions> J;
345 };
346 
349 class StaticAppearance
350 {
351  public:
352  static const int dimensions = 0;
353  template <typename PIXEL>
354  double difference(const PIXEL& warped, const PIXEL& templatePixel) const
355  {
356  return warped - templatePixel;
357  }
358  template <typename PIXEL>
359  std::pair<double, TooN::Vector<dimensions>> difference_jacobian(const PIXEL& warped, const PIXEL& templatePixel) const
360  {
361  return std::make_pair(difference(warped, templatePixel), TooN::Vector<dimensions>());
362  }
363  TooN::Vector<2> image_jacobian(const TooN::Vector<2>& gradWarped, const TooN::Vector<2>& gradTemplate) const
364  {
365  return (gradWarped + gradTemplate) * 0.25;
366  }
367  void update(const TooN::Vector<dimensions>& d) { }
368 };
369 
373 class OffsetAppearance
374 {
375  public:
376  static const int dimensions = 1;
377  template <typename PIXEL>
378  double difference(const PIXEL& warped, const PIXEL& templatePixel) const
379  {
380  return warped - templatePixel - offset;
381  }
382  template <typename PIXEL>
383  std::pair<double, TooN::Vector<dimensions>> difference_jacobian(const PIXEL& warped, const PIXEL& templatePixel) const
384  {
385  return std::make_pair(difference(warped, templatePixel), TooN::makeVector(1));
386  }
387  TooN::Vector<2> image_jacobian(const TooN::Vector<2>& gradWarped, const TooN::Vector<2>& gradTemplate) const
388  {
389  return (gradWarped + gradTemplate) * 0.25;
390  }
391  void update(const TooN::Vector<dimensions>& d)
392  {
393  offset += d[0];
394  }
395 
396  OffsetAppearance()
397  : offset(0)
398  {
399  }
400  double offset;
401 };
402 
407 class BlurAppearance
408 {
409  public:
410  static const int dimensions = 0;
411  template <typename PIXEL>
412  double difference(const PIXEL& warped, const PIXEL& templatePixel) const
413  {
414  return warped - templatePixel;
415  }
416  template <typename PIXEL>
417  std::pair<double, TooN::Vector<dimensions>> difference_jacobian(const PIXEL& warped, const PIXEL& templatePixel) const
418  {
419  return std::make_pair(difference(warped, templatePixel), TooN::Vector<dimensions>());
420  }
421  TooN::Vector<2> image_jacobian(const TooN::Vector<2>& gradWarped, const TooN::Vector<2>& gradTemplate) const
422  {
423  return (gradWarped + 0.5 * gradTemplate) * 0.25;
424  }
425  void update(const TooN::Vector<dimensions>& d)
426  {
427  }
428 };
429 
430 #if 0
431 
432  class LinearAppearance {
433  static const int dimensions = 2;
434 
435  template <typename PIXEL> double difference( const PIXEL & templatePixel, const PIXEL & warped ) const { return warped - templatePixel*scale - offset; }
436  Vector<2> image_jacobian( const Vector<2> & gradTemplate, const Vector<2> & gradImage ) const { return gradTemplate + gradImage; }
437  Vector<dimensions> get_jacobian( const Vector<2> & point ) { return TooN::makeVector(1.0); }
438  void update( const Vector<dimensions> & d ) { offset += d[0]; }
439 
440  double offset;
441  double scale;
442  };
443 #endif
444 
445 template <typename P>
446 inline TooN::Matrix<3, 3, P> scaleHomography(const TooN::Matrix<3, 3, P>& H, const P& f)
447 {
448  const TooN::Vector<3, P> s = TooN::makeVector<P>(1 / f, 1 / f, 1);
449  const TooN::Vector<3, P> is = TooN::makeVector<P>(f, f, 1);
450  return is.as_diagonal() * H * s.as_diagonal();
451 }
452 
453 #if 0
454 
457  template<int PARAMS, typename APPEARANCE, typename IMAGE>
458  inline TooN::Matrix<3> inter_frame_homography( const BasicImage<IMAGE> & from, const BasicImage<IMAGE> & to, const TooN::Matrix<3> & init = TooN::Identity, ESMResult * const res = NULL){
459  TooN::Matrix<3> prefix = TooN::Identity;
460 #if 1
461  prefix(0,2) = -from.size().x/2;
462  prefix(1,2) = -from.size().y/2;
463 #else
464  prefix(0,0) = 2.0/from.size().x;
465  prefix(1,1) = 2.0/from.size().y;
466  prefix(0,2) = -1;
467  prefix(1,2) = -1;
468  cout << prefix << endl;
469 #endif
470 
471  HomographyPrefix<PARAMS> H(init, prefix);
472  APPEARANCE appearance;
473  ESMTransform<HomographyPrefix<PARAMS>, APPEARANCE> T(H, appearance);
474 
475  ESMResult r = esm_opt(T, from, to, 40, 1e-2);
476 
477  if(res != NULL)
478  *res = r;
479  return T.transform.get_matrix();
480  }
481 
482 #endif
483 
487 template <typename TRANSFORM, typename APPEARANCE, typename IMAGE = CVD::byte, typename GRADIENT = TooN::Vector<2, typename Pixel::traits<IMAGE>::wider_type>>
488 class ESMEstimator
489 {
490  public:
491  TRANSFORM transform;
492  APPEARANCE appearance;
493 
494  Image<IMAGE> templ;
495  Image<GRADIENT> templGradient;
496 
497  int max_iterations;
498  double min_delta;
499  double max_RMSE;
500 
501  ESMResult result;
502 
503  ESMEstimator()
504  : max_iterations(40)
505  , min_delta(1e-5)
506  , max_RMSE(1e-2)
507  {
508  }
509  ESMEstimator(const Image<IMAGE>& t, const Image<GRADIENT>& g)
510  : templ(t)
511  , templGradient(g)
512  , max_iterations(40)
513  , min_delta(1e-5)
514  , max_RMSE(1e-2)
515  {
516  }
517  ESMEstimator(const Image<IMAGE>& t)
518  : templ(t)
519  , max_iterations(40)
520  , min_delta(1e-5)
521  , max_RMSE(1e-2)
522  {
523  templGradient = Internal::gradient<IMAGE, GRADIENT>(templ);
524  }
525  ESMEstimator(const BasicImage<IMAGE>& t)
526  : max_iterations(40)
527  , min_delta(1e-5)
528  , max_RMSE(1e-2)
529  {
530  templ.copy_from(t);
531  templGradient = Internal::gradient<IMAGE, GRADIENT>(templ);
532  }
533 
534  void set_image(const Image<IMAGE>& t, const Image<GRADIENT>& g)
535  {
536  assert(t.size() == g.size());
537  templ = t;
538  templGradient = g;
539  }
540 
541  void set_image(const Image<IMAGE>& t)
542  {
543  templ = t;
544  templGradient = Internal::gradient<GRADIENT>(templ);
545  }
546 
547  void set_image(const BasicImage<IMAGE>& t)
548  {
549  Image<IMAGE> temp;
550  temp.copy_from(t);
551  set_image(temp);
552  }
553 
554  void reset()
555  {
556  transform = TRANSFORM();
557  appearance = APPEARANCE();
558  }
559 
560  const ESMResult& optimize(const BasicImage<IMAGE>& to)
561  {
562  return result = Internal::esm_opt(transform, appearance, templ, templGradient, to, max_iterations, min_delta, max_RMSE);
563  }
564 
565  const ESMResult& optimize(const BasicImage<IMAGE>& from, const BasicImage<IMAGE>& to)
566  {
567  Image<GRADIENT> fromGradient = Internal::gradient<GRADIENT>(from);
568  return optimize(from, fromGradient, to);
569  }
570 
571  const ESMResult& optimize(const BasicImage<IMAGE>& from, const BasicImage<GRADIENT>& fromGradient, const BasicImage<IMAGE>& to)
572  {
573  return result = Internal::esm_opt(transform, appearance, from, fromGradient, to, max_iterations, min_delta, max_RMSE);
574  }
575 
576  const ESMResult& get_result() const { return result; }
577 };
578 
581 template <typename APPEARANCE, typename IMAGE = CVD::byte, typename GRADIENT = TooN::Vector<2, typename Pixel::traits<IMAGE>::wider_type>>
582 class RotationEstimator : public ESMEstimator<CameraRotation, APPEARANCE, IMAGE, GRADIENT>
583 {
584  public:
585  using ESMEstimator<CameraRotation, APPEARANCE, IMAGE, GRADIENT>::transform;
586  using ESMEstimator<CameraRotation, APPEARANCE, IMAGE, GRADIENT>::appearance;
587 
588  RotationEstimator(const TooN::Vector<4>& cam_params, const TooN::SO3<>& init = TooN::SO3<>())
589  {
590  transform.set_camera(cam_params);
591  transform.get_rotation() = init;
592  }
593 
594  void reset()
595  {
596  transform.get_rotation() = TooN::SO3<>();
597  appearance = APPEARANCE();
598  }
599 };
600 
601 namespace Internal
602 {
603 
604  // intersects the line AB with the rectangle rect, returns the interval for the parameter t
605  // for X = A + (B-A)*t
606  // A, B in homogeneous coordinates, yielding the correct perspective interpolation for t
607  inline TooN::Vector<2> getBounds(const TooN::Vector<2>& rect, const TooN::Vector<3>& A, const TooN::Vector<3>& B)
608  {
609  const TooN::Vector<3> dir = A - B;
610  TooN::Vector<2> interval = TooN::makeVector(0, 1);
611 
612  TooN::Matrix<4, 3> lines;
613  lines[0] = TooN::makeVector(1, 0, 0);
614  lines[1] = TooN::makeVector(-1, 0, rect[0]);
615  lines[2] = TooN::makeVector(0, 1, 0);
616  lines[3] = TooN::makeVector(0, -1, rect[1]);
617 
618  const TooN::Vector<4> hitA = lines * A;
619  const TooN::Vector<4> hitB = lines * B;
620 
621  for(int i = 0; i < 4; ++i)
622  {
623  if(hitA[i] <= 0 && hitB[i] <= 0)
624  return TooN::makeVector(0, 0);
625  if(hitA[i] * hitB[i] < 0)
626  {
627  const double t = hitA[i] / (lines[i] * dir);
628  if(hitA[i] < 0)
629  {
630  interval[0] = std::max(t, interval[0]);
631  }
632  else
633  {
634  interval[1] = std::min(t, interval[1]);
635  }
636  }
637  }
638 
639  if(interval[0] > interval[1])
640  return TooN::makeVector(0, 0);
641  return interval;
642  }
643 
644  inline std::vector<TooN::Vector<2, int>> getBounds(const TooN::Vector<2>& rect, const ImageRef& in, const TooN::Matrix<3>& H)
645  {
646  std::vector<TooN::Vector<2, int>> bounds(in.y);
647  for(int y = 0; y < in.y; ++y)
648  {
649  // map the current scan line
650  const TooN::Vector<3> A = H * TooN::makeVector(0, y, 1);
651  const TooN::Vector<3> B = H * TooN::makeVector(in.x - 1, y, 1);
652  // determine valid sample interval
653  const TooN::Vector<2> b = getBounds(rect, A, B) * (in.x - 1);
654  bounds[y] = TooN::makeVector<int>(ceil(b[0]), floor(b[1]) + 1);
655  }
656  return bounds;
657  }
658 
659  inline std::vector<TooN::Vector<2, int>> erode(const std::vector<TooN::Vector<2, int>>& in)
660  {
661  using std::max;
662  using std::min;
663  std::vector<TooN::Vector<2, int>> out(in.size());
664  out.front() = out.back() = TooN::makeVector(0, 0);
665  for(unsigned i = 1; i < in.size() - 1; ++i)
666  {
667  out[i][0] = max(in[i - 1][0], max(in[i][0] + 1, in[i + 1][0])); // the right most of all three
668  out[i][1] = min(in[i - 1][1], min(in[i][1] - 1, in[i + 1][1])); // the left most of all three
669  if(out[i][0] > out[i][1]) // if its empty, then mark as (0,0)
670  out[i][0] = out[i][1] = 0;
671  }
672  return out;
673  }
674 
675  // H takes pixels in out to pixels in in as a 2D homography (3x3 matrix)
676  template <typename T>
677  inline std::vector<TooN::Vector<2, int>> transform_perspective(BasicImage<T>& out, const BasicImage<T>& in, const TooN::Matrix<3>& H)
678  {
679  const ImageRef& size = out.size();
680  const TooN::Vector<2> insize = vec(in.size() - ImageRef(1, 1));
681  const TooN::Vector<3> across = H.T()[0];
682  const TooN::Vector<3> down = H.T()[1];
683  TooN::Vector<3> base = H.T()[2];
684 
685  const std::vector<TooN::Vector<2, int>> bounds = getBounds(insize, size, H);
686 
687  for(int y = 0; y < size.y; ++y)
688  {
689  TooN::Vector<3> X = base + bounds[y][0] * across;
690  T* data = out[y] + bounds[y][0];
691  for(int x = bounds[y][0]; x < bounds[y][1]; ++x, X += across, ++data)
692  {
693  const TooN::DefaultPrecision inv = 1 / X[2];
694  sample(in, X[0] * inv, X[1] * inv, *data);
695  }
696  base += down; // next line
697  }
698 
699  return bounds;
700  }
701 
702  // H takes pixels in out to pixels in in as a 2D affine transformation (3x3 matrix)
703  template <typename T>
704  inline std::vector<TooN::Vector<2, int>> transform_affine(BasicImage<T>& out, const BasicImage<T>& in, const TooN::Matrix<3>& H)
705  {
706  const ImageRef& size = out.size();
707  const TooN::Vector<2> insize = vec(in.size() - ImageRef(1, 1));
708  const TooN::Vector<2> across = H.T()[0].slice<0, 2>();
709  const TooN::Vector<2> down = H.T()[1].slice<0, 2>();
710  TooN::Vector<2> base = H.T()[2].slice<0, 2>();
711 
712  const std::vector<TooN::Vector<2, int>> bounds = getBounds(insize, size, H);
713 
714  for(int y = 0; y < size.y; ++y)
715  {
716  TooN::Vector<2> X = base + bounds[y][0] * across;
717  T* data = out[y] + bounds[y][0];
718  for(int x = bounds[y][0]; x < bounds[y][1]; ++x, X += across, ++data)
719  {
720  sample(in, X[0], X[1], *data);
721  }
722  base += down; // next line
723  }
724 
725  return bounds;
726  }
727 
728  // H takes pixels in out to pixels in in as a 2D translation only (stored in the 3x3 matrix)
729  // This is extra optimized to use the constant mixing factors in the bi-linear interpolation
730  template <typename T>
731  inline std::vector<TooN::Vector<2, int>> transform_translation(BasicImage<T>& out, const BasicImage<T>& in, const TooN::Matrix<3>& H)
732  {
733  const ImageRef& size = out.size();
734  const TooN::Vector<2> insize = vec(in.size() - ImageRef(1, 1));
735  const TooN::Vector<2> across = H.T()[0].slice<0, 2>();
736  const TooN::Vector<2> down = H.T()[1].slice<0, 2>();
737  TooN::Vector<2> base = H.T()[2].slice<0, 2>();
738 
739  std::vector<TooN::Vector<2, int>> bounds = getBounds(insize, size, H);
740 
741  const double fx = H(0, 2) - floor(H(0, 2));
742  const double fy = H(1, 2) - floor(H(1, 2));
743  const double ul = fx * fy;
744  const double ur = (1 - fx) * fy;
745  const double ll = fx * (1 - fy);
746  const double lr = (1 - fx) * (1 - fy);
747 
748  for(int y = 0; y < size.y; ++y)
749  {
750  const TooN::Vector<2>& interval = bounds[y]; // determine valid sample interval
751  int x = interval[0];
752  T* data = out[y] + x;
753  TooN::Vector<2> X = base + x * across; // then sample
754  for(; x < interval[1]; ++x, ++data, X += across)
755  {
756  sample(in, X[0], X[1], *data);
757  }
758  base += down; // next line
759  }
760 
761  return bounds;
762  }
763 
764  template <typename T>
765  inline std::vector<TooN::Vector<2, int>> transform(BasicImage<T>& out, const BasicImage<T>& in, const TooN::Matrix<3>& H)
766  {
767  const double perspective = H(2, 0) * H(2, 0) + H(2, 1) * H(2, 1);
768  if(perspective < 1e-10)
769  return transform_affine(out, in, H);
770  return transform_perspective(out, in, H);
771  }
772 
773  template <typename GRADIENT, typename IMAGE>
774  inline Image<GRADIENT> gradient(const BasicImage<IMAGE>& img, const std::vector<TooN::Vector<2, int>>& bounds)
775  {
776  assert(bounds.size() == unsigned(img.size().y));
777  Image<GRADIENT> grad(img.size());
778  for(int y = 1; y < img.size().y - 1; ++y)
779  {
780  for(int x = bounds[y][0]; x < bounds[y][1]; ++x)
781  {
782  grad[y][x][0] = img[y][x + 1] - img[y][x - 1];
783  grad[y][x][1] = img[y + 1][x] - img[y - 1][x];
784  }
785  }
786  return grad;
787  }
788 
789  template <typename GRADIENT, typename IMAGE>
790  inline Image<GRADIENT> gradient(const BasicImage<IMAGE>& img)
791  {
792  Image<GRADIENT> grad(img.size());
793  for(int y = 1; y < img.size().y - 1; ++y)
794  {
795  for(int x = 1; x < img.size().x - 1; ++x)
796  {
797  grad[y][x][0] = img[y][x + 1] - img[y][x - 1];
798  grad[y][x][1] = img[y + 1][x] - img[y - 1][x];
799  }
800  }
801  return grad;
802  }
803 
804  template <typename TRANSFORM, typename APPEARANCE, typename IMAGE, typename GRADIENT>
805  inline ESMResult esm_opt(TRANSFORM& T, APPEARANCE& A, const BasicImage<IMAGE>& templateImage, const BasicImage<GRADIENT>& templateGradient, const BasicImage<IMAGE>& target, const int max_iterations, const double min_delta, const double max_RMSE)
806  {
807  assert(templateImage.size() == templateGradient.size());
808 
809  const int dimensions = TRANSFORM::dimensions + APPEARANCE::dimensions;
810 
811  Image<IMAGE> warped(templateImage.size());
812 
813  TooN::WLS<dimensions> wls;
814 
815  ESMResult result = { 1e100, 1e100, 0, 0 };
816 
817  // first warp here to be able to compare error before and after
818  std::vector<TooN::Vector<2, int>> mask = Internal::transform(warped, target, T.get_matrix());
819  mask = Internal::erode(mask);
820 
821  do
822  {
823  // get the gradient for the warped image
824  Image<GRADIENT> grad_warped = Internal::gradient<GRADIENT>(warped, mask);
825 
826  // create the least squares system
827  wls.clear();
828  wls.add_prior(1e-10);
829  TooN::Vector<dimensions> J;
830  for(unsigned y = 0; y < mask.size(); ++y)
831  {
832  for(int x = mask[y][0]; x < mask[y][1]; ++x)
833  {
834  std::pair<double, TooN::Vector<APPEARANCE::dimensions>> da = A.difference_jacobian(warped[y][x], templateImage[y][x]);
835  J.template slice<0, TRANSFORM::dimensions>() = T.get_jacobian(TooN::makeVector(x, y), A.image_jacobian(grad_warped[y][x], templateGradient[y][x]));
836  J.template slice<TRANSFORM::dimensions, APPEARANCE::dimensions>() = da.second;
837  wls.add_mJ(da.first, J);
838  }
839  }
840 
841  // solve and update
842  wls.compute();
843  T.update(wls.get_mu().template slice<0, TRANSFORM::dimensions>());
844  A.update(wls.get_mu().template slice<TRANSFORM::dimensions, APPEARANCE::dimensions>());
845  ++result.iterations;
846 
847  // compute new warp to calculate new error
848  mask = Internal::transform(warped, target, T.get_matrix());
849  mask = Internal::erode(mask);
850 
851  // compute error after update
852  result.error = 0;
853  // and number of pixels for RMS computation
854  result.pixels = 0;
855  for(unsigned y = 0; y < mask.size(); ++y)
856  {
857  result.pixels += mask[y][1] - mask[y][0];
858  for(int x = mask[y][0]; x < mask[y][1]; ++x)
859  {
860  const double d = A.difference(warped[y][x], templateImage[y][x]);
861  result.error += d * d;
862  }
863  }
864 
865  // store results
866  result.delta = norm(wls.get_mu());
867 
868 #if 0
869  if(DEBUG_ESM){
870  std::ostringstream sout;
871  sout << "debug_" << std::setw(4) << std::setfill('0') << DEBUG_ESM << "_" << std::setw(4) << std::setfill('0') << result.iterations << ".jpg";
872  Image<byte> warped(templateImage.size());
873  warped.fill(0);
874  Internal::transform(warped, target, T.get_matrix());
875  img_save(warped, sout.str());
876  std::cout << wls.get_mu() << "\t" << T << std::endl;
877  std::cout << wls.get_C_inv() << std::endl;
878  std::cout << wls.get_vector() << std::endl;
879  std::cout << result << std::endl;
880  }
881 #endif
882 
883  } while(result.iterations < max_iterations && result.delta > min_delta && result.RMSE() > max_RMSE);
884 
885  return result;
886  }
887 
888 } // namespace Internal
889 
890 template <int PARAMS>
891 inline std::ostream& operator<<(std::ostream& out, const Homography<PARAMS>& t)
892 {
893  out << t.get_matrix()[0] << " " << t.get_matrix()[1] << " " << t.get_matrix()[2];
894  return out;
895 }
896 
897 template <int PARAMS>
898 inline std::ostream& operator<<(std::ostream& out, const HomographyPrefix<PARAMS>& t)
899 {
900  out << t.get_matrix()[0] << " " << t.get_matrix()[1] << " " << t.get_matrix()[2];
901  return out;
902 }
903 
904 inline std::ostream& operator<<(std::ostream& out, const StaticAppearance& t)
905 {
906  return out;
907 }
908 
909 inline std::ostream& operator<<(std::ostream& out, const OffsetAppearance& t)
910 {
911  out << t.offset;
912  return out;
913 }
914 
915 inline std::ostream& operator<<(std::ostream& out, const BlurAppearance& t)
916 {
917  return out;
918 }
919 
920 } // namespace CVD
921 
922 #endif // CVD_ESM_H
CVD::BlurAppearance
Blur appearance model that assumes that the input image was subject to blur.
Definition: esm.h:407
CVD::gradient
void gradient(const BasicImage< S > &im, BasicImage< T > &out)
computes the gradient image from an image.
Definition: vision.h:348
CVD::StaticAppearance
Basic appearance model implementing no change in the image.
Definition: esm.h:349
CVD::ESMResult::pixels
int pixels
common pixels in last iteration
Definition: esm.h:63
CVD::RotationEstimator
a specialization of ESMEstimator for pure camera rotation only.
Definition: esm.h:582
CVD
All classes and functions are within the CVD namespace.
Definition: argb.h:6
CVD::BasicImage::size
ImageRef size() const
What is the size of this image?
Definition: image.h:557
CVD::Homography
a generic implementation for 2D homography-based transformations parameterized by a 3x3 matrix with d...
Definition: esm.h:137
CVD::ESMResult::error
double error
final total squared error
Definition: esm.h:61
CVD::CameraRotation::J
TooN::Vector< dimensions > J
internal temporary to avoid multiple re-initialisations
Definition: esm.h:344
CVD::vec
TooN::Vector< 2 > vec(const ImageRef &ir)
Convert an image co-ordinate into a Vector.
Definition: vector_image_ref.h:13
CVD::CameraRotation
a special implementation for 2D homography-based transformations described as a camera rotating aroun...
Definition: esm.h:287
CVD::ESMEstimator
The main class for the ESM module.
Definition: esm.h:488
CVD::img_save
void img_save(const BasicImage< PixelType > &im, std::ostream &o, ImageType::ImageType t, const std::map< std::string, Parameter<>> &p=std::map< std::string, Parameter<>>())
Save an image to a stream.
Definition: image_io.h:280
CVD::CameraRotation::R
TooN::SO3 R
the transformation, a rotation of the camera around its centre
Definition: esm.h:343
CVD::ESMResult
Result class storing some information about the optimization.
Definition: esm.h:59
CVD::CameraRotation::Kinv
TooN::Matrix< 3 > Kinv
linear camera matrix and its inverse
Definition: esm.h:341
CVD::Internal::esm_opt
ESMResult esm_opt(TRANSFORM &T, APPEARANCE &A, const BasicImage< IMAGE > &templateImage, const BasicImage< GRADIENT > &templateGradient, const BasicImage< IMAGE > &target, const int max_iterations=40, const double min_delta=1e-8, const double max_RMSE=1.0)
a full ESM optimization function.
Definition: esm.h:805
CVD::OffsetAppearance
Simple appearance model that assumes a constant offset in the intensities of the image vs...
Definition: esm.h:373
CVD::ImageRef::x
int x
The x co-ordinate.
Definition: image_ref.h:172
CVD::ESMResult::RMSE
double RMSE() const
returns RMSE error computed from error and
Definition: esm.h:67
CVD::ImageRef
Definition: image_ref.h:29
CVD::BasicImage::fill
void fill(const T d)
Set all the pixels in the image to a value.
Definition: image.h:574
CVD::BasicImage
A generic image class to manage a block of arbitrarily padded data as an image.
Definition: image.h:273
CVD::ESMResult::iterations
int iterations
number of iterations performed
Definition: esm.h:64
CVD::ImageRef::y
int y
The y co-ordinate.
Definition: image_ref.h:173
CVD::CameraRotation::kinv
TooN::Vector< 4 > kinv
same parameters in vector form for convenience
Definition: esm.h:342
CVD::ESMResult::delta
double delta
norm of the last update
Definition: esm.h:62
CVD::HomographyPrefix
This class provides a generic 2D homography transformation, but also applies a fixed transformation o...
Definition: esm.h:233
Generated by   doxygen 1.8.13