1 // This is mul/vimt3d/vimt3d_transform_3d.cxx
2 #include <iostream>
3 #include <cstdlib>
4 #include <sstream>
5 #include "vimt3d_transform_3d.h"
6 //:
7 // \file
8 // \brief A class to define and apply a 3D transformation up to affine.
9 // \author Graham Vincent, Tim Cootes
10
11 #include <cassert>
12 #ifdef _MSC_VER
13 # include "vcl_msvc_warnings.h"
14 #endif
15 #include "vsl/vsl_indent.h"
16 #include "vnl/vnl_matrix_fixed.h"
17 #include "vnl/vnl_vector.h"
18 #include <vnl/algo/vnl_determinant.h>
19 #include "vnl/vnl_math.h"
20 #include "vul/vul_string.h"
21 #include "vul/vul_sprintf.h"
22 #include <mbl/mbl_read_props.h>
23 #include <mbl/mbl_exception.h>
24 #include <mbl/mbl_parse_sequence.h>
25
26
27 //=======================================================================
28
matrix() const29 vnl_matrix<double> vimt3d_transform_3d::matrix() const
30 {
31 vnl_matrix<double> M(4,4);
32 matrix(M);
33 return M;
34 }
35
36 //=======================================================================
37
matrix(vnl_matrix<double> & M) const38 void vimt3d_transform_3d::matrix(vnl_matrix<double>& M) const
39 {
40 #if 0 //grv
41 if ((M.rows()!=4) || (M.columns()!=4)) M.resize(4,4);
42 #endif
43 M.set_size(4,4);
44 double**m_data = M.data_array();
45 m_data[0][0]=xx_; m_data[0][1]=xy_; m_data[0][2]=xz_; m_data[0][3]=xt_;
46 m_data[1][0]=yx_; m_data[1][1]=yy_; m_data[1][2]=yz_; m_data[1][3]=yt_;
47 m_data[2][0]=zx_; m_data[2][1]=zy_; m_data[2][2]=zz_; m_data[2][3]=zt_;
48 m_data[3][0]=tx_; m_data[3][1]=ty_; m_data[3][2]=tz_; m_data[3][3]=tt_;
49 }
50
51 //=======================================================================
52 // Define the transform in terms of a 4x4 homogeneous matrix.
set_matrix(const vnl_matrix<double> & M)53 void vimt3d_transform_3d::set_matrix(const vnl_matrix<double>& M)
54 {
55 if (M.rows()!=4 || M.cols()!=4)
56 mbl_exception_error(mbl_exception_abort("vimt3d_transform_3d::set_matrix(matrix): input matrix must be 4x4"));
57
58 form_=Affine;
59 xx_=M[0][0]; xy_=M[0][1]; xz_=M[0][2]; xt_=M[0][3];
60 yx_=M[1][0]; yy_=M[1][1]; yz_=M[1][2]; yt_=M[1][3];
61 zx_=M[2][0]; zy_=M[2][1]; zz_=M[2][2]; zt_=M[2][3];
62 tx_=M[3][0]; ty_=M[3][1]; tz_=M[3][2]; tt_=M[3][3];
63 }
64
65 //=======================================================================
66 // See also vnl_rotation_matrix(), vgl_rotation_3d, and vnl_quaternion
angles(double & phi_x,double & phi_y,double & phi_z) const67 void vimt3d_transform_3d::angles(double& phi_x, double& phi_y, double& phi_z) const
68 {
69 // NB: in affine case will probably have to store s_x, s_y, s_z etc somewhere else!
70 // also won't work properly in rigid body case either!
71 double det=+xx_*yy_*zz_-xx_*zy_*yz_-yx_*xy_*zz_+yx_*zy_*xz_+zx_*xy_*yz_-zx_*yy_*xz_;
72
73 double xlen = std::sqrt(xx_*xx_ + yx_*yx_ + zx_*zx_)* vnl_math::sgn(det);
74 double ylen = std::sqrt(xy_*xy_ + yy_*yy_ + zy_*zy_)* vnl_math::sgn(det);
75 double zlen = std::sqrt(xz_*xz_ + yz_*yz_ + zz_*zz_)* vnl_math::sgn(det);
76
77 double xx3 = xx_ / xlen;
78 double xy3 = xy_ / ylen;
79 double xz3 = xz_ / zlen;
80 double yz3 = yz_ / zlen;
81 double zz3 = zz_ / zlen;
82
83 phi_x = std::atan2(-yz3,zz3);
84 phi_y = std::atan2(-xz3*std::cos(phi_x),zz3);
85 phi_z=std::atan2(-xy3,xx3);
86
87 // nb the equation for phi_z doesn't work in affine case
88 // because sy and sx aren't necessarily the same
89
90 // calc scaling factor
91 // ie assuming similarity transform here
92 // assume s is always positive
93 // to recover original angle
94 double s;
95 if (std::sin(phi_y) < 1e-20)
96 s = 1.0;
97 else
98 s = std::fabs( xz3/ (-1*std::sin(phi_y) ) );
99
100 #ifdef DEBUG
101 std::cout<<"s= "<<s<<std::endl;
102 #endif
103
104 // the angles may be wrong by +-vnl_math::pi - we can
105 // only tell by checking against the signs
106 // of the original entries in the rotation matrix
107 if (std::fabs(std::sin(phi_y)*s + xz3) > 1e-6)
108 {
109 if (phi_y > 0)
110 phi_y -= vnl_math::pi;
111 else
112 phi_y += vnl_math::pi;
113 //phi_y *= -1;
114 }
115
116 const double cos_y = std::cos(phi_y);
117
118 if (std::fabs(std::sin(phi_x)*cos_y*s + yz3) > 1e-6 ||
119 std::fabs(std::cos(phi_x)*cos_y*s - zz3) > 1e-6)
120 {
121 if (phi_x > 0)
122 phi_x -= vnl_math::pi;
123 else
124 phi_x += vnl_math::pi;
125 }
126
127 if (std::fabs(std::cos(phi_z)*cos_y*s - xx3) > 1e-6 ||
128 std::fabs(std::sin(phi_z)*cos_y*s + xy3) > 1e-6)
129 {
130 if (phi_z > 0)
131 phi_z -= vnl_math::pi;
132 else
133 phi_z += vnl_math::pi;
134 }
135
136 // now compress the angles towards zero as much as possible
137 // (we can add +-vnl_math::pi to each angle and negate phi_y without changing
138 // the rotation matrix)
139 int count = 0;
140 if (std::fabs(phi_x) > vnl_math::pi/2) ++count;
141 if (std::fabs(phi_y) > vnl_math::pi/2) ++count;
142 if (std::fabs(phi_z) > vnl_math::pi/2) ++count;
143
144 if (count > 1)
145 {
146 if (phi_x > 0)
147 phi_x -= vnl_math::pi;
148 else
149 phi_x += vnl_math::pi;
150
151 phi_y=-phi_y;
152 if (phi_y > 0)
153 phi_y -= vnl_math::pi;
154 else
155 phi_y += vnl_math::pi;
156
157 if (phi_z > 0)
158 phi_z -= vnl_math::pi;
159 else
160 phi_z += vnl_math::pi;
161 }
162 }
163
164 //=======================================================================
165
params(vnl_vector<double> & v) const166 void vimt3d_transform_3d::params(vnl_vector<double>& v) const
167 {
168 switch (form_)
169 {
170 case (Identity):
171 v.set_size(0);
172 break;
173 case (Translation):
174 if (v.size()!=3) v.set_size(3);
175 v[0]=xt_; v[1]=yt_; v[2]=zt_;
176 break;
177 case (ZoomOnly):
178 if (v.size()!=6) v.set_size(6);
179 v[0]=xx_; v[1]=yy_; v[2]=zz_;
180 v[3]=xt_; v[4]=yt_; v[5]=zt_;
181 break;
182 case (RigidBody):
183 if (v.size()!=6) v.set_size(6);
184 angles(v[0],v[1],v[2]);
185 v[3]=xt_; v[4]=yt_; v[5]=zt_;
186 break;
187 case (Similarity): // not sure this is right - kds
188 // I think it's fixed now -dac
189 if (v.size()!=7) v.set_size(7);
190 angles(v[1],v[2],v[3]);
191 // compute scaling factor
192 v[0]= xx_/ ( std::cos( v[2] ) *std::cos( v[3] ) );
193 v[4]=xt_; v[5]=yt_; v[6]=zt_;
194 break;
195 case (Affine): // not sure this is right - kds
196 // I'm sure it's not correct -dac
197 {
198 v.set_size(9);
199 // computation of angles doesn't work unless
200 // sx, sy, sz are all the same!
201
202 angles(v[3],v[4],v[5]);
203 // try to compute scaling factors
204 double det=+xx_*yy_*zz_-xx_*zy_*yz_-yx_*xy_*zz_+yx_*zy_*xz_+zx_*xy_*yz_-zx_*yy_*xz_;
205 v[0]=std::sqrt(xx_*xx_ + yx_*yx_ + zx_*zx_)* vnl_math::sgn(det);
206 v[1]=std::sqrt(xy_*xy_ + yy_*yy_ + zy_*zy_)* vnl_math::sgn(det);
207 v[2]=std::sqrt(xz_*xz_ + yz_*yz_ + zz_*zz_)* vnl_math::sgn(det);
208 v[6]=xt_; v[7]=yt_; v[8]=zt_;
209 break;
210 }
211 default:
212 mbl_exception_error(mbl_exception_abort(
213 vul_sprintf("vimt3d_transform_3d::params() Unexpected form: %d", form_) ));
214 }
215 }
216
217
218 //=======================================================================
simplify(double tol)219 void vimt3d_transform_3d::simplify(double tol /*=1e-10*/)
220 {
221 double rx, ry, rz;
222 double sx, sy, sz;
223 double det;
224 switch (form_)
225 {
226 case Affine:
227 { // Not really true affine, because shear is forbidden.
228 angles(rx, ry, rz);
229 double matrix_form[]= {xx_, yx_, zx_, xy_, yy_, zy_, xz_, yz_, zz_};
230 vnl_matrix_fixed<double, 3, 3> X(matrix_form);
231 vnl_matrix_fixed<double, 3, 3> S2 = X.transpose() * X;
232 // if X=R*S (where S is a diagonal matrix) then X'X = S'*R'*R*S
233 // if R is a rotation matrix then R'*R=I and so X'X = S'*S = [s_x^2 0 0; 0 s_y^2 0; 0 0 s_z^2]
234 if (S2(0,1)*S2(0,1) + S2(0,2)*S2(0,2) + S2(1,0)*S2(1,0) +
235 S2(1,2)*S2(1,2) + S2(2,0)*S2(2,0) + S2(2,1)*S2(2,1) >= tol*tol*6)
236 return;
237
238 // mirroring if det is negative;
239 double mirror=vnl_math::sgn(vnl_determinant(X[0], X[1], X[2]));
240
241 sx = std::sqrt(std::abs(S2(0,0))) * mirror;
242 sy = std::sqrt(std::abs(S2(1,1))) * mirror;
243 sz = std::sqrt(std::abs(S2(2,2))) * mirror;
244 if (vnl_math::sqr(sx-sy) + vnl_math::sqr(sx-sz) < tol*tol)
245 this->set_similarity(sx, rx, ry, rz,
246 xt_, yt_, zt_ );
247 else if (rx*rx+ry*ry+rz*rz < tol*tol)
248 this->set_zoom_only(sx, sy, sz,
249 xt_, yt_, zt_);
250 else
251 return;
252 simplify(tol);
253 return;
254 }
255 case Similarity:
256 angles(rx, ry, rz);
257 det=+xx_*yy_*zz_-xx_*zy_*yz_-yx_*xy_*zz_+yx_*zy_*xz_+zx_*xy_*yz_-zx_*yy_*xz_;
258 sx=std::sqrt(xx_*xx_ + yx_*yx_ + zx_*zx_)* vnl_math::sgn(det);
259 if (rx*rx+ry*ry+rz*rz < tol*tol)
260 this->set_zoom_only(sx, xt_, yt_, zt_);
261 else if (vnl_math::sqr(sx-1.0) < tol*tol)
262 this->set_rigid_body(rx, ry, rz, xt_, yt_, zt_);
263 else
264 return;
265 simplify(tol);
266 return;
267
268 case RigidBody:
269 angles(rx, ry, rz);
270 if (rx*rx+ry*ry+rz*rz >= tol*tol)
271 return;
272 this->set_translation(xt_, yt_, zt_);
273 simplify(tol);
274 return;
275 case ZoomOnly:
276 if (vnl_math::sqr(xx_-1.0) + vnl_math::sqr(yy_-1.0) + vnl_math::sqr(zz_-1.0) >= tol*tol)
277 return;
278 set_translation(xt_, yt_, zt_);
279 case Translation:
280 if (xt_*xt_+yt_*yt_+zt_*zt_<tol*tol)
281 set_identity();
282 return;
283 case Identity:
284 return;
285 default:
286 mbl_exception_error(mbl_exception_abort(
287 vul_sprintf("vimt3d_transform_3d::simplify() Unexpected form: %d", form_) ));
288 }
289 }
290
291 //=======================================================================
292
setCheck(int n1,int n2,const char * str) const293 void vimt3d_transform_3d::setCheck(int n1,int n2,const char* str) const
294 {
295 if (n1==n2) return;
296 std::ostringstream ss;
297 ss << "vimt3d_transform_3d::set() " << n1 << " parameters required for "
298 << str << ". Passed " << n2;
299 mbl_exception_error(mbl_exception_abort(ss.str()));
300 }
301
302 //=======================================================================
303
set(const vnl_vector<double> & v,Form form)304 void vimt3d_transform_3d::set(const vnl_vector<double>& v, Form form)
305 {
306 int n=v.size();
307
308 switch (form)
309 {
310 case (Identity):
311 set_identity();
312 break;
313 case (Translation):
314 setCheck(3,n,"Translation");
315 set_translation(v[0],v[1],v[2]);
316 break;
317 case (ZoomOnly):
318 setCheck(6,n,"ZoomOnly");
319 set_zoom_only( v[0],v[1],v[2],
320 v[3],v[4],v[5]);
321 break;
322 case (RigidBody):
323 setCheck(6,n,"RigidBody");
324 set_rigid_body( v[0],v[1],v[2],
325 v[3],v[4],v[5]);
326 break;
327 case (Similarity):
328 setCheck(7,n,"Similarity");
329 set_similarity( v[0],v[1],v[2],
330 v[3],v[4],v[5], v[6]);
331 form_ = Similarity;
332 inv_uptodate_=false;
333 break;
334 case (Affine): // not sure this is right - kds
335 // it works unless you call params()
336 // later which gives the wrong answer
337 // in affine case -dac
338 setCheck(9,n,"Affine");
339 set_affine( v[0],v[1],v[2],
340 v[3],v[4],v[5],
341 v[6],v[7],v[8]);
342 form_ = Affine;
343 inv_uptodate_=false;
344 break;
345 default:
346 mbl_exception_error(mbl_exception_abort(
347 vul_sprintf("vimt3d_transform_3d::set() Unexpected form: %d", form_) ));
348 }
349 }
350
351 //=======================================================================
352 // See also vnl_rotation_matrix(), vgl_rotation_3d, and vnl_quaternion
setRotMat(double r_x,double r_y,double r_z)353 void vimt3d_transform_3d::setRotMat( double r_x, double r_y, double r_z )
354 {
355 double sinx=std::sin(r_x);
356 double siny=std::sin(r_y);
357 double sinz=std::sin(r_z);
358 double cosx=std::cos(r_x);
359 double cosy=std::cos(r_y);
360 double cosz=std::cos(r_z);
361
362 // set R_mat = Rx*Ry*Rz
363 xx_ = cosy*cosz;
364 xy_ = -cosy*sinz;
365 xz_ = -siny;
366 yx_ = cosx*sinz - sinx*siny*cosz;
367 yy_ = cosx*cosz + sinx*siny*sinz;
368 yz_ = -sinx*cosy;
369 zx_ = sinx*sinz + cosx*siny*cosz;
370 zy_ = sinx*cosz - cosx*siny*sinz;
371 zz_ = cosx*cosy;
372 }
373
374 //=======================================================================
375
set_origin(const vgl_point_3d<double> & p)376 void vimt3d_transform_3d::set_origin( const vgl_point_3d<double> & p )
377 {
378 xt_=p.x()*tt_;
379 yt_=p.y()*tt_;
380 zt_=p.z()*tt_;
381
382 if (form_ == Identity) form_=Translation;
383
384 inv_uptodate_=false;
385 }
386
387 //=======================================================================
388
set_identity()389 void vimt3d_transform_3d::set_identity()
390 {
391 if (form_==Identity) return;
392 form_=Identity;
393
394 xx_=yy_=zz_=tt_=1;
395 xy_=xz_=xt_=0;
396 yx_=yz_=yt_=0;
397 zx_=zy_=zt_=0;
398 tx_=ty_=tz_=0;
399 inv_uptodate_=false;
400 }
401
402 //=======================================================================
403
set_translation(double t_x,double t_y,double t_z)404 void vimt3d_transform_3d::set_translation(double t_x, double t_y, double t_z)
405 {
406 if (t_x==0 && t_y==0 && t_z==0)
407 set_identity();
408 else
409 {
410 form_=Translation;
411
412 // Set translation (first 3 elements of final column)
413 xt_=t_x;
414 yt_=t_y;
415 zt_=t_z;
416
417 // Set all other elements to defaults
418 xx_=yy_=zz_=tt_=1;
419 xy_=xz_=yx_=yz_=zx_=zy_=0;
420 tx_=ty_=tz_=0;
421 }
422
423 inv_uptodate_=false;
424 }
425
426 //=======================================================================
427
set_zoom_only(double s_x,double s_y,double s_z,double t_x,double t_y,double t_z)428 void vimt3d_transform_3d::set_zoom_only(double s_x, double s_y, double s_z,
429 double t_x, double t_y, double t_z)
430 {
431 form_=ZoomOnly;
432
433 // Set scaling (first 3 diagonal elements)
434 xx_=s_x;
435 yy_=s_y;
436 zz_=s_z;
437
438 // Set translation (first 3 elements of final column)
439 xt_=t_x;
440 yt_=t_y;
441 zt_=t_z;
442
443 // Set all other elements to defaults
444 tx_=ty_=tz_=0;
445 xy_=xz_=yx_=yz_=zx_=zy_=0;
446 tt_=1;
447
448 inv_uptodate_=false;
449 }
450
451 //=======================================================================
452
set_rigid_body(double r_x,double r_y,double r_z,double t_x,double t_y,double t_z)453 void vimt3d_transform_3d::set_rigid_body(double r_x, double r_y, double r_z,
454 double t_x, double t_y, double t_z)
455 {
456 if (r_x==0.0 && r_y==0.0 && r_z==0.0)
457 {
458 set_translation(t_x,t_y,t_z);
459 }
460 else
461 {
462 form_=RigidBody;
463
464 // Set rotation matrix
465 setRotMat(r_x,r_y,r_z);
466
467 // Set translation (first 3 elements of final column)
468 xt_=t_x;
469 yt_=t_y;
470 zt_=t_z;
471
472 // Set all other elements to defaults
473 tx_=0; ty_=0; tz_=0; tt_=1;
474 }
475
476 inv_uptodate_=false;
477 }
478
479 //: Sets the transformation to be rotation, followed by translation.
480 // The transformation is separable affine.
481 // \param q Unit quaternion defining rotation
set_rigid_body(const vnl_quaternion<double> & q,double t_x,double t_y,double t_z)482 void vimt3d_transform_3d::set_rigid_body(const vnl_quaternion<double>& q,
483 double t_x, double t_y, double t_z)
484 {
485 if (q.angle()==0.0)
486 {
487 set_translation(t_x,t_y,t_z);
488 return;
489 }
490 vnl_matrix_fixed<double,3,3> R = q.rotation_matrix_transpose();
491 form_=RigidBody;
492
493 // Set rotation terms from R, which is transpose of Rot mat.
494 xx_=R[0][0]; xy_= R[1][0]; xz_ = R[2][0];
495 yx_=R[0][1]; yy_= R[1][1]; yz_ = R[2][1];
496 zx_=R[0][2]; zy_= R[1][2]; zz_ = R[2][2];
497
498 // Set translation
499 xt_=t_x;
500 yt_=t_y;
501 zt_=t_z;
502
503 inv_uptodate_=false;
504 }
505
506 //: Sets the transformation to be similarity: scale, rotation, followed by translation.
507 // The transformation is separable affine.
508 // \param unit_q Unit quaternion defining rotation
set_similarity(double s,const vnl_quaternion<double> & q,double t_x,double t_y,double t_z)509 void vimt3d_transform_3d::set_similarity(double s, const vnl_quaternion<double>& q,
510 double t_x, double t_y, double t_z)
511 {
512 if (q.angle()==0.0)
513 {
514 set_zoom_only(s,t_x,t_y,t_z);
515 return;
516 }
517 vnl_matrix_fixed<double,3,3> R = q.rotation_matrix_transpose();
518 form_=RigidBody;
519
520 // Set scale/rotation terms from R, which is transpose of Rot mat.
521 xx_=s*R[0][0]; xy_= s*R[1][0]; xz_ = s*R[2][0];
522 yx_=s*R[0][1]; yy_= s*R[1][1]; yz_ = s*R[2][1];
523 zx_=s*R[0][2]; zy_= s*R[1][2]; zz_ = s*R[2][2];
524
525 // Set translation
526 xt_=t_x;
527 yt_=t_y;
528 zt_=t_z;
529
530 inv_uptodate_=false;
531 }
532 //=======================================================================
533
set_similarity(double s,double r_x,double r_y,double r_z,double t_x,double t_y,double t_z)534 void vimt3d_transform_3d::set_similarity(double s,
535 double r_x, double r_y, double r_z,
536 double t_x, double t_y, double t_z)
537 {
538 if (s==1.0)
539 {
540 set_rigid_body(r_x,r_y,r_z,t_x,t_y,t_z);
541 }
542 else
543 {
544 form_=Similarity;
545
546 // Set rotation matrix
547 setRotMat(r_x,r_y,r_z);
548
549 #ifdef DEBUG // debug test
550 double r_x1, r_y1, r_z1;
551 angles( r_x1, r_y1, r_z1 );
552 std::cout << "r_x = " << r_x << '\n'
553 << "r_x1= " << r_x1 << '\n'
554 << "r_y = " << r_y << '\n'
555 << "r_y1= " << r_y1 << '\n'
556 << "r_z = " << r_z << '\n'
557 << "r_z1= " << r_z1 << std::endl;
558 #endif
559
560 // Account for scaling (this actually means that scaling was done BEFORE rotation)
561 xx_*=s; xy_*=s; xz_*=s;
562 yx_*=s; yy_*=s; yz_*=s;
563 zx_*=s; zy_*=s; zz_*=s;
564
565 // Set translation (first 3 elements of final column)
566 xt_=t_x;
567 yt_=t_y;
568 zt_=t_z;
569
570 // Set all other elements to defaults
571 tx_=0; ty_=0; tz_=0; tt_=1;
572 }
573 inv_uptodate_=false;
574 }
575
576 //=======================================================================
577
set_affine(double s_x,double s_y,double s_z,double r_x,double r_y,double r_z,double t_x,double t_y,double t_z)578 void vimt3d_transform_3d::set_affine(double s_x, double s_y, double s_z,
579 double r_x, double r_y, double r_z,
580 double t_x, double t_y, double t_z)
581 {
582 form_=Affine;
583
584 // Set rotation matrix
585 setRotMat(r_x,r_y,r_z);
586
587 // Account for scaling (this actually means that scaling was done BEFORE rotation)
588 xx_*=s_x; xy_*=s_y; xz_*=s_z;
589 yx_*=s_x; yy_*=s_y; yz_*=s_z;
590 zx_*=s_x; zy_*=s_y; zz_*=s_z;
591
592 // Set translation (first 3 elements of final column)
593 xt_=t_x;
594 yt_=t_y;
595 zt_=t_z;
596
597 // Set all other elements to defaults
598 tx_=0; ty_=0; tz_=0; tt_=1;
599
600 inv_uptodate_=false;
601 }
602 //=======================================================================
603
set_affine(double s_x,double s_y,double s_z,vgl_vector_3d<double> c_x,vgl_vector_3d<double> c_y,vgl_vector_3d<double> c_z,double t_x,double t_y,double t_z)604 void vimt3d_transform_3d::set_affine(double s_x, double s_y, double s_z,
605 vgl_vector_3d<double> c_x,
606 vgl_vector_3d<double> c_y,
607 vgl_vector_3d<double> c_z,
608 double t_x, double t_y, double t_z)
609 {
610 form_=Affine;
611
612 // Set rotation matrix
613 xx_ = c_x.x(); xy_ = c_y.x(); xz_ = c_z.x();
614 yx_ = c_x.y(); yy_ = c_y.y(); yz_ = c_z.y();
615 zx_ = c_x.z(); zy_ = c_y.z(); zz_ = c_z.z();
616
617
618 // Account for scaling (this actually means that scaling was done BEFORE rotation)
619 xx_*=s_x; xy_*=s_y; xz_*=s_z;
620 yx_*=s_x; yy_*=s_y; yz_*=s_z;
621 zx_*=s_x; zy_*=s_y; zz_*=s_z;
622
623 // Set translation (first 3 elements of final column)
624 xt_=t_x;
625 yt_=t_y;
626 zt_=t_z;
627
628 // Set all other elements to defaults
629 tx_=0; ty_=0; tz_=0; tt_=1;
630
631 inv_uptodate_=false;
632 }
633
634 //=======================================================================
635
set_affine(const vgl_point_3d<double> & p,const vgl_vector_3d<double> & u,const vgl_vector_3d<double> & v,const vgl_vector_3d<double> & w)636 void vimt3d_transform_3d::set_affine(const vgl_point_3d<double>& p,
637 const vgl_vector_3d<double>& u,
638 const vgl_vector_3d<double>& v,
639 const vgl_vector_3d<double>& w)
640 {
641 form_=Affine;
642
643 #ifndef NDEBUG
644 const double tol=1e-6;
645
646 // Get normalized vectors
647 vgl_vector_3d<double> uh = normalized(u);
648 vgl_vector_3d<double> vh = normalized(v);
649 vgl_vector_3d<double> wh = normalized(w);
650
651 // Test for orthogonality of input vectors
652 assert(std::fabs(dot_product(uh,vh))<tol);
653 assert(std::fabs(dot_product(vh,wh))<tol);
654 assert(std::fabs(dot_product(wh,uh))<tol);
655
656 // Test for right-handedness of input vectors
657 assert(std::fabs((cross_product(uh,vh)-wh).length())<tol);
658 #endif
659
660 // Set rotation and scaling (this actually means that scaling was done BEFORE rotation)
661 xx_=u.x(); xy_=v.x(); xz_=w.x();
662 yx_=u.y(); yy_=v.y(); yz_=w.y();
663 zx_=u.z(); zy_=v.z(); zz_=w.z();
664
665 // Set translation (first 3 elements of final column)
666 xt_=p.x();
667 yt_=p.y();
668 zt_=p.z();
669
670 // Set final row with default values (for all transforms up to affine)
671 tx_=0; ty_=0; tz_=0; tt_=1;
672
673 inv_uptodate_=false;
674 }
675
676 //=======================================================================
677
inverse() const678 vimt3d_transform_3d vimt3d_transform_3d::inverse() const
679 {
680 if (!inv_uptodate_) calcInverse();
681
682 vimt3d_transform_3d inv;
683
684 inv.xx_ = xx2_; inv.xy_ = xy2_; inv.xz_ = xz2_; inv.xt_ = xt2_;
685 inv.yx_ = yx2_; inv.yy_ = yy2_; inv.yz_ = yz2_; inv.yt_ = yt2_;
686 inv.zx_ = zx2_; inv.zy_ = zy2_; inv.zz_ = zz2_; inv.zt_ = zt2_;
687 inv.tx_ = tx2_; inv.ty_ = ty2_; inv.tz_ = tz2_; inv.tt_ = tt2_;
688
689 inv.xx2_ = xx_; inv.xy2_ = xy_; inv.xz2_ = xz_; inv.xt2_ = xt_;
690 inv.yx2_ = yx_; inv.yy2_ = yy_; inv.yz2_ = yz_; inv.yt2_ = yt_;
691 inv.zx2_ = zx_; inv.zy2_ = zy_; inv.zz2_ = zz_; inv.zt2_ = zt_;
692 inv.tx2_ = tx_; inv.ty2_ = ty_; inv.tz2_ = tz_; inv.tt2_ = tt_;
693
694 inv.form_ = form_;
695 inv.inv_uptodate_ = true;
696
697 return inv;
698 }
699
700 //=======================================================================
701
calcInverse() const702 void vimt3d_transform_3d::calcInverse() const
703 {
704 xx2_=yy2_=zz2_=tt2_=1;
705 xy2_ = xz2_= xt2_ = yx2_ = yz2_= yt2_ = zx2_ = zy2_ = zt2_ = tx2_ = ty2_ = tz2_ = 0;
706
707 switch (form_)
708 {
709 case Identity :
710 break;
711 case Translation :
712 xt2_=-xt_;
713 yt2_=-yt_;
714 zt2_=-zt_;
715 break;
716 case ZoomOnly :
717 xx2_=1.0/xx_;
718 yy2_=1.0/yy_;
719 zz2_=1.0/zz_;
720 xt2_=-xt_/xx_;
721 yt2_=-yt_/yy_;
722 zt2_=-zt_/zz_;
723 break;
724 case RigidBody :
725 // transpose x,y,z part
726 xx2_=xx_;
727 xy2_=yx_;
728 xz2_=zx_;
729 yx2_=xy_;
730 yy2_=yy_;
731 yz2_=zy_;
732 zx2_=xz_;
733 zy2_=yz_;
734 zz2_=zz_;
735 xt2_=-(xx2_*xt_ + xy2_*yt_ + xz2_*zt_);
736 yt2_=-(yx2_*xt_ + yy2_*yt_ + yz2_*zt_);
737 zt2_=-(zx2_*xt_ + zy2_*yt_ + zz2_*zt_);
738 break;
739 case Similarity :
740 case Affine : {
741 // affine inverse (plugged in from symbolic matlab)
742 double det=-xx_*yy_*zz_+xx_*zy_*yz_+yx_*xy_*zz_-yx_*zy_*xz_-zx_*xy_*yz_+zx_*yy_*xz_;
743 if (det==0)
744 {
745 std::cerr<<"vimt3d_transform_3d() : No inverse exists for this affine transform (det==0)\n";
746 std::abort();
747 }
748
749 xx2_=(-yy_*zz_+zy_*yz_)/det;
750 xy2_=( xy_*zz_-zy_*xz_)/det;
751 xz2_=(-xy_*yz_+yy_*xz_)/det;
752 xt2_=(xy_*yz_*zt_-xy_*yt_*zz_-yy_*xz_*zt_+yy_*xt_*zz_+zy_*xz_*yt_-zy_*xt_*yz_)/det;
753
754 yx2_=( yx_*zz_-zx_*yz_)/det;
755 yy2_=(-xx_*zz_+zx_*xz_)/det;
756 yz2_=( xx_*yz_-yx_*xz_)/det;
757 yt2_=(-xx_*yz_*zt_+xx_*yt_*zz_+yx_*xz_*zt_-yx_*xt_*zz_-zx_*xz_*yt_+zx_*xt_*yz_)/det;
758
759 zx2_=(-yx_*zy_+zx_*yy_)/det;
760 zy2_=( xx_*zy_-zx_*xy_)/det;
761 zz2_=(-xx_*yy_+yx_*xy_)/det;
762 zt2_=( xx_*yy_*zt_-xx_*yt_*zy_-yx_*xy_*zt_+yx_*xt_*zy_+zx_*xy_*yt_-zx_*xt_*yy_)/det;
763
764 break; }
765 default:
766 mbl_exception_error(mbl_exception_abort(
767 vul_sprintf("vimt3d_transform_3d::calcInverse() Unexpected form: %d", form_) ));
768 }
769
770 inv_uptodate_=true;
771 }
772
773 //=======================================================================
774
operator ==(const vimt3d_transform_3d & t) const775 bool vimt3d_transform_3d::operator==( const vimt3d_transform_3d& t) const
776 {
777 return
778 xx_ == t.xx_ &&
779 yy_ == t.yy_ &&
780 zz_ == t.zz_ &&
781 tt_ == t.tt_ &&
782 xy_ == t.xy_ &&
783 xz_ == t.xz_ &&
784 xt_ == t.xt_ &&
785 yx_ == t.yx_ &&
786 yz_ == t.yz_ &&
787 yt_ == t.yt_ &&
788 zx_ == t.zx_ &&
789 zy_ == t.zy_ &&
790 zt_ == t.zt_ &&
791 tx_ == t.tx_ &&
792 ty_ == t.ty_ &&
793 tz_ == t.tz_ ;
794 }
795
796 //=======================================================================
797 //: Calculates the product LR
798 // \param L Transform
799 // \param R Transform
800 // \return Transform LR = R followed by L
operator *(const vimt3d_transform_3d & L,const vimt3d_transform_3d & R)801 vimt3d_transform_3d operator*(const vimt3d_transform_3d& L, const vimt3d_transform_3d& R)
802 {
803 vimt3d_transform_3d T;
804
805 if (L.form() == vimt3d_transform_3d::Identity)
806 return R;
807 else
808 if (R.form() == vimt3d_transform_3d::Identity)
809 return L;
810 else
811 {
812 /// full multiplication - inefficient but works for
813 // arbitrary 4*4 transformation matrix
814 T.xx_ = L.xx_*R.xx_ + L.xy_*R.yx_+ L.xz_*R.zx_ + L.xt_*R.tx_;
815 T.xy_ = L.xx_*R.xy_ + L.xy_*R.yy_+ L.xz_*R.zy_ + L.xt_*R.ty_;
816 T.xz_ = L.xx_*R.xz_ + L.xy_*R.yz_+ L.xz_*R.zz_ + L.xt_*R.tz_;
817 T.xt_ = L.xx_*R.xt_ + L.xy_*R.yt_+ L.xz_*R.zt_ + L.xt_*R.tt_;
818
819 T.yx_ = L.yx_*R.xx_ + L.yy_*R.yx_+ L.yz_*R.zx_ + L.yt_*R.tx_;
820 T.yy_ = L.yx_*R.xy_ + L.yy_*R.yy_+ L.yz_*R.zy_ + L.yt_*R.ty_;
821 T.yz_ = L.yx_*R.xz_ + L.yy_*R.yz_+ L.yz_*R.zz_ + L.yt_*R.tz_;
822 T.yt_ = L.yx_*R.xt_ + L.yy_*R.yt_+ L.yz_*R.zt_ + L.yt_*R.tt_;
823
824 T.zx_ = L.zx_*R.xx_ + L.zy_*R.yx_+ L.zz_*R.zx_ + L.zt_*R.tx_;
825 T.zy_ = L.zx_*R.xy_ + L.zy_*R.yy_+ L.zz_*R.zy_ + L.zt_*R.ty_;
826 T.zz_ = L.zx_*R.xz_ + L.zy_*R.yz_+ L.zz_*R.zz_ + L.zt_*R.tz_;
827 T.zt_ = L.zx_*R.xt_ + L.zy_*R.yt_+ L.zz_*R.zt_ + L.zt_*R.tt_;
828
829 T.tx_ = L.tx_*R.xx_ + L.ty_*R.yx_+ L.tz_*R.zx_ + L.tt_*R.tx_;
830 T.ty_ = L.tx_*R.xy_ + L.ty_*R.yy_+ L.tz_*R.zy_ + L.tt_*R.ty_;
831 T.tz_ = L.tx_*R.xz_ + L.ty_*R.yz_+ L.tz_*R.zz_ + L.tt_*R.tz_;
832 T.tt_ = L.tx_*R.xt_ + L.ty_*R.yt_+ L.tz_*R.zt_ + L.tt_*R.tt_;
833
834 // now set the type using the type of L and R
835 // not sure this right - kds
836 if (R.form() == L.form())
837 T.form_ = R.form();
838 else
839 {
840 if (R.form() == vimt3d_transform_3d::Affine ||
841 L.form() == vimt3d_transform_3d::Affine)
842 T.form_ = vimt3d_transform_3d::Affine;
843 else
844 if (R.form() == vimt3d_transform_3d::Similarity ||
845 L.form() == vimt3d_transform_3d::Similarity)
846 T.form_ = vimt3d_transform_3d::Similarity;
847 else
848 if (R.form() == vimt3d_transform_3d::RigidBody ||
849 L.form() == vimt3d_transform_3d::RigidBody)
850 {
851 if (R.form() == vimt3d_transform_3d::ZoomOnly)
852 {
853 if (R.xx_ == R.yy_ &&
854 R.xx_ == R.zz_)
855 T.form_ = vimt3d_transform_3d::Similarity;
856 else
857 T.form_ = vimt3d_transform_3d::Affine;
858 }
859 else
860 if (L.form() == vimt3d_transform_3d::ZoomOnly)
861 {
862 if (L.xx_ == L.yy_ &&
863 L.xx_ == L.zz_)
864 T.form_ = vimt3d_transform_3d::Similarity;
865 else
866 T.form_ = vimt3d_transform_3d::Affine;
867 }
868 else
869 T.form_ = vimt3d_transform_3d::RigidBody;
870 }
871 else
872 if (R.form() == vimt3d_transform_3d::ZoomOnly ||
873 L.form() == vimt3d_transform_3d::ZoomOnly)
874 T.form_ = vimt3d_transform_3d::ZoomOnly;
875 else
876 T.form_ = vimt3d_transform_3d::Translation;
877 }
878 }
879
880 T.inv_uptodate_ = false;
881
882 return T;
883 }
884
885 //=======================================================================
print_summary(std::ostream & o) const886 void vimt3d_transform_3d::print_summary(std::ostream& o) const
887 {
888 o << vsl_indent()<< "Form: ";
889 vsl_indent_inc(o);
890 switch (form_)
891 {
892 case Identity:
893 o << "Identity\n";
894 break;
895
896 case Translation: {
897 vnl_vector<double> p(3);
898 params(p);
899 o << "Translation (" << p(0) << ',' << p(1) << ',' << p(2) << ")\n";
900 break; }
901
902 case ZoomOnly: {
903 vnl_vector<double> p(6);
904 params(p);
905 o << "ZoomOnly\n"
906 << vsl_indent()<< "scale factor = (" << p(0) << ',' << p(1) << ',' << p(2) << ")\n"
907 << vsl_indent() << "translation = (" << p(3) << ',' << p(4) << ',' << p(5) << ")\n";
908 break; }
909
910 case RigidBody: {
911 vnl_vector<double> p(6);
912 params(p);
913 o << "RigidBody\n"
914 << vsl_indent()<< "angles = " << p(0) << ',' << p(1) << ',' << p(2) << '\n'
915 << vsl_indent()<< "translation = (" << p(3) << ',' << p(4) << ',' << p(5) << ")\n";
916 break; }
917
918 case Similarity: {
919 vnl_vector<double> p(7);
920 params(p);
921 // not sure this is right - kds
922 // returns euler angles of rotation
923 // which might not be same as rotations specified by set command
924 // cos euler angles not unique
925 o << "Similarity\n"
926 << vsl_indent()<< "scale factor = " << p(0) << '\n'
927 << vsl_indent()<< "angles = " << p(1) << ',' << p(2) << ',' << p(3) << '\n'
928 << vsl_indent()<< "translation = (" << p(4) << ',' << p(5) << ',' << p(6) << ")\n";
929 break; }
930 case Affine: {
931 vnl_vector<double> p(9);
932 params(p);
933 // not sure this is right - kds
934 // params(p) call is broken for affine
935 // only works if sx=sy=sz in set command
936 o << "Affine\n"
937 << vsl_indent()<< "scale factors = " << p(0) << ',' << p(1) << ',' << p(2) << '\n'
938 << vsl_indent()<< "angles = " << p(3) << ',' << p(4) << ',' << p(5) << '\n'
939 << vsl_indent()<< "translation = (" << p(6) << ',' << p(7) << ',' << p(8) << ")\n";
940 break; }
941 default:
942 mbl_exception_error(mbl_exception_abort(
943 vul_sprintf("vimt3d_transform_3d::print_summary() Unexpected form: %d", form_) ));
944 }
945 vsl_indent_dec(o);
946 }
947
948 //=======================================================================
949 // Print class to os
print_all(std::ostream & os) const950 void vimt3d_transform_3d::print_all(std::ostream& os) const
951 {
952 os << vsl_indent() << "Form: ";
953 switch (form_)
954 {
955 case Identity:
956 os << "Identity\n";
957 break;
958
959 case Translation:
960 os << "Translation\n";
961 break;
962
963 case ZoomOnly:
964 os << "ZoomOnly\n";
965 break;
966
967 case RigidBody:
968 os << "RigidBody\n";
969 break;
970
971 case Similarity:
972 os << "Similarity\n";
973 break;
974
975 case Affine:
976 os << "Affine\n";
977 break;
978 default:
979 mbl_exception_error(mbl_exception_abort(
980 vul_sprintf("vimt3d_transform_3d::print_all() Unexpected form: %d", form_) ));
981 }
982
983 os << vsl_indent() << "Matrix:\n";
984 vsl_indent_inc(os);
985 os << vsl_indent() << xx_ << ' ' << xy_ << ' ' << xz_ << ' ' << xt_ << '\n'
986 << vsl_indent() << yx_ << ' ' << yy_ << ' ' << yz_ << ' ' << yt_ << '\n'
987 << vsl_indent() << zx_ << ' ' << zy_ << ' ' << zz_ << ' ' << zt_ << '\n'
988 << vsl_indent() << tx_ << ' ' << ty_ << ' ' << tz_ << ' ' << tt_ << '\n';
989 vsl_indent_dec(os);
990 }
991
992
993 //=======================================================================
config(std::istream & is)994 void vimt3d_transform_3d::config(std::istream& is)
995 {
996 mbl_read_props_type props = mbl_read_props_ws(is);
997 std::string form = props.get_required_property("form");
998 vul_string_downcase(form);
999
1000 bool done=false;
1001
1002 if (form == "identity")
1003 {
1004 done=true;
1005 form_ = Identity;
1006 }
1007 else if (form == "translation")
1008 form_ = Translation;
1009 else if (form == "zoomonly")
1010 form_ = ZoomOnly;
1011 else if (form == "rigidbody")
1012 form_ = RigidBody;
1013 else if (form == "similarity")
1014 form_ = Similarity;
1015 else if (form == "affine")
1016 form_ = Affine;
1017 else
1018 throw mbl_exception_parse_error("Unknown transformation: \"" + form + "\"");
1019
1020 std::string vector = props.get_optional_property("vector");
1021 if (!vector.empty())
1022 {
1023 std::istringstream ss(vector);
1024
1025 std::vector<double> vec1;
1026 mbl_parse_sequence(ss, std::back_inserter(vec1), double());
1027 if (vec1.empty())
1028 throw mbl_exception_parse_error("Could not find elements for transformation vector: \""+vector+"\"");
1029 vnl_vector<double> vec2(&vec1.front(), vec1.size());
1030 try
1031 { // translate exception abort into parse error.
1032 this->set(vec2, form_);
1033 }
1034 catch (mbl_exception_abort & e)
1035 {
1036 throw mbl_exception_parse_error(e.what());
1037 }
1038 done = true;
1039 }
1040 if (!done && form_==Translation)
1041 {
1042 this->set_translation(
1043 vul_string_atof(props.get_required_property("t_x")),
1044 vul_string_atof(props.get_required_property("t_y")),
1045 vul_string_atof(props.get_required_property("t_z")) );
1046 done = true;
1047 }
1048 if (!done && form_==ZoomOnly)
1049 {
1050 std::string s_str = props.get_optional_property("s");
1051 if (!s_str.empty())
1052 this->set_zoom_only(
1053 vul_string_atof(s_str),
1054 vul_string_atof(props.get_optional_property("t_x")),
1055 vul_string_atof(props.get_optional_property("t_y")),
1056 vul_string_atof(props.get_optional_property("t_z")) );
1057 else
1058 this->set_zoom_only(
1059 vul_string_atof(props.get_optional_property("s_x")),
1060 vul_string_atof(props.get_optional_property("s_y")),
1061 vul_string_atof(props.get_optional_property("s_z")),
1062 vul_string_atof(props.get_optional_property("t_x")),
1063 vul_string_atof(props.get_optional_property("t_y")),
1064 vul_string_atof(props.get_optional_property("t_z")) );
1065 done = true;
1066 }
1067 if (!done && form_==RigidBody)
1068 {
1069 set_rigid_body(
1070 vul_string_atof(props.get_optional_property("r_x")),
1071 vul_string_atof(props.get_optional_property("r_y")),
1072 vul_string_atof(props.get_optional_property("r_z")),
1073 vul_string_atof(props.get_optional_property("t_x")),
1074 vul_string_atof(props.get_optional_property("t_y")),
1075 vul_string_atof(props.get_optional_property("t_z")) );
1076 done = true;
1077 }
1078
1079 if (!done) throw mbl_exception_parse_error("Not enough transformation values specified");
1080
1081 mbl_read_props_look_for_unused_props(
1082 "vimt3d_transform_3d::config", props, mbl_read_props_type());
1083 return;
1084 }
1085
1086
1087 //=======================================================================
1088
b_write(vsl_b_ostream & bfs) const1089 void vimt3d_transform_3d::b_write(vsl_b_ostream& bfs) const
1090 {
1091 constexpr short version_no = 2;
1092 vsl_b_write(bfs,version_no);
1093 vsl_b_write(bfs,int(form_));
1094 vsl_b_write(bfs,xx_); vsl_b_write(bfs,xy_); vsl_b_write(bfs,xz_); vsl_b_write(bfs,xt_);
1095 vsl_b_write(bfs,yx_); vsl_b_write(bfs,yy_); vsl_b_write(bfs,yz_); vsl_b_write(bfs,yt_);
1096 vsl_b_write(bfs,zx_); vsl_b_write(bfs,zy_); vsl_b_write(bfs,zz_); vsl_b_write(bfs,zt_);
1097 vsl_b_write(bfs,tx_); vsl_b_write(bfs,ty_); vsl_b_write(bfs,tz_); vsl_b_write(bfs,tt_);
1098 }
1099
1100 //=======================================================================
1101
b_read(vsl_b_istream & bfs)1102 void vimt3d_transform_3d::b_read(vsl_b_istream& bfs)
1103 {
1104 short version;
1105 vsl_b_read(bfs,version);
1106 int f;
1107 switch (version)
1108 {
1109 case 1:
1110 vsl_b_read(bfs,f);
1111 if (f==0) // Old Form enum had "Undefined" as the first value. It is never needed, and so was removed.
1112 set_identity();
1113 else
1114 {
1115 form_=Form(f-1);
1116 vsl_b_read(bfs,xx_); vsl_b_read(bfs,xy_); vsl_b_read(bfs,xz_); vsl_b_read(bfs,xt_);
1117 vsl_b_read(bfs,yx_); vsl_b_read(bfs,yy_); vsl_b_read(bfs,yz_); vsl_b_read(bfs,yt_);
1118 vsl_b_read(bfs,zx_); vsl_b_read(bfs,zy_); vsl_b_read(bfs,zz_); vsl_b_read(bfs,zt_);
1119 vsl_b_read(bfs,tx_); vsl_b_read(bfs,ty_); vsl_b_read(bfs,tz_); vsl_b_read(bfs,tt_);
1120 }
1121 break;
1122 case 2:
1123 vsl_b_read(bfs,f); form_=Form(f);
1124 vsl_b_read(bfs,xx_); vsl_b_read(bfs,xy_); vsl_b_read(bfs,xz_); vsl_b_read(bfs,xt_);
1125 vsl_b_read(bfs,yx_); vsl_b_read(bfs,yy_); vsl_b_read(bfs,yz_); vsl_b_read(bfs,yt_);
1126 vsl_b_read(bfs,zx_); vsl_b_read(bfs,zy_); vsl_b_read(bfs,zz_); vsl_b_read(bfs,zt_);
1127 vsl_b_read(bfs,tx_); vsl_b_read(bfs,ty_); vsl_b_read(bfs,tz_); vsl_b_read(bfs,tt_);
1128 break;
1129 default:
1130 std::cerr<<"vimt3d_transform_3d::load : Illegal version number "<<version<<'\n';
1131 std::abort();
1132 }
1133
1134 inv_uptodate_ = false;
1135 }
1136
1137 //=======================================================================
1138
vsl_b_write(vsl_b_ostream & bfs,const vimt3d_transform_3d & b)1139 void vsl_b_write(vsl_b_ostream& bfs, const vimt3d_transform_3d& b)
1140 {
1141 b.b_write(bfs);
1142 }
1143
1144 //=======================================================================
1145
vsl_b_read(vsl_b_istream & bfs,vimt3d_transform_3d & b)1146 void vsl_b_read(vsl_b_istream& bfs, vimt3d_transform_3d& b)
1147 {
1148 b.b_read(bfs);
1149 }
1150
1151 //=======================================================================
1152
operator <<(std::ostream & os,const vimt3d_transform_3d & b)1153 std::ostream& operator<<(std::ostream& os,const vimt3d_transform_3d& b)
1154 {
1155 vsl_indent_inc(os);
1156 b.print_summary(os);
1157 vsl_indent_dec(os);
1158 return os;
1159 }
1160
1161 //========================================================================
1162 // Test whether a 3D transform is zoom-only or lesser, i.e. there may
1163 // be translation and (anisotropic) scaling but no rotation.
1164 // This tests only for a commonly-occurring special case; there may
1165 // be other zoom-only transforms that are not detected.
1166 //========================================================================
vimt3d_transform_is_zoom_only(const vimt3d_transform_3d & transf,const double zero_tol)1167 bool vimt3d_transform_is_zoom_only(const vimt3d_transform_3d& transf,
1168 const double zero_tol/*=1e-9*/)
1169 {
1170 // Check whether the top-left 3x3 submatrix part of the transform is
1171 // diagonal with strictly-positive elements. Such cases have zero rotation
1172 // and positive (possibly anisotropic) scaling.
1173 vnl_matrix<double> M = transf.matrix().extract(3,3,0,0);
1174
1175 // Are any diagonal elements zero or negative?
1176 for (unsigned i=0; i<3; ++i)
1177 if (M(i,i)<=zero_tol) return false;
1178
1179 // Are any off-diagonal elements non-zero?
1180 for (unsigned j=0; j<3; ++j)
1181 {
1182 for (unsigned i=0; i<3; ++i)
1183 {
1184 if (i!=j && std::fabs(M(i,j))>=zero_tol) return false;
1185 }
1186 }
1187
1188 return true;
1189 }
1190