1 /*=========================================================================
2  *
3  *  Copyright Insight Software Consortium
4  *
5  *  Licensed under the Apache License, Version 2.0 (the "License");
6  *  you may not use this file except in compliance with the License.
7  *  You may obtain a copy of the License at
8  *
9  *         http://www.apache.org/licenses/LICENSE-2.0.txt
10  *
11  *  Unless required by applicable law or agreed to in writing, software
12  *  distributed under the License is distributed on an "AS IS" BASIS,
13  *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14  *  See the License for the specific language governing permissions and
15  *  limitations under the License.
16  *
17  *=========================================================================*/
18 
19 #include "itkScaleVersor3DTransform.h"
20 #include <iostream>
21 
22 class TransformHelperType : public itk::ScaleVersor3DTransform<double>
23 {
24 public:
25 
26   using Self = TransformHelperType;
27   using Superclass = itk::ScaleVersor3DTransform<double>;
28   using Pointer = itk::SmartPointer<Self>;
29   using ConstPointer = itk::SmartPointer<const Self>;
30 
31   /** New macro for creation of through a Smart Pointer. */
32   itkNewMacro( Self );
33 
34   /** Run-time type information (and related methods). */
35   itkTypeMacro( TransformHelperType, ScaleVersor3DTransform );
36 
TriggerExceptionFromComputeMatrixParameters()37   void TriggerExceptionFromComputeMatrixParameters()
38   {
39     this->ComputeMatrixParameters();
40   }
41 
42 };
43 
itkScaleVersor3DTransformTest(int,char * [])44 int itkScaleVersor3DTransformTest(int, char * [] )
45 {
46   using ValueType = double;
47 
48   const ValueType epsilon = 1e-12;
49 
50   using TransformType = itk::ScaleVersor3DTransform<ValueType>;
51   using VersorType = TransformType::VersorType;
52   using VectorType = TransformType::InputVectorType;
53   using ParametersType = TransformType::ParametersType;
54   using JacobianType = TransformType::JacobianType;
55   using MatrixType = TransformType::MatrixType;
56 
57     {
58     std::cout << "Test default constructor... ";
59 
60     TransformType::Pointer transform = TransformType::New();
61 
62     std::cout << transform->GetNameOfClass() << std::endl;
63 
64     transform->Print( std::cout );
65 
66     VectorType axis(1.5);
67 
68     ValueType angle = 120.0 * std::atan(1.0) / 45.0;
69 
70     VersorType versor;
71     versor.Set( axis, angle );
72 
73     ParametersType parameters( transform->GetNumberOfParameters() ); // Number of parameters
74 
75     parameters[0] = versor.GetX();
76     parameters[1] = versor.GetY();
77     parameters[2] = versor.GetZ();
78     parameters[3] = 0.0;             // Translation
79     parameters[4] = 0.0;
80     parameters[5] = 0.0;
81     parameters[6] = 1.0;
82     parameters[7] = 1.0;
83     parameters[8] = 1.0;
84 
85     transform->SetParameters( parameters );
86 
87     if( 0.0 > epsilon )
88       {
89       std::cout << "Error ! " << std::endl;
90       return EXIT_FAILURE;
91       }
92 
93     transform->Print( std::cout );
94 
95     std::cout << " PASSED !" << std::endl;
96     }
97 
98     {
99     std::cout << "Test initial rotation matrix " << std::endl;
100     TransformType::Pointer transform = TransformType::New();
101     MatrixType             matrix = transform->GetMatrix();
102     std::cout << "Matrix = " << std::endl;
103     std::cout <<    matrix   << std::endl;
104 
105     MatrixType identity;
106     identity.SetIdentity();
107 
108     try
109       {
110       // SetMatrix is not fully implemented, and it is expected
111       // to throw an exception at this point.
112       transform->SetMatrix( identity );
113       std::cerr << "ERROR: Missed expected exception when calling SetMatrix() " << std::endl;
114       return EXIT_FAILURE;
115       }
116     catch( itk::ExceptionObject & itkNotUsed( excp ) )
117       {
118       std::cerr << "Got Normal expected exception when calling SetMatrix() " << std::endl;
119       }
120 
121     }
122 
123   /* Create a Rigid 3D transform with rotation */
124 
125     {
126     bool Ok = true;
127 
128     TransformType::Pointer rotation = TransformType::New();
129 
130     itk::Vector<double, 3> axis(1);
131 
132     const double angle = (std::atan(1.0) / 45.0) * 120.0; // turn 120 degrees
133 
134     // this rotation will permute the axis x->y, y->z, z->x
135     rotation->SetRotation( axis, angle );
136 
137     TransformType::OffsetType offset = rotation->GetOffset();
138     std::cout << "pure Rotation test:  ";
139     std::cout << offset << std::endl;
140     for( unsigned int i = 0; i < 3; i++ )
141       {
142       if( std::fabs( offset[i] - 0.0 ) > epsilon )
143         {
144         Ok = false;
145         break;
146         }
147       }
148 
149     if( !Ok )
150       {
151       std::cerr << "Get Offset  differs from null in rotation " << std::endl;
152       return EXIT_FAILURE;
153       }
154 
155     VersorType versor;
156     versor.Set( axis, angle );
157 
158       {
159       // Rotate an itk::Point
160       TransformType::InputPointType::ValueType pInit[3] = {1, 4, 9};
161       TransformType::InputPointType            p = pInit;
162       TransformType::OutputPointType           q;
163       q = versor.Transform( p );
164 
165       TransformType::OutputPointType r;
166       r = rotation->TransformPoint( p );
167       for( unsigned int i = 0; i < 3; i++ )
168         {
169         if( std::fabs( q[i] - r[i] ) > epsilon )
170           {
171           Ok = false;
172           break;
173           }
174         }
175       if( !Ok )
176         {
177         std::cerr << "Error rotating point : " << p << std::endl;
178         std::cerr << "Result should be     : " << q << std::endl;
179         std::cerr << "Reported Result is   : " << r << std::endl;
180         return EXIT_FAILURE;
181         }
182       else
183         {
184         std::cout << "Ok rotating an itk::Point " << std::endl;
185         }
186       }
187 
188       {
189       // Translate an itk::Vector
190       TransformType::InputVectorType::ValueType pInit[3] = {1, 4, 9};
191       TransformType::InputVectorType            p = pInit;
192       TransformType::OutputVectorType           q;
193       q = versor.Transform( p );
194 
195       TransformType::OutputVectorType r;
196       r = rotation->TransformVector( p );
197       for( unsigned int i = 0; i < 3; i++ )
198         {
199         if( std::fabs( q[i] - r[i] ) > epsilon )
200           {
201           Ok = false;
202           break;
203           }
204         }
205       if( !Ok )
206         {
207         std::cerr << "Error rotating vector : " << p << std::endl;
208         std::cerr << "Result should be      : " << q << std::endl;
209         std::cerr << "Reported Result is    : " << r << std::endl;
210         return EXIT_FAILURE;
211         }
212       else
213         {
214         std::cout << "Ok rotating an itk::Vector " << std::endl;
215         }
216       }
217 
218       {
219       // Translate an itk::CovariantVector
220       TransformType::InputCovariantVectorType::ValueType pInit[3] = {1, 4, 9};
221       TransformType::InputCovariantVectorType            p = pInit;
222       TransformType::OutputCovariantVectorType           q;
223       q = versor.Transform( p );
224 
225       TransformType::OutputCovariantVectorType r;
226       r = rotation->TransformCovariantVector( p );
227       for( unsigned int i = 0; i < 3; i++ )
228         {
229         if( std::fabs( q[i] - r[i] ) > epsilon )
230           {
231           Ok = false;
232           break;
233           }
234         }
235       if( !Ok )
236         {
237         std::cerr << "Error rotating covariant vector : " << p << std::endl;
238         std::cerr << "Result should be                : " << q << std::endl;
239         std::cerr << "Reported Result is              : " << r << std::endl;
240         return EXIT_FAILURE;
241         }
242       else
243         {
244         std::cout << "Ok rotating an itk::CovariantVector " << std::endl;
245         }
246       }
247 
248       {
249       // Translate a vnl_vector
250       TransformType::InputVnlVectorType p;
251       p[0] = 1;
252       p[1] = 4;
253       p[2] = 9;
254 
255       TransformType::OutputVnlVectorType q;
256       q = versor.Transform( p );
257 
258       TransformType::OutputVnlVectorType r;
259       r = rotation->TransformVector( p );
260       for( unsigned int i = 0; i < 3; i++ )
261         {
262         if( std::fabs( q[i] - r[i] ) > epsilon )
263           {
264           Ok = false;
265           break;
266           }
267         }
268       if( !Ok )
269         {
270         std::cerr << "Error rotating vnl_vector : " << p << std::endl;
271         std::cerr << "Result should be          : " << q << std::endl;
272         std::cerr << "Reported Result is        : " << r << std::endl;
273         return EXIT_FAILURE;
274         }
275       else
276         {
277         std::cout << "Ok rotating an vnl_Vector " << std::endl;
278         }
279       }
280 
281     }
282 
283   //  Exercise the SetCenter method
284     {
285     bool Ok = true;
286 
287     TransformType::Pointer transform = TransformType::New();
288 
289     itk::Vector<double, 3> axis(1);
290 
291     const double angle = (std::atan(1.0) / 45.0) * 30.0; // turn 30 degrees
292 
293     transform->SetRotation( axis, angle );
294 
295     TransformType::InputPointType center;
296     center[0] = 31;
297     center[1] = 62;
298     center[2] = 93;
299 
300     transform->SetCenter( center );
301 
302     TransformType::OutputPointType transformedPoint;
303     transformedPoint = transform->TransformPoint( center );
304     for( unsigned int i = 0; i < 3; i++ )
305       {
306       if( std::fabs( center[i] - transformedPoint[i] ) > epsilon )
307         {
308         Ok = false;
309         break;
310         }
311       }
312 
313     if( !Ok )
314       {
315       std::cerr << "The center point was not invariant to rotation " << std::endl;
316       return EXIT_FAILURE;
317       }
318     else
319       {
320       std::cout << "Ok center is invariant to rotation." << std::endl;
321       }
322 
323     const unsigned int np = transform->GetNumberOfParameters();
324 
325     ParametersType parameters( np ); // Number of parameters
326     parameters.Fill( 0.0 );
327 
328     VersorType versor;
329 
330     parameters[0] = versor.GetX();   // Rotation axis * sin(t/2)
331     parameters[1] = versor.GetY();
332     parameters[2] = versor.GetZ();
333     parameters[3] = 8.0;             // Translation
334     parameters[4] = 7.0;
335     parameters[5] = 6.0;
336     parameters[6] = 1.0;             // Scale
337     parameters[7] = 1.0;
338     parameters[8] = 1.0;
339 
340     transform->SetParameters( parameters );
341 
342     ParametersType parameters2 = transform->GetParameters();
343 
344     const double tolerance = 1e-8;
345     for( unsigned int p = 0; p < np; p++ )
346       {
347       if( std::fabs( parameters[p] - parameters2[p] ) > tolerance )
348         {
349         std::cerr << "Output parameter does not match input " << std::endl;
350         return EXIT_FAILURE;
351         }
352       }
353     std::cout << "Input/Output parameter check Passed !"  << std::endl;
354 
355     // Try the ComputeJacobianWithRespectToParameters method
356     TransformType::InputPointType aPoint;
357     aPoint[0] = 10.0;
358     aPoint[1] = 20.0;
359     aPoint[2] = -10.0;
360     JacobianType jacobian;
361     transform->ComputeJacobianWithRespectToParameters( aPoint, jacobian );
362     std::cout << "Jacobian: "  << std::endl;
363     std::cout << jacobian << std::endl;
364 
365     // copy the read one just for getting the right matrix size
366     JacobianType TheoreticalJacobian = jacobian;
367 
368     TheoreticalJacobian[0][0] =    0.0;
369     TheoreticalJacobian[1][0] =  206.0;
370     TheoreticalJacobian[2][0] =  -84.0;
371 
372     TheoreticalJacobian[0][1] = -206.0;
373     TheoreticalJacobian[1][1] =    0.0;
374     TheoreticalJacobian[2][1] =   42.0;
375 
376     TheoreticalJacobian[0][2] =   84.0;
377     TheoreticalJacobian[1][2] =  -42.0;
378     TheoreticalJacobian[2][2] =    0.0;
379 
380     TheoreticalJacobian[0][3] = 1.0;
381     TheoreticalJacobian[1][3] = 0.0;
382     TheoreticalJacobian[2][3] = 0.0;
383 
384     TheoreticalJacobian[0][4] = 0.0;
385     TheoreticalJacobian[1][4] = 1.0;
386     TheoreticalJacobian[2][4] = 0.0;
387 
388     TheoreticalJacobian[0][5] = 0.0;
389     TheoreticalJacobian[1][5] = 0.0;
390     TheoreticalJacobian[2][5] = 1.0;
391 
392     TheoreticalJacobian[0][6] =  -21.0;
393     TheoreticalJacobian[1][6] =    0.0;
394     TheoreticalJacobian[2][6] =    0.0;
395 
396     TheoreticalJacobian[0][7] =    0.0;
397     TheoreticalJacobian[1][7] =  -42.0;
398     TheoreticalJacobian[2][7] =    0.0;
399 
400     TheoreticalJacobian[0][8] =    0.0;
401     TheoreticalJacobian[1][8] =    0.0;
402     TheoreticalJacobian[2][8] = -103.0;
403     for( unsigned int ii = 0; ii < 3; ii++ )
404       {
405       for( unsigned int jj = 0; jj < 9; jj++ )
406         {
407         if( itk::Math::abs( TheoreticalJacobian[ii][jj] - jacobian[ii][jj] ) > 1e-5 )
408           {
409           std::cerr << "Jacobian components differ from expected values ";
410           std::cerr << std::endl << std::endl;
411           std::cerr << "Expected Jacobian = " << std::endl;
412           std::cerr << TheoreticalJacobian << std::endl << std::endl;
413           std::cerr << "Computed Jacobian = " << std::endl;
414           std::cerr << jacobian << std::endl << std::endl;
415           std::cerr << std::endl << "Test FAILED ! " << std::endl;
416           return EXIT_FAILURE;
417           }
418         }
419       }
420     }
421 
422     {
423     std::cout << " Exercise the SetIdentity() method " << std::endl;
424     TransformType::Pointer transform = TransformType::New();
425 
426     itk::Vector<double, 3> axis(1);
427 
428     const double angle = (std::atan(1.0) / 45.0) * 30.0; // turn 30 degrees
429 
430     transform->SetRotation( axis, angle );
431 
432     TransformType::InputPointType center;
433     center[0] = 31;
434     center[1] = 62;
435     center[2] = 93;
436 
437     transform->SetCenter( center );
438 
439     transform->SetIdentity();
440 
441     const unsigned int np = transform->GetNumberOfParameters();
442 
443     ParametersType parameters( np ); // Number of parameters
444 
445     VersorType versor;
446 
447     parameters[0]  = versor.GetX();   // Rotation axis * sin(t/2)
448     parameters[1]  = versor.GetY();
449     parameters[2]  = versor.GetZ();
450     parameters[3]  = 0.0;             // Translation
451     parameters[4]  = 0.0;
452     parameters[5]  = 0.0;
453     parameters[6]  = 1.0;             // Scale
454     parameters[7]  = 1.0;
455     parameters[8]  = 1.0;
456 
457     ParametersType parameters2 = transform->GetParameters();
458 
459     const double tolerance = 1e-8;
460     for( unsigned int p = 0; p < np; p++ )
461       {
462       if( std::fabs( parameters[p] - parameters2[p] ) > tolerance )
463         {
464         std::cerr << "Output parameter does not match input " << std::endl;
465         return EXIT_FAILURE;
466         }
467       }
468     std::cout << "Input/Output parameter check Passed !"  << std::endl;
469     }
470 
471     {
472     std::cout << " Exercise the Scaling methods " << std::endl;
473     TransformType::Pointer transform = TransformType::New();
474 
475     itk::Vector<double, 3> axis(1);
476 
477     const double angle = (std::atan(1.0) / 45.0) * 30.0; // turn 30 degrees
478 
479     transform->SetRotation( axis, angle );
480 
481     TransformType::InputPointType center;
482     center[0] = 31;
483     center[1] = 62;
484     center[2] = 93;
485 
486     transform->SetCenter( center );
487 
488     TransformType::OutputVectorType translation;
489     translation[0] = 17;
490     translation[1] = 19;
491     translation[2] = 23;
492 
493     transform->SetTranslation( translation );
494 
495     TransformType::ScaleVectorType scale;
496     scale.Fill( 2.5 );
497 
498     transform->SetScale( scale );
499 
500     TransformType::ScaleVectorType rscale = transform->GetScale();
501 
502     const double tolerance = 1e-8;
503     for( unsigned int j = 0; j < 3; j++ )
504       {
505       if( std::fabs( rscale[j] - scale[j] ) > tolerance )
506         {
507         std::cerr << "Error in Set/Get Scale() " << std::endl;
508         std::cerr << "Input scale: " << scale << std::endl;
509         std::cerr << "Output scale: " << rscale << std::endl;
510         return EXIT_FAILURE;
511         }
512       }
513 
514     const unsigned int np = transform->GetNumberOfParameters();
515 
516     ParametersType parameters( np ); // Number of parameters
517 
518     VersorType versor;
519     versor.Set( axis, angle );
520 
521     parameters.Fill( 0.0 );
522     parameters[0] = versor.GetX();   // Rotation axis * sin(t/2)
523     parameters[1] = versor.GetY();
524     parameters[2] = versor.GetZ();
525     parameters[3] = translation[0];
526     parameters[4] = translation[1];
527     parameters[5] = translation[2];
528     parameters[6] = scale[0];
529     parameters[7] = scale[1];
530     parameters[8] = scale[2];
531 
532     ParametersType parameters2 = transform->GetParameters();
533     for( unsigned int p = 0; p < np; p++ )
534       {
535       if( std::fabs( parameters[p] - parameters2[p] ) > tolerance )
536         {
537         std::cerr << "Output parameter does not match input " << std::endl;
538         return EXIT_FAILURE;
539         }
540       }
541     std::cout << "Input/Output parameter check Passed !"  << std::endl;
542     }
543 
544     { // Exercise exceptions
545     std::cout << " Exercise Exceptions " << std::endl;
546 
547     TransformHelperType::Pointer transform = TransformHelperType::New();
548 
549     // At this point the method ComputeMatrixParameters() is expected to throw
550     // an exception.
551     try
552       {
553       transform->TriggerExceptionFromComputeMatrixParameters();
554       std::cerr << "ERROR: Missed an expected exceptions from ComputeMatrixParameters() " << std::endl;
555       return EXIT_FAILURE;
556       }
557     catch( itk::ExceptionObject & itkNotUsed( excp ) )
558       {
559       std::cout << "Got Correct expected exception from ComputeMatrixParameters() " << std::endl;
560       }
561     }
562 
563     {
564     std::cout << "Exercise SetParameters with Versor norm > 1.0 - epsilon" << std::endl;
565 
566     TransformType::Pointer transform = TransformType::New();
567 
568     const unsigned int np = transform->GetNumberOfParameters();
569 
570     ParametersType parameters( np ); // Number of parameters
571     parameters.Fill( 0.0 );
572 
573     parameters[0] = 1.0;  // Set the Versor to norm 1.0
574     parameters[1] = 0.0;
575     parameters[2] = 0.0;
576 
577     transform->SetParameters( parameters );
578 #if 0 //TODO: Need to instrument inverse of ScaleVersor3DTransform
579       {
580       TransformType::Pointer tInverse = TransformType::New();
581       if(!transform->GetInverse(tInverse))
582         {
583         std::cout << "Cannot create inverse transform" << std::endl;
584         return EXIT_FAILURE;
585         }
586       std::cout << "translation: " << transform;
587       std::cout << "translationInverse: " << tInverse;
588       }
589 #endif
590     }
591 
592   std::cout << std::endl << "Test PASSED ! " << std::endl;
593 
594   return EXIT_SUCCESS;
595 }
596