vgl_h_matrix_2d.hxx
Go to the documentation of this file.
1 // This is core/vgl/algo/vgl_h_matrix_2d.hxx
2 #ifndef vgl_h_matrix_2d_hxx_
3 #define vgl_h_matrix_2d_hxx_
4 
5 #include <limits>
6 #include <cstdlib>
7 #include <fstream>
8 #include <iostream>
9 #include "vgl_h_matrix_2d.h"
10 #include <vnl/vnl_inverse.h>
11 #include <vnl/vnl_vector_fixed.h>
12 #include <vnl/algo/vnl_svd.h>
13 #include <vcl_compiler_detection.h>
14 #ifdef _MSC_VER
15 # include <vcl_msvc_warnings.h>
16 #endif
17 #include <cassert>
18 #include <vcl_deprecated.h>
19 
20 template <class T>
22 {
23  t12_matrix_.read_ascii(s);
24 }
25 
26 template <class T>
28 {
29  std::ifstream f(filename);
30  if (!f.good())
31  std::cerr << "vgl_h_matrix_2d::read: Error opening " << filename << std::endl;
32  else
33  t12_matrix_.read_ascii(f);
34 }
35 
36 template <class T>
38  std::vector<vgl_homg_point_2d<T> > const& points2)
39 {
40  vnl_matrix<T> W;
41  assert(points1.size() == points2.size());
42  unsigned int numpoints = points1.size();
43  if (numpoints < 4)
44  {
45  std::cerr << "\nvhl_h_matrix_2d - minimum of 4 points required\n";
46  std::exit(0);
47  }
48 
49  W.set_size(2*numpoints, 9);
50 
51  for (unsigned int i = 0; i < numpoints; ++i)
52  {
53  T x1 = points1[i].x(), y1 = points1[i].y(), w1 = points1[i].w();
54  T x2 = points2[i].x(), y2 = points2[i].y(), w2 = points2[i].w();
55 
56  W[i*2][0]=x1*w2; W[i*2][1]=y1*w2; W[i*2][2]=w1*w2;
57  W[i*2][3]=0.0; W[i*2][4]=0.0; W[i*2][5]=0.0;
58  W[i*2][6]=-x1*x2; W[i*2][7]=-y1*x2; W[i*2][8]=-w1*x2;
59 
60  W[i*2+1][0]=0.0; W[i*2+1][1]=0.0; W[i*2+1][2]=0.0;
61  W[i*2+1][3]=x1*w2; W[i*2+1][4]=y1*w2; W[i*2+1][5]=w1*w2;
62  W[i*2+1][6]=-x1*y2; W[i*2+1][7]=-y1*y2; W[i*2+1][8]=-w1*y2;
63  }
64 
65  vnl_svd<T> SVD(W);
66  t12_matrix_ = vnl_matrix_fixed<T,3,3>(SVD.nullvector().data_block()); // 9-dim. nullvector
67 }
68 
69 template <class T>
70 vgl_h_matrix_2d<T>::vgl_h_matrix_2d(vnl_matrix_fixed<T,2,2> const& M,
71  vnl_vector_fixed<T,2> const& m)
72 {
73  for (int r = 0; r < 2; ++r) {
74  for (int c = 0; c < 2; ++c)
75  (t12_matrix_)(r, c) = M(r,c);
76  (t12_matrix_)(r, 2) = m(r);
77  }
78  for (int c = 0; c < 2; ++c)
79  (t12_matrix_)(2,c) = 0;
80  (t12_matrix_)(2,2) = 1;
81 }
82 
83 
84 // == OPERATIONS ==
85 
86 template <class T>
89 {
90  vnl_vector_fixed<T,3> v2 = t12_matrix_ * vnl_vector_fixed<T,3>(p.x(), p.y(), p.w());
91  return vgl_homg_point_2d<T>(v2[0], v2[1], v2[2]);
92 }
93 
94 template <class T>
97 {
98  vnl_vector_fixed<T,3> v2 = t12_matrix_ * vnl_vector_fixed<T,3>(p.x(), p.y(), p.w());
99  return vgl_homg_line_2d<T>(v2[0], v2[1], v2[2]);
100 }
101 
102 template <class T>
105 {
106  vnl_vector_fixed<T,3> v2 = t12_matrix_.transpose() * vnl_vector_fixed<T,3>(l.a(), l.b(), l.c());
107  return vgl_homg_line_2d<T>(v2[0], v2[1], v2[2]);
108 }
109 
110 template <class T>
113 {
114  vnl_vector_fixed<T,3> v2 = t12_matrix_ * vnl_vector_fixed<T,3>(l.a(), l.b(), l.c());
115  return vgl_homg_point_2d<T>(v2[0], v2[1], v2[2]);
116 }
117 
118 template <class T>
121 {
122  T a=C.a(), b=C.b()/2, c = C.c(), d = C.d()/2, e = C.e()/2, f = C.f();
123  vnl_matrix_fixed<T,3,3> M, Mp;
124  M(0,0) = a; M(0,1) = b; M(0,2) = d;
125  M(1,0) = b; M(1,1) = c; M(1,2) = e;
126  M(2,0) = d; M(2,1) = e; M(2,2) = f;
127  Mp = (t12_matrix_.transpose())*M*t12_matrix_;
128  return vgl_conic<T>(Mp(0,0),(Mp(0,1)+Mp(1,0)),Mp(1,1),(Mp(0,2)+Mp(2,0)),
129  (Mp(1,2)+Mp(2,1)), Mp(2,2));
130 }
131 
132 template <class T>
135 {
136  T a=C.a(), b=C.b()/2, c = C.c(), d = C.d()/2, e = C.e()/2, f = C.f();
137  vnl_matrix_fixed<T,3,3> M, Mp;
138  M(0,0) = a; M(0,1) = b; M(0,2) = d;
139  M(1,0) = b; M(1,1) = c; M(1,2) = e;
140  M(2,0) = d; M(2,1) = e; M(2,2) = f;
141  Mp = vnl_inverse_transpose(t12_matrix_)*M*vnl_inverse(t12_matrix_);
142  return vgl_conic<T>(Mp(0,0),(Mp(0,1)+Mp(1,0)),Mp(1,1),(Mp(0,2)+Mp(2,0)),
143  (Mp(1,2)+Mp(2,1)), Mp(2,2));
144 }
145 
146 
147 template <class T>
150 {
151  vnl_vector_fixed<T,3> v = vnl_inverse(t12_matrix_) * vnl_vector_fixed<T,3>(p.x(), p.y(), p.w());
152  return vgl_homg_point_2d<T>(v[0], v[1], v[2]);
153 }
154 
155 template <class T>
158 {
159  vnl_vector_fixed<T,3> v = vnl_inverse_transpose(t12_matrix_) * vnl_vector_fixed<T,3>(l.a(), l.b(), l.c());
160  return vgl_homg_line_2d<T>(v[0], v[1], v[2]);
161 }
162 
163 template <class T>
164 std::ostream& operator<<(std::ostream& s, vgl_h_matrix_2d<T> const& h)
165 {
166  return s << h.get_matrix();
167 }
168 
169 template <class T>
170 bool vgl_h_matrix_2d<T>::read(std::istream& s)
171 {
172  t12_matrix_.read_ascii(s);
173  return s.good() || s.eof();
174 }
175 
176 template <class T>
177 bool vgl_h_matrix_2d<T>::read(char const* filename)
178 {
179  std::ifstream f(filename);
180  if (!f.good())
181  std::cerr << "vgl_h_matrix_2d::read: Error opening " << filename << std::endl;
182  return read(f);
183 }
184 
185 // == DATA ACCESS ==
186 
187 template <class T>
188 T vgl_h_matrix_2d<T>::get(unsigned int row_index, unsigned int col_index) const
189 {
190  return t12_matrix_. get(row_index, col_index);
191 }
192 
193 template <class T>
194 void vgl_h_matrix_2d<T>::get(T* H) const
195 {
196  for (T const* iter = t12_matrix_.begin(); iter < t12_matrix_.end(); ++iter)
197  *H++ = *iter;
198 }
199 
200 template <class T>
201 void vgl_h_matrix_2d<T>::get(vnl_matrix_fixed<T,3,3>* H) const
202 {
203  *H = t12_matrix_;
204 }
205 
206 template <class T>
207 void vgl_h_matrix_2d<T>::get(vnl_matrix<T>* H) const
208 {
209  VXL_DEPRECATED_MACRO("vgl_h_matrix_2d<T>::get(vnl_matrix<T>*) const");
210  *H = t12_matrix_.as_ref(); // size 3x3
211 }
212 
213 template <class T>
216 {
217  t12_matrix_.set_identity();
218  return *this;
219 }
220 
221 template <class T>
224 {
225  for (T* iter = t12_matrix_.begin(); iter < t12_matrix_.end(); ++iter)
226  *iter = *H++;
227  return *this;
228 }
229 
230 template <class T>
232 vgl_h_matrix_2d<T>::set(vnl_matrix_fixed<T,3,3> const& H)
233 {
234  t12_matrix_ = H;
235  return *this;
236 }
237 
238 //-------------------------------------------------------------------
239 template <class T>
241 projective_basis(std::vector<vgl_homg_point_2d<T> > const& points)
242 {
243  if (points.size()!=4)
244  return false;
245  vnl_vector_fixed<T,3> p0(points[0].x(), points[0].y(), points[0].w());
246  vnl_vector_fixed<T,3> p1(points[1].x(), points[1].y(), points[1].w());
247  vnl_vector_fixed<T,3> p2(points[2].x(), points[2].y(), points[2].w());
248  vnl_vector_fixed<T,3> p3(points[3].x(), points[3].y(), points[3].w());
249  vnl_matrix_fixed<T,3,4> point_matrix;
250  point_matrix.set_column(0, p0);
251  point_matrix.set_column(1, p1);
252  point_matrix.set_column(2, p2);
253  point_matrix.set_column(3, p3);
254 
255  if (! point_matrix.is_finite() || point_matrix.has_nans())
256  {
257  std::cerr << "vgl_h_matrix_2d<T>::projective_basis():\n"
258  << " given points have infinite or NaN values\n";
259  this->set_identity();
260  return false;
261  }
262  vnl_svd<T> svd1(point_matrix.as_ref(), 1e-8); // size 3x4
263  if (svd1.rank() < 3)
264  {
265  std::cerr << "vgl_h_matrix_2d<T>::projective_basis():\n"
266  << " At least three out of the four points are nearly collinear\n";
267  this->set_identity();
268  return false;
269  }
270 
271  vnl_matrix_fixed<T,3,3> back_matrix;
272  back_matrix.set_column(0, p0);
273  back_matrix.set_column(1, p1);
274  back_matrix.set_column(2, p2);
275 
276  vnl_vector_fixed<T,3> scales_vector = vnl_inverse(back_matrix) * p3;
277 
278  back_matrix.set_column(0, scales_vector[0] * p0);
279  back_matrix.set_column(1, scales_vector[1] * p1);
280  back_matrix.set_column(2, scales_vector[2] * p2);
281 
282  if (! back_matrix.is_finite() || back_matrix.has_nans())
283  {
284  std::cerr << "vgl_h_matrix_2d<T>::projective_basis():\n"
285  << " back matrix has infinite or NaN values\n";
286  this->set_identity();
287  return false;
288  }
289  this->set(vnl_inverse(back_matrix));
290  return true;
291 }
292 
293 template <class T>
295 {
296  return t12_matrix_.get(0,2) == (T)0
297  && t12_matrix_.get(1,2) == (T)0
298  && this->is_euclidean();
299 }
300 
301 template <class T>
303 {
304  if ( t12_matrix_.get(2,0) != (T)0 ||
305  t12_matrix_.get(2,1) != (T)0 ||
306  t12_matrix_.get(2,2) != (T)1 )
307  return false; // should not have a translation part
308 
309  // use an error tolerance on the orthonormality constraint
310  vnl_matrix_fixed<T,2,2> R = get_upper_2x2_matrix();
311  R *= R.transpose();
312  R(0,0) -= T(1);
313  R(1,1) -= T(1);
314  return R.absolute_value_max() <= 10*std::numeric_limits<T>::epsilon();
315 }
316 
317 template <class T>
319 {
320  return t12_matrix_.is_identity();
321 }
322 
323 //-------------------------------------------------------------------
324 template <class T>
326 {
327  if (lines.size()!=4)
328  return false;
329  vnl_vector_fixed<T,3> l0(lines[0].a(), lines[0].b(), lines[0].c());
330  vnl_vector_fixed<T,3> l1(lines[1].a(), lines[1].b(), lines[1].c());
331  vnl_vector_fixed<T,3> l2(lines[2].a(), lines[2].b(), lines[2].c());
332  vnl_vector_fixed<T,3> l3(lines[3].a(), lines[3].b(), lines[3].c());
333  vnl_matrix_fixed<T,3,4> line_matrix;
334  line_matrix.set_column(0, l0);
335  line_matrix.set_column(1, l1);
336  line_matrix.set_column(2, l2);
337  line_matrix.set_column(3, l3);
338 
339  if (! line_matrix.is_finite() || line_matrix.has_nans())
340  {
341  std::cerr << "vgl_h_matrix_2d<T>::projective_basis():\n"
342  << " given lines have infinite or NaN values\n";
343  this->set_identity();
344  return false;
345  }
346  vnl_svd<T> svd1(line_matrix.as_ref(), 1e-8); // size 3x4
347  if (svd1.rank() < 3)
348  {
349  std::cerr << "vgl_h_matrix_2d<T>::projective_basis():\n"
350  << " At least three out of the four lines are nearly concurrent\n";
351  this->set_identity();
352  return false;
353  }
354 
355  vnl_matrix_fixed<T,3,3> back_matrix;
356  back_matrix.set_column(0, l0);
357  back_matrix.set_column(1, l1);
358  back_matrix.set_column(2, l2);
359  vnl_vector_fixed<T,3> scales_vector = vnl_inverse(back_matrix) * l3;
360  back_matrix.set_row(0, scales_vector[0] * l0);
361  back_matrix.set_row(1, scales_vector[1] * l1);
362  back_matrix.set_row(2, scales_vector[2] * l2);
363 
364  if (! back_matrix.is_finite() || back_matrix.has_nans())
365  {
366  std::cerr << "vgl_h_matrix_2d<T>::projective_basis():\n"
367  << " back matrix has infinite or NaN values\n";
368  this->set_identity();
369  return false;
370  }
371  this->set(back_matrix);
372  return true;
373 }
374 
375 template <class T>
378 {
379  return vgl_h_matrix_2d<T>(vnl_inverse(t12_matrix_));
380 }
381 
382 
383 template <class T>
386 {
387  t12_matrix_[0][2] = tx; t12_matrix_[1][2] = ty;
388  return *this;
389 }
390 
391 template <class T>
394 {
395  double theta_d = (double)theta;
396  double c = std::cos(theta_d), s = std::sin(theta_d);
397  t12_matrix_[0][0] = (T)c; t12_matrix_[0][1] = -(T)s;
398  t12_matrix_[1][0] = (T)s; t12_matrix_[1][1] = (T)c;
399  return *this;
400 }
401 
402 template <class T>
405 {
406  for (unsigned r = 0; r<2; ++r)
407  for (unsigned c = 0; c<3; ++c)
408  t12_matrix_[r][c]*=scale;
409  return *this;
410 }
411 
412 template <class T>
415  T tx, T ty)
416 {
417  T a=s*std::cos(theta);
418  T b=s*std::sin(theta);
419  t12_matrix_[0][0] = a; t12_matrix_[0][1] = -b; t12_matrix_[0][2] = tx;
420  t12_matrix_[1][0] = b; t12_matrix_[1][1] = a; t12_matrix_[1][2] = ty;
421  t12_matrix_[2][0]=T(0); t12_matrix_[2][1]=T(0); t12_matrix_[2][2] = T(1);
422  return *this;
423 }
424 
425 template <class T>
428 {
429  for (unsigned c = 0; c<3; ++c)
430  t12_matrix_[1][c]*=aspect_ratio;
431  return *this;
432 }
433 
434 
435 template <class T>
437 vgl_h_matrix_2d<T>::set_affine(vnl_matrix_fixed<T,2,3> const& M23)
438 {
439  for (unsigned r = 0; r<2; ++r)
440  for (unsigned c = 0; c<3; ++c)
441  t12_matrix_[r][c] = M23[r][c];
442  t12_matrix_[2][0] = T(0); t12_matrix_[2][1] = T(0); t12_matrix_[2][2] = T(1);
443  return *this;
444 }
445 
446 template <class T>
448 vgl_h_matrix_2d<T>::set_affine(vnl_matrix<T> const& M23)
449 {
450  VXL_DEPRECATED_MACRO("vgl_h_matrix_2d<T>::set_affine(vnl_matrix<T> const&)");
451  assert (M23.rows()==2 && M23.columns()==3);
452  for (unsigned r = 0; r<2; ++r)
453  for (unsigned c = 0; c<3; ++c)
454  t12_matrix_[r][c] = M23[r][c];
455  t12_matrix_[2][0] = T(0); t12_matrix_[2][1] = T(0); t12_matrix_[2][2] = T(1);
456  return *this;
457 }
458 
459 template <class T>
462 {
463  //only sensible for affine transformations
464  T d = t12_matrix_[2][2];
465  assert(d<-1e-9 || d>1e-9);
466  vnl_matrix_fixed<T,3,3> m(0.0);
467  for (unsigned r = 0; r<2; ++r)
468  for (unsigned c = 0; c<2; ++c)
469  m[r][c] = t12_matrix_[r][c]/d;
470  m[2][2]=1.0;
471  return vgl_h_matrix_2d<T>(m);
472 }
473 
474 template <class T>
475 vnl_matrix_fixed<T,2,2>
477 {
478  vnl_matrix_fixed<T,2,2> R;
479  vgl_h_matrix_2d<T> m = this->get_upper_2x2();
480  for (unsigned r = 0; r<3; ++r)
481  for (unsigned c = 0; c<3; ++c)
482  R[r][c] = m.get(r,c);
483  return R;
484 }
485 
486 template <class T>
489 {
490  //only sensible for affine transformations
491  T d = t12_matrix_[2][2];
492  assert(d<-1e-9 || d>1e-9);
493  return vgl_homg_point_2d<T>(t12_matrix_[0][2]/d,
494  t12_matrix_[1][2]/d,
495  (T)1);
496 }
497 
498 template <class T>
499 vnl_vector_fixed<T,2>
501 {
502  vgl_homg_point_2d<T> p = this->get_translation();
503  return vnl_vector_fixed<T,2>(p.x(), p.y());
504 }
505 
506 //----------------------------------------------------------------------------
507 #undef VGL_H_MATRIX_2D_INSTANTIATE
508 #define VGL_H_MATRIX_2D_INSTANTIATE(T) \
509 template class vgl_h_matrix_2d<T >; \
510 template std::ostream& operator << (std::ostream& s, vgl_h_matrix_2d<T > const& h); \
511 template std::istream& operator >> (std::istream& s, vgl_h_matrix_2d<T >& h)
512 
513 #endif // vgl_h_matrix_2d_hxx_
A class to hold a plane-to-plane projective transformation matrix and to perform common operations us...
Definition: vgl_algo_fwd.h:11
vnl_matrix_fixed< T, 3, 3 > const & get_matrix() const
Return the 3x3 homography matrix.
bool read(std::istream &s)
Read H from std::istream.
std::istream & read(std::istream &is)
Read from stream, possibly with formatting.
Represents a homogeneous 2D line.
Definition: vgl_fwd.h:14
vgl_h_matrix_2d & set_aspect_ratio(T aspect_ratio)
compose the transform with diagonal aspect ratio transform.
vgl_h_matrix_2d< T > get_upper_2x2() const
corresponds to rotation for Euclidean transformations.
bool is_identity() const
vgl_h_matrix_2d & set_affine(vnl_matrix_fixed< T, 2, 3 > const &M23)
set the transform to a general affine transform matrix.
vgl_h_matrix_2d & set(unsigned int row_index, unsigned int col_index, T value)
Set an element of the 3x3 homography matrix.
vgl_h_matrix_2d & set_rotation(T theta)
the upper 2x2 part of the matrix is replaced by a rotation matrix.
bool is_euclidean() const
T f() const
Returns the coefficient of .
Definition: vgl_conic.h:135
3x3 plane-to-plane projectivity
std::ostream & operator<<(std::ostream &s, vgl_orient_box_3d< Type > const &p)
Write box to stream.
vgl_homg_point_2d< T > operator()(vgl_homg_point_2d< T > const &p) const
Return the transformed point given by $q = {\tt H} p$.
T a() const
Returns the coefficient of .
Definition: vgl_conic.h:120
vnl_matrix_fixed< T, 2, 2 > get_upper_2x2_matrix() const
corresponds to rotation for Euclidean transformations.
#define v
Definition: vgl_vector_2d.h:74
T c() const
Returns the coefficient of .
Definition: vgl_conic.h:126
vgl_h_matrix_2d & set_similarity(T s, T theta, T tx, T ty)
set the transform to a similarity mapping.
T e() const
Returns the coefficient of .
Definition: vgl_conic.h:132
A quadratic plane curve.
Definition: vgl_conic.h:70
vgl_homg_line_2d< T > preimage(vgl_homg_line_2d< T > const &l) const
Return the transformed line given by $m = {\tt H} l$.
T b() const
Returns the coefficient of .
Definition: vgl_conic.h:123
vnl_vector_fixed< T, 2 > get_translation_vector() const
corresponds to translation for affine transformations.
void get(vnl_matrix_fixed< T, 3, 3 > *M) const
Fill M with contents of the 3x3 homography matrix.
T d() const
Returns the coefficient of .
Definition: vgl_conic.h:129
vgl_homg_line_2d< T > correlation(vgl_homg_point_2d< T > const &p) const
#define l
vgl_h_matrix_2d & set_translation(T tx, T ty)
set H[0][2] = tx and H[1][2] = ty, other elements unaltered.
bool is_rotation() const
vgl_homg_point_2d< T > get_translation() const
corresponds to translation for affine transformations.
vgl_h_matrix_2d get_inverse() const
Return the inverse homography.
vgl_h_matrix_2d & set_scale(T scale)
compose the current transform with a uniform scaling transformation, S.
vgl_h_matrix_2d()=default
Represents a homogeneous 2D point.
Definition: vgl_fwd.h:8
bool projective_basis(std::vector< vgl_homg_point_2d< T > > const &four_points)
transformation to projective basis (canonical frame).
vgl_h_matrix_2d & set_identity()
initialize the transformation to identity.