2 #ifndef vgl_h_matrix_2d_hxx_ 3 #define vgl_h_matrix_2d_hxx_ 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> 15 # include <vcl_msvc_warnings.h> 18 #include <vcl_deprecated.h> 23 t12_matrix_.read_ascii(s);
29 std::ifstream f(filename);
31 std::cerr <<
"vgl_h_matrix_2d::read: Error opening " << filename << std::endl;
33 t12_matrix_.read_ascii(f);
41 assert(points1.size() == points2.size());
42 unsigned int numpoints = points1.size();
45 std::cerr <<
"\nvhl_h_matrix_2d - minimum of 4 points required\n";
49 W.set_size(2*numpoints, 9);
51 for (
unsigned int i = 0; i < numpoints; ++i)
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();
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;
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;
66 t12_matrix_ = vnl_matrix_fixed<T,3,3>(SVD.nullvector().data_block());
71 vnl_vector_fixed<T,2>
const& m)
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);
78 for (
int c = 0; c < 2; ++c)
79 (t12_matrix_)(2,c) = 0;
80 (t12_matrix_)(2,2) = 1;
90 vnl_vector_fixed<T,3> v2 = t12_matrix_ * vnl_vector_fixed<T,3>(p.
x(), p.
y(), p.
w());
98 vnl_vector_fixed<T,3> v2 = t12_matrix_ * vnl_vector_fixed<T,3>(p.
x(), p.
y(), p.
w());
106 vnl_vector_fixed<T,3> v2 = t12_matrix_.transpose() * vnl_vector_fixed<T,3>(
l.a(),
l.b(),
l.c());
114 vnl_vector_fixed<T,3> v2 = t12_matrix_ * vnl_vector_fixed<T,3>(
l.a(),
l.b(),
l.c());
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));
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));
151 vnl_vector_fixed<T,3>
v = vnl_inverse(t12_matrix_) * vnl_vector_fixed<T,3>(p.
x(), p.
y(), p.
w());
159 vnl_vector_fixed<T,3>
v = vnl_inverse_transpose(t12_matrix_) * vnl_vector_fixed<T,3>(
l.a(),
l.b(),
l.c());
172 t12_matrix_.read_ascii(s);
173 return s.good() || s.eof();
179 std::ifstream f(filename);
181 std::cerr <<
"vgl_h_matrix_2d::read: Error opening " << filename << std::endl;
190 return t12_matrix_. get(row_index, col_index);
196 for (T
const* iter = t12_matrix_.begin(); iter < t12_matrix_.end(); ++iter)
209 VXL_DEPRECATED_MACRO(
"vgl_h_matrix_2d<T>::get(vnl_matrix<T>*) const");
210 *H = t12_matrix_.as_ref();
225 for (T* iter = t12_matrix_.begin(); iter < t12_matrix_.end(); ++iter)
243 if (points.size()!=4)
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);
255 if (! point_matrix.is_finite() || point_matrix.has_nans())
257 std::cerr <<
"vgl_h_matrix_2d<T>::projective_basis():\n" 258 <<
" given points have infinite or NaN values\n";
259 this->set_identity();
262 vnl_svd<T> svd1(point_matrix.as_ref(), 1e-8);
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();
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);
276 vnl_vector_fixed<T,3> scales_vector = vnl_inverse(back_matrix) * p3;
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);
282 if (! back_matrix.is_finite() || back_matrix.has_nans())
284 std::cerr <<
"vgl_h_matrix_2d<T>::projective_basis():\n" 285 <<
" back matrix has infinite or NaN values\n";
286 this->set_identity();
289 this->set(vnl_inverse(back_matrix));
296 return t12_matrix_.get(0,2) == (T)0
297 && t12_matrix_.get(1,2) == (T)0
298 && this->is_euclidean();
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 )
310 vnl_matrix_fixed<T,2,2> R = get_upper_2x2_matrix();
314 return R.absolute_value_max() <= 10*std::numeric_limits<T>::epsilon();
320 return t12_matrix_.is_identity();
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);
339 if (! line_matrix.is_finite() || line_matrix.has_nans())
341 std::cerr <<
"vgl_h_matrix_2d<T>::projective_basis():\n" 342 <<
" given lines have infinite or NaN values\n";
343 this->set_identity();
346 vnl_svd<T> svd1(line_matrix.as_ref(), 1e-8);
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();
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);
364 if (! back_matrix.is_finite() || back_matrix.has_nans())
366 std::cerr <<
"vgl_h_matrix_2d<T>::projective_basis():\n" 367 <<
" back matrix has infinite or NaN values\n";
368 this->set_identity();
371 this->set(back_matrix);
387 t12_matrix_[0][2] = tx; t12_matrix_[1][2] = ty;
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;
406 for (
unsigned r = 0; r<2; ++r)
407 for (
unsigned c = 0; c<3; ++c)
408 t12_matrix_[r][c]*=scale;
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);
429 for (
unsigned c = 0; c<3; ++c)
430 t12_matrix_[1][c]*=aspect_ratio;
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);
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);
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;
475 vnl_matrix_fixed<T,2,2>
478 vnl_matrix_fixed<T,2,2> R;
480 for (
unsigned r = 0; r<3; ++r)
481 for (
unsigned c = 0; c<3; ++c)
482 R[r][c] = m.
get(r,c);
491 T d = t12_matrix_[2][2];
492 assert(d<-1e-9 || d>1e-9);
499 vnl_vector_fixed<T,2>
503 return vnl_vector_fixed<T,2>(p.
x(), p.
y());
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) 513 #endif // vgl_h_matrix_2d_hxx_ A class to hold a plane-to-plane projective transformation matrix and to perform common operations us...
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.
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.
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 .
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 .
vnl_matrix_fixed< T, 2, 2 > get_upper_2x2_matrix() const
corresponds to rotation for Euclidean transformations.
T c() const
Returns the coefficient of .
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 .
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 .
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 .
vgl_homg_line_2d< T > correlation(vgl_homg_point_2d< T > const &p) const
vgl_h_matrix_2d & set_translation(T tx, T ty)
set H[0][2] = tx and H[1][2] = ty, other elements unaltered.
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.
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.