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 <iostream>
20 
21 #include "itkAffineTransform.h"
22 #include "itkCompositeTransform.h"
23 #include "itkTranslationTransform.h"
24 #include "itkMath.h"
25 
26 namespace
27 {
28 
29 const double epsilon = 1e-10;
30 
31 template <typename TPoint>
testPoint(const TPoint & p1,const TPoint & p2)32 bool testPoint( const TPoint & p1, const TPoint & p2 )
33 {
34   bool pass = true;
35 
36   for( unsigned int i = 0; i < TPoint::PointDimension; i++ )
37     {
38     if( std::fabs( p1[i] - p2[i] ) > epsilon )
39       {
40       pass = false;
41       }
42     }
43   return pass;
44 }
45 
46 template <typename TMatrix>
testMatrix(const TMatrix & m1,const TMatrix & m2)47 bool testMatrix( const TMatrix & m1, const TMatrix & m2 )
48 {
49   unsigned int i, j;
50   bool         pass = true;
51 
52   for( i = 0; i < TMatrix::RowDimensions; i++ )
53     {
54     for( j = 0; j < TMatrix::ColumnDimensions; j++ )
55       {
56       if( std::fabs( m1[i][j] - m2[i][j] ) > epsilon )
57         {
58         pass = false;
59         }
60       }
61     }
62   return pass;
63 }
64 
65 template <typename TArray2D>
testJacobian(const TArray2D & m1,const TArray2D & m2)66 bool testJacobian( const TArray2D & m1, const TArray2D & m2 )
67 {
68   unsigned int i, j;
69   bool         pass = true;
70 
71   for( i = 0; i < m1.rows(); i++ )
72     {
73     for( j = 0; j < m1.cols(); j++ )
74       {
75       if( std::fabs( m1[i][j] - m2[i][j] ) > epsilon )
76         {
77         pass = false;
78         }
79       }
80     }
81   return pass;
82 }
83 
84 template <typename TVector>
testVectorArray(const TVector & v1,const TVector & v2)85 bool testVectorArray( const TVector & v1, const TVector & v2 )
86 {
87   bool pass = true;
88 
89   for( unsigned int i = 0; i < v1.Size(); i++ )
90     {
91     if( std::fabs( v1[i] - v2[i] ) > epsilon )
92       {
93       pass = false;
94       }
95     }
96   return pass;
97 }
98 
99 } // namespace
100 
101 /******/
102 
itkCompositeTransformTest(int,char * [])103 int itkCompositeTransformTest(int, char *[] )
104 {
105   constexpr unsigned int NDimensions = 2;
106 
107   /* Create composite transform */
108   using CompositeType = itk::CompositeTransform<double, NDimensions>;
109   using ScalarType = CompositeType::ScalarType;
110 
111   CompositeType::Pointer compositeTransform = CompositeType::New();
112 
113   /* Test obects */
114   using Matrix2Type = itk::Matrix<ScalarType, NDimensions, NDimensions>;
115   using Vector2Type = itk::Vector<ScalarType, NDimensions>;
116 
117   /* Test that we have an empty the queue */
118   if( compositeTransform->GetNumberOfTransforms() != 0 )
119     {
120     std::cout << "Failed. Expected GetNumberOfTransforms to return 0." << std::endl;
121     return EXIT_FAILURE;
122     }
123 
124   /* Add an affine transform */
125   using AffineType = itk::AffineTransform<ScalarType, NDimensions>;
126   AffineType::Pointer affine = AffineType::New();
127   Matrix2Type         matrix2;
128   Vector2Type         vector2;
129   matrix2[0][0] = 1;
130   matrix2[0][1] = 2;
131   matrix2[1][0] = 3;
132   matrix2[1][1] = 4;
133   vector2[0] = 5;
134   vector2[1] = 6;
135   affine->SetMatrix(matrix2);
136   affine->SetOffset(vector2);
137 
138   compositeTransform->AddTransform( affine );
139 
140   /* Test that we have one transform in the queue */
141   if( compositeTransform->GetNumberOfTransforms() != 1 )
142     {
143     std::cout << "Failed adding transform to queue." << std::endl;
144     return EXIT_FAILURE;
145     }
146 
147   //std::cout << "Composite Transform:" << std::endl << compositeTransform;
148 
149   /* Retrieve the transform and check that it's the same */
150   std::cout << "Retrieve 1st transform." << std::endl;
151   AffineType::ConstPointer affineGet = dynamic_cast<AffineType const *>( compositeTransform->GetNthTransformConstPointer(0) );
152   if( affineGet.IsNull() )
153     {
154     std::cout << "Failed retrieving transform from queue." << std::endl;
155     return EXIT_FAILURE;
156     }
157 
158   std::cout << "Retrieve matrix and offset. " << std::endl;
159   Matrix2Type matrix2Get = affineGet->GetMatrix();
160   Vector2Type vector2Get = affineGet->GetOffset();
161   if( !testMatrix(matrix2, matrix2Get) || !testVectorArray(vector2, vector2Get ) )
162     {
163     std::cout << "Failed retrieving correct transform data." << std::endl;
164     return EXIT_FAILURE;
165     }
166 
167   /* Get parameters with single transform.
168    * Should be same as GetParameters from affine transform. */
169   std::cout << "Get Parameters: " << std::endl;
170   CompositeType::ParametersType parametersTest, parametersTruth;
171   parametersTest = compositeTransform->GetParameters();
172   parametersTruth = affine->GetParameters();
173   std::cout << "affine parametersTruth: " << std::endl << parametersTruth
174             << std::endl
175             << "parametersTest from Composite: " << std::endl << parametersTest
176             << std::endl;
177 
178   if( !testVectorArray( parametersTest, parametersTruth ) )
179     {
180     std::cout << "Failed GetParameters() for single transform." << std::endl;
181     return EXIT_FAILURE;
182     }
183 
184   /* Set parameters with single transform. */
185   CompositeType::ParametersType parametersNew(6), parametersReturned;
186   parametersNew[0] = 0;
187   parametersNew[1] = 10;
188   parametersNew[2] = 20;
189   parametersNew[3] = 30;
190   parametersNew[4] = 40;
191   parametersNew[5] = 50;
192   std::cout << "Set Parameters: " << std::endl;
193   compositeTransform->SetParameters( parametersNew );
194   std::cout << "retrieving... " << std::endl;
195   parametersReturned = compositeTransform->GetParameters();
196   std::cout << "parametersNew: " << std::endl << parametersNew << std::endl
197             << "parametersReturned: " << std::endl << parametersReturned
198             << std::endl;
199   if( !testVectorArray( parametersNew, parametersReturned ) )
200     {
201     std::cout << "Failed SetParameters() for single transform." << std::endl;
202     return EXIT_FAILURE;
203     }
204 
205   /* Test fixed parameters set/get */
206   parametersTest = compositeTransform->GetFixedParameters();
207   parametersTruth = affine->GetFixedParameters();
208   std::cout << "Get Fixed Parameters: " << std::endl
209             << "affine parametersTruth: " << std::endl << parametersTruth
210             << std::endl
211             << "parametersTest from Composite: " << std::endl << parametersTest
212             << std::endl;
213 
214   if( !testVectorArray( parametersTest, parametersTruth ) )
215     {
216     std::cout << "Failed GetFixedParameters() for single transform." << std::endl;
217     return EXIT_FAILURE;
218     }
219 
220   parametersNew.SetSize( parametersTruth.Size() );
221   parametersNew.Fill(1);
222   parametersNew[0] = 42;
223 
224   std::cout << "Set Fixed Parameters: " << std::endl;
225   compositeTransform->SetFixedParameters( parametersNew );
226   std::cout << "retrieving... " << std::endl;
227   parametersReturned = compositeTransform->GetFixedParameters();
228   std::cout << "parametersNew: " << std::endl << parametersNew << std::endl
229             << "parametersReturned: " << std::endl << parametersReturned
230             << std::endl;
231   if( !testVectorArray( parametersNew, parametersReturned ) )
232     {
233     std::cout << "Failed SetFixedParameters() for single transform." << std::endl;
234     return EXIT_FAILURE;
235     }
236 
237   /* Reset affine transform to original values */
238   compositeTransform->ClearTransformQueue();
239 
240   affine = AffineType::New();
241   affine->SetMatrix(matrix2);
242   affine->SetOffset(vector2);
243   compositeTransform->AddTransform( affine );
244 
245   /* Setup test point and truth value for tests */
246   CompositeType::InputPointType  inputPoint;
247   CompositeType::OutputPointType outputPoint, affineTruth;
248   inputPoint[0] = 2;
249   inputPoint[1] = 3;
250   affineTruth[0] = 13;
251   affineTruth[1] = 24;
252 
253   CompositeType::InputVectorType inputVector;
254   CompositeType::OutputVectorType outputVector;
255   inputVector[0] = 0.4;
256   inputVector[1] = 0.6;
257 
258   CompositeType::InputCovariantVectorType inputCVector;
259   CompositeType::OutputCovariantVectorType outputCVector;
260   inputCVector[0] = 0.4;
261   inputCVector[1] = 0.6;
262 
263   /* Test transforming the point with just the single affine transform */
264   outputPoint = compositeTransform->TransformPoint( inputPoint );
265   if( !testPoint( outputPoint, affineTruth) )
266     {
267     std::cout << "Failed transforming point with single transform."
268               << std::endl;
269     return EXIT_FAILURE;
270     }
271 
272   /* Test inverse */
273   CompositeType::Pointer         inverseTransform = CompositeType::New();
274   CompositeType::OutputPointType inverseTruth, inverseOutput;
275   if( !compositeTransform->GetInverse( inverseTransform ) )
276     {
277     std::cout << "ERROR: GetInverse() failed." << std::endl;
278     return EXIT_FAILURE;
279     }
280   inverseTruth = inputPoint;
281   inverseOutput = inverseTransform->TransformPoint( affineTruth );
282   std::cout << "Transform point with inverse composite transform: "
283             << std::endl
284             << "  Test point: " << affineTruth << std::endl
285             << "  Truth: " << inverseTruth << std::endl
286             << "  Output: " << inverseOutput << std::endl;
287   if( !testPoint( inverseOutput, inverseTruth ) )
288     {
289     std::cout << "Failed transform point with inverse composite transform (1)."
290               << std::endl;
291     return EXIT_FAILURE;
292     }
293 
294   /* Test ComputeJacobianWithRespectToParameters */
295 
296   CompositeType::JacobianType   jacComposite, jacSingle;
297   CompositeType::InputPointType jacPoint;
298   jacPoint[0] = 1;
299   jacPoint[1] = 2;
300   affine->ComputeJacobianWithRespectToParameters( jacPoint, jacSingle );
301   std::cout << "Single jacobian:" << std::endl << jacSingle << std::endl;
302   compositeTransform->ComputeJacobianWithRespectToParameters( jacPoint, jacComposite );
303   std::cout << "Composite jacobian:" << std::endl << jacComposite << std::endl;
304   if( !testJacobian( jacComposite, jacSingle ) )
305     {
306     std::cout << "Failed getting jacobian for single transform." << std::endl;
307     return EXIT_FAILURE;
308     }
309 
310   /*
311    * Create and add 2nd transform
312    */
313   AffineType::Pointer affine2 = AffineType::New();
314   matrix2[0][0] = 11;
315   matrix2[0][1] = 22;
316   matrix2[1][0] = 33;
317   matrix2[1][1] = 44;
318   vector2[0] = 55;
319   vector2[1] = 65;
320   affine2->SetMatrix(matrix2);
321   affine2->SetOffset(vector2);
322 
323   compositeTransform->ClearTransformQueue();
324   compositeTransform->AppendTransform( affine2 );
325   compositeTransform->PrependTransform( affine );
326 
327   std::cout << std::endl << "Two-component Composite Transform:"
328             << std::endl << compositeTransform;
329   std::cout << std::endl << "Transform at queue position 0: "
330             << std::endl << compositeTransform->GetNthTransformConstPointer( 0 );
331 
332   /* Test that we have two tranforms in the queue */
333   if( compositeTransform->GetNumberOfTransforms() != 2 )
334     {
335     std::cout << "Failed adding 2nd transform to queue." << std::endl;
336     return EXIT_FAILURE;
337     }
338 
339   /* Transform a point with both transforms. Remember that transforms
340    * are applied in *reverse* queue order, with most-recently added transform first. */
341   CompositeType::OutputPointType compositeTruth;
342   compositeTruth = affine2->TransformPoint( inputPoint );
343   compositeTruth = affine->TransformPoint( compositeTruth );
344 
345   outputPoint = compositeTransform->TransformPoint( inputPoint );
346   std::cout << "Transform point with two-component composite transform: "
347             << std::endl
348             << "  Test point: " << inputPoint << std::endl
349             << "  Truth: " << compositeTruth << std::endl
350             << "  Output: " << outputPoint << std::endl;
351 
352   if( !testPoint( outputPoint, compositeTruth) )
353     {
354     std::cout << "Failed transforming point with two transforms."
355               << std::endl;
356     return EXIT_FAILURE;
357     }
358 
359   CompositeType::OutputVectorType compositeTruthVector;
360   compositeTruthVector = affine2->TransformVector( inputVector );
361   compositeTruthVector = affine->TransformVector( compositeTruthVector );
362   outputVector = compositeTransform->TransformVector( inputVector );
363   std::cout << "Transform vector with two-component composite transform: "
364             << std::endl
365             << "  Test vector: " << inputVector << std::endl
366             << "  Truth: " << compositeTruthVector << std::endl
367             << "  Output: " << outputVector << std::endl;
368 
369   CompositeType::OutputCovariantVectorType compositeTruthCVector;
370   compositeTruthCVector = affine2->TransformCovariantVector( inputCVector );
371   compositeTruthCVector = affine->TransformCovariantVector( compositeTruthCVector );
372   outputCVector = compositeTransform->TransformCovariantVector( inputCVector );
373   std::cout << "Transform covariant vector with two-component composite transform: "
374             << std::endl
375             << "  Test vector: " << inputCVector << std::endl
376             << "  Truth: " << compositeTruthCVector << std::endl
377             << "  Output: " << outputCVector << std::endl;
378 
379 
380   CompositeType::InputDiffusionTensor3DType inputTensor;
381   CompositeType::OutputDiffusionTensor3DType outputTensor;
382   inputTensor[0] = 3.0;
383   inputTensor[1] = 0.3;
384   inputTensor[2] = 0.2;
385   inputTensor[3] = 2.0;
386   inputTensor[4] = 0.1;
387   inputTensor[5] = 1.0;
388   CompositeType::OutputDiffusionTensor3DType compositeTruthTensor;
389   compositeTruthTensor = affine2->TransformDiffusionTensor3D( inputTensor );
390   compositeTruthTensor = affine->TransformDiffusionTensor3D( compositeTruthTensor );
391   outputTensor = compositeTransform->TransformDiffusionTensor3D( inputTensor );
392   std::cout << "Transform tensor with two-component composite transform: "
393             << std::endl
394             << "  Test tensor: " << inputTensor << std::endl
395             << "  Truth: " << compositeTruthTensor << std::endl
396             << "  Output: " << outputTensor << std::endl;
397 
398   CompositeType::InputSymmetricSecondRankTensorType inputSTensor;
399   CompositeType::OutputSymmetricSecondRankTensorType outputSTensor;
400   inputSTensor(1,0) = 0.5;
401   inputSTensor(0,0) = 3.0;
402   inputSTensor(1,1) = 2.0;
403 
404   CompositeType::OutputSymmetricSecondRankTensorType compositeTruthSTensor;
405   compositeTruthSTensor = affine2->TransformSymmetricSecondRankTensor( inputSTensor );
406   compositeTruthSTensor = affine->TransformSymmetricSecondRankTensor( compositeTruthSTensor );
407   outputSTensor = compositeTransform->TransformSymmetricSecondRankTensor( inputSTensor );
408   std::cout << "Transform tensor with two-component composite transform: "
409             << std::endl
410             << "  Test tensor: " << inputSTensor << std::endl
411             << "  Truth: " << compositeTruthSTensor << std::endl
412             << "  Output: " << outputSTensor << std::endl;
413 
414   /* Test inverse with two transforms, with only one set to optimize. */
415   compositeTransform->SetAllTransformsToOptimize( false );
416   compositeTransform->SetNthTransformToOptimizeOn( 0 );
417   if( !compositeTransform->GetInverse( inverseTransform ) )
418     {
419     std::cout << "Expected GetInverse() to succeed." << std::endl;
420     return EXIT_FAILURE;
421     }
422   std::cout << "Inverse two-component transform: " << inverseTransform;
423 
424   /* Check that optimization flag inverse worked */
425   if( inverseTransform->GetNthTransformToOptimize( 0 ) ||
426       !inverseTransform->GetNthTransformToOptimize( 1 ) )
427     {
428     std::cout << "GetInverse failed for TransformsToOptimize flags."
429               << std::endl;
430     return EXIT_FAILURE;
431     }
432   compositeTransform->SetAllTransformsToOptimizeOn(); // Set back to do all.
433   inverseTransform->SetAllTransformsToOptimizeOn();
434 
435   /* Transform point with inverse */
436   inverseTruth = inputPoint;
437   inverseOutput = inverseTransform->TransformPoint( compositeTruth );
438   std::cout << "Transform point with two-component inverse composite transform: "
439             << std::endl
440             << "  Test point: " << compositeTruth << std::endl
441             << "  Truth: " << inverseTruth << std::endl
442             << "  Output: " << inverseOutput << std::endl;
443   if( !testPoint( inverseOutput, inverseTruth ) )
444     {
445     std::cout << "Failed transform point with two-component inverse composite transform."
446               << std::endl;
447     return EXIT_FAILURE;
448     }
449 
450   /* Get inverse transform again, but using other accessor. */
451   CompositeType::ConstPointer inverseTransform2;
452   std::cout << "Call GetInverseTransform():" << std::endl;
453   inverseTransform2 = dynamic_cast<const CompositeType *>( compositeTransform->GetInverseTransform().GetPointer() );
454   if( !inverseTransform2 )
455     {
456     std::cout << "Failed calling GetInverseTransform()." << std::endl;
457     return EXIT_FAILURE;
458     }
459   std::cout << "Transform point: " << std::endl;
460   inverseOutput = inverseTransform2->TransformPoint( compositeTruth );
461   if( !testPoint( inverseOutput, inverseTruth ) )
462     {
463     std::cout << "Failed transform point with two-component inverse composite transform (2)." << std::endl;
464     return EXIT_FAILURE;
465     }
466 
467   /* Test IsLinear() by calling on each sub transform */
468   std::cout << "Test IsLinear" << std::endl;
469   bool allAreLinear = true;
470   for( unsigned int n = 0; n < compositeTransform->GetNumberOfTransforms(); n++ )
471     {
472     if( !compositeTransform->GetNthTransformConstPointer( n )->IsLinear() )
473       {
474       allAreLinear = false;
475       }
476     }
477   if( compositeTransform->IsLinear() != allAreLinear )
478     {
479     std::cout << "compositeTransform returned unexpected value for IsLinear(). Expected " << allAreLinear << std::endl;
480     return EXIT_FAILURE;
481     }
482 
483   /* Test GetNumberOfParameters */
484   std::cout << "GetNumberOfParameters: " << std::endl;
485   unsigned int affineParamsN = affine->GetNumberOfParameters();
486   unsigned int affine2ParamsN = affine2->GetNumberOfParameters();
487   unsigned int nParameters = compositeTransform->GetNumberOfParameters();
488   std::cout << "Number of parameters: " << nParameters << std::endl;
489   if( nParameters != affineParamsN + affine2ParamsN )
490     {
491     std::cout << "GetNumberOfParameters failed for multi-transform."
492               << std::endl << "Expected " << affineParamsN + affine2ParamsN
493               << std::endl;
494     }
495 
496   /* Get parameters with multi-transform. They're filled from transforms in
497    * same order as transforms are applied, from back of queue to front. */
498   parametersTest = compositeTransform->GetParameters();
499   parametersTruth.SetSize( affine2ParamsN + affineParamsN );
500   /* Fill using different method than is used in the class.
501      Remember we added affine2 2nd, so it's at front of queue */
502   for( unsigned int n = 0; n < affine2ParamsN; n++ )
503     {
504     parametersTruth.SetElement(
505       n, affine2->GetParameters().GetElement( n ) );
506     }
507   for( unsigned int n = 0; n < affineParamsN; n++ )
508     {
509     parametersTruth.SetElement( n + affine2ParamsN,
510                                 affine->GetParameters().GetElement( n ) );
511     }
512   std::cout << "Get Multi-transform Parameters: " << std::endl
513             << "parametersTruth: " << std::endl << parametersTruth
514             << std::endl
515             << "parametersTest from Composite: " << std::endl << parametersTest
516             << std::endl;
517 
518   if( !testVectorArray( parametersTest, parametersTruth ) )
519     {
520     std::cout << "Failed GetParameters() for multi transform." << std::endl;
521     return EXIT_FAILURE;
522     }
523 
524   /* Set parameters with multi transform. */
525   parametersNew.SetSize( parametersTruth.Size() );
526   parametersNew.Fill( 3.14 );
527   parametersNew[0] = 19;
528   parametersNew[parametersTruth.Size() - 1] = 71;
529   std::cout << "Set Multi-transform Parameters: " << std::endl;
530   compositeTransform->SetParameters( parametersNew );
531   std::cout << "retrieving... " << std::endl;
532   parametersReturned = compositeTransform->GetParameters();
533   std::cout << "parametersNew: " << std::endl << parametersNew << std::endl
534             << "parametersReturned: " << std::endl << parametersReturned
535             << std::endl;
536   if( !testVectorArray( parametersNew, parametersReturned ) )
537     {
538     std::cout << "Failed SetParameters() for multi transform." << std::endl;
539     return EXIT_FAILURE;
540     }
541 
542   /* Test get fixed parameters with multi-transform */
543   parametersTest = compositeTransform->GetFixedParameters();
544   affineParamsN = affine->GetFixedParameters().Size();
545   affine2ParamsN = affine2->GetFixedParameters().Size();
546   parametersTruth.SetSize( affine2ParamsN + affineParamsN );
547   parametersTruth.Fill(0); // Try this to quiet valgrind
548   for( unsigned int n = 0; n < affine2ParamsN; n++ )
549     {
550     parametersTruth.SetElement(
551       n, affine2->GetFixedParameters().GetElement( n ) );
552     }
553   for( unsigned int n = 0; n < affineParamsN; n++ )
554     {
555     parametersTruth.SetElement( n + affine2ParamsN,
556                                 affine->GetFixedParameters().GetElement( n ) );
557     }
558   std::cout << "Get Multi-transform Fixed Parameters: " << std::endl
559             << "parametersTruth: " << std::endl << parametersTruth
560             << std::endl
561             << "parametersTest: " << std::endl << parametersTest
562             << std::endl;
563 
564   if( !testVectorArray( parametersTest, parametersTruth ) )
565     {
566     std::cout << "Failed GetFixedParameters() for multi transform." << std::endl;
567     return EXIT_FAILURE;
568     }
569 
570   /* Test set fixed parameters with multi-transform */
571   std::cout << "Set Multi-transform Fixed Parameters: " << std::endl;
572   compositeTransform->SetFixedParameters( parametersTruth );
573   std::cout << "retrieving... " << std::endl;
574   parametersReturned = compositeTransform->GetFixedParameters();
575   std::cout << "parametersTruth: " << std::endl << parametersTruth << std::endl
576             << "parametersReturned: " << std::endl << parametersReturned
577             << std::endl;
578   // std::cout << "Composite Transform: " << std::endl << compositeTransform;
579   if( !testVectorArray( parametersTruth, parametersReturned ) )
580     {
581     std::cout << "Failed SetFixedParameters() for multi transform." << std::endl;
582     return EXIT_FAILURE;
583     }
584 
585   /*
586    * Add a third transform
587    */
588 
589   /* Add yet another affine transform */
590   AffineType::Pointer affine3 = AffineType::New();
591   matrix2[0][0] = 1.1;
592   matrix2[0][1] = 2.2;
593   matrix2[1][0] = 3.3;
594   matrix2[1][1] = 4.4;
595   vector2[0] = 5.5;
596   vector2[1] = 6.5;
597   affine3->SetMatrix(matrix2);
598   affine3->SetOffset(vector2);
599 
600   compositeTransform->AddTransform( affine3 );
601   // std::cout << "compositeTransform with 3 subs: "
602   //          << std::endl << compositeTransform << std::endl;
603 
604   /* Reset first affine to non-singular values */
605   matrix2[0][0] = 1;
606   matrix2[0][1] = 2;
607   matrix2[1][0] = 3;
608   matrix2[1][1] = 4;
609   vector2[0] = 5;
610   vector2[1] = 6;
611   affine->SetMatrix(matrix2);
612   affine->SetOffset(vector2);
613 
614   /* Test TransformsToOptimize flags */
615   compositeTransform->SetAllTransformsToOptimizeOff();
616   if( compositeTransform->GetNthTransformToOptimize(0) ||
617       compositeTransform->GetNthTransformToOptimize(1) ||
618       compositeTransform->GetNthTransformToOptimize(2) )
619     {
620     std::cout << "Failed clearing all TransformToOptimize flags. " << std::endl;
621     return EXIT_FAILURE;
622     }
623 
624   compositeTransform->SetOnlyMostRecentTransformToOptimizeOn();
625   if( compositeTransform->GetNthTransformToOptimize(0) ||
626       compositeTransform->GetNthTransformToOptimize(1) ||
627       !compositeTransform->GetNthTransformToOptimize(2) )
628     {
629     std::cout << "Failed setting only most recent TransformsToOptimize flag. "
630               << std::endl;
631     return EXIT_FAILURE;
632     }
633 
634   /* Test accessors */
635   CompositeType::TransformQueueType transformQueue =
636     compositeTransform->GetTransformQueue();
637   if( transformQueue.size() != 3 )
638     {
639     std::cout << "Failed getting transform queue." << std::endl;
640     return EXIT_FAILURE;
641     }
642   std::cout << "Got TransformQueue." << std::endl;
643 
644   CompositeType::TransformsToOptimizeFlagsType flagsQueue =
645     compositeTransform->GetTransformsToOptimizeFlags();
646   if( flagsQueue.size() != 3 )
647     {
648     std::cout << "Failed getting optimize flags queue." << std::endl;
649     return EXIT_FAILURE;
650     }
651 
652   /* Get inverse and check TransformsToOptimize flags are correct */
653   CompositeType::ConstPointer inverseTransform3;
654   inverseTransform3 = dynamic_cast<const CompositeType *>
655     ( compositeTransform->GetInverseTransform().GetPointer() );
656   if( !inverseTransform3 )
657     {
658     std::cout << "Failed calling GetInverseTransform() (3)." << std::endl;
659     return EXIT_FAILURE;
660     }
661   if( !inverseTransform3->GetNthTransformToOptimize(0) ||
662       inverseTransform3->GetNthTransformToOptimize(1) ||
663       inverseTransform3->GetNthTransformToOptimize(2) )
664     {
665     std::cout << "Failed checking TransformsToOptimize flags on inverse. "
666               << std::endl;
667     return EXIT_FAILURE;
668     }
669 
670   /* Test get params with only 1st and last transforms set to optimize.
671    * This implicitly tests the m_PreviousTransformsToOptimizeUpdateTime mechanism
672    * for updating m_TransformsToOptimizeQueue.
673    * This includes the affine and affine3 transforms */
674 
675   compositeTransform->SetNthTransformToOptimize(0, true);
676   if( !compositeTransform->GetNthTransformToOptimize(0) ||
677       compositeTransform->GetNthTransformToOptimize(1) ||
678       !compositeTransform->GetNthTransformToOptimize(2) )
679     {
680     std::cout << "Failed setting last TransformToOptimize flag. "
681               << "Composite Transform: " << std::endl << compositeTransform
682               << std::endl;
683     return EXIT_FAILURE;
684     }
685 
686   parametersTest = compositeTransform->GetParameters();
687   affineParamsN = affine->GetNumberOfParameters();
688   unsigned int affine3ParamsN = affine3->GetNumberOfParameters();
689   parametersTruth.SetSize( affineParamsN + affine3ParamsN );
690   for( unsigned int n = 0; n < affine3ParamsN; n++ )
691     {
692     parametersTruth.SetElement(
693       n, affine3->GetParameters().GetElement( n ) );
694     }
695   for( unsigned int n = 0; n < affineParamsN; n++ )
696     {
697     parametersTruth.SetElement( n + affine3ParamsN,
698                                 affine->GetParameters().GetElement( n ) );
699     }
700   std::cout << "Get 1st and 3rd transform Parameters: " << std::endl
701             << "parametersTruth: " << std::endl << parametersTruth
702             << std::endl
703             << "parametersTest from Composite: " << std::endl << parametersTest
704             << std::endl;
705 
706   if( !testVectorArray( parametersTest, parametersTruth ) )
707     {
708     std::cout << "Failed GetParameters() for 1st and 3rd transforms." << std::endl;
709     return EXIT_FAILURE;
710     }
711 
712   /* Test ComputeJacobianWithRespectToParameters with three transforms, two of which (1st and 3rd) are active.
713    * Remember that the point gets transformed by preceding transforms
714    * before its used for individual Jacobian. */
715   std::cout << "Test ComputeJacobianWithRespectToParameters with three transforms: " << std::endl;
716   CompositeType::JacobianType   jacTruth, jacComposite2, jacAffine, jacAffine3;
717   CompositeType::InputPointType jacPoint2;
718   jacPoint2[0] = 1;
719   jacPoint2[1] = 2;
720   compositeTransform->ComputeJacobianWithRespectToParameters( jacPoint2, jacComposite2 );
721   affine3->ComputeJacobianWithRespectToParameters( jacPoint2, jacAffine3 );
722   jacPoint2 = affine3->TransformPoint( jacPoint2 );
723   jacPoint2 = affine2->TransformPoint( jacPoint2 );
724   affine->ComputeJacobianWithRespectToParameters( jacPoint2, jacAffine );
725   jacTruth.SetSize( jacAffine3.rows(), jacAffine.cols() + jacAffine3.cols() );
726   jacTruth.update( affine->GetMatrix() * affine2->GetMatrix() * jacAffine3, 0, 0 );
727   jacTruth.update( jacAffine, 0, jacAffine3.cols() );
728   std::cout << "transformed jacPoint: " << jacPoint2 << std::endl;
729   std::cout << "Affine jacobian:" << std::endl << jacAffine;
730   std::cout << "affine3 jacobian:" << std::endl << jacAffine3;
731   std::cout << "Truth jacobian:" << std::endl << jacTruth;
732   std::cout << "Composite jacobian:" << std::endl << jacComposite2;
733   if( !testJacobian( jacComposite2, jacTruth ) )
734     {
735     std::cout << "Failed getting jacobian for two active transforms." << std::endl;
736     return EXIT_FAILURE;
737     }
738 
739   /* Test UpdateTransformParameters.
740    * NOTE Once there are transforms that do something other than simple
741    * addition in TransformUpdateParameters, this should be updated here.
742    */
743     {
744     /* Single transform full update, of last transform only */
745     compositeTransform->SetOnlyMostRecentTransformToOptimizeOn();
746     CompositeType::ParametersType truth = compositeTransform->GetParameters();
747     CompositeType::DerivativeType
748     update( compositeTransform->GetNumberOfParameters() );
749     update.Fill(10);
750     truth += update;
751     compositeTransform->UpdateTransformParameters( update );
752     CompositeType::ParametersType
753       updateResult = compositeTransform->GetParameters();
754     std::cout << "Testing UpdateTransformParameters 1. "
755               << std::endl;
756     if( !testVectorArray( truth, updateResult ) )
757       {
758       std::cout << "UpdateTransformParameters 1 failed. " << std::endl
759                 << " truth:  " << truth << std::endl
760                 << " result: " << updateResult << std::endl;
761       return EXIT_FAILURE;
762       }
763 
764     /* Update partially two transforms, with a scaling factor */
765     compositeTransform->SetNthTransformToOptimizeOn(0);
766     truth = compositeTransform->GetParameters();
767     update.SetSize( compositeTransform->GetNumberOfParameters() );
768     AffineType::ScalarType factor = 0.5;
769     for( unsigned int i = 0;
770          i < compositeTransform->GetNumberOfParameters(); i++ )
771       {
772       update[i] = i;
773       truth[i] += update[i] * factor;
774       }
775     compositeTransform->UpdateTransformParameters( update, factor );
776     updateResult = compositeTransform->GetParameters();
777     std::cout << "Testing UpdateTransformParameters 3. "
778               << std::endl;
779     if( !testVectorArray( truth, updateResult ) )
780       {
781       std::cout << "UpdateTransformParameters 3 failed. " << std::endl
782                 << " truth:  " << truth << std::endl
783                 << " result: " << updateResult << std::endl;
784       return EXIT_FAILURE;
785       }
786     }
787 
788   /* Test RemoveTransform */
789   bool opt1 = compositeTransform->GetTransformsToOptimizeFlags()[0];
790   bool opt2 = compositeTransform->GetTransformsToOptimizeFlags()[1];
791   compositeTransform->RemoveTransform();
792   if( compositeTransform->GetNumberOfTransforms() != 2 )
793     {
794     std::cout << "ERROR: expected 2 transforms, got " << compositeTransform->GetNumberOfTransforms() << std::endl;
795     return EXIT_FAILURE;
796     }
797   if( affine != compositeTransform->GetNthTransformConstPointer( 0 ) )
798     {
799     std::cout << "ERROR: 1st transform is not affine" << std::endl;
800     return EXIT_FAILURE;
801     }
802   if( affine2 != compositeTransform->GetNthTransformConstPointer( 1 ) )
803     {
804     std::cout << "ERROR: 2nd transform is not affine2" << std::endl;
805     return EXIT_FAILURE;
806     }
807   if( compositeTransform->GetTransformsToOptimizeFlags().size() != 2 )
808     {
809     std::cout << "ERROR: TransformsToOptimizeQueue is not length 2. It is " << compositeTransform->GetTransformsToOptimizeFlags().size() << std::endl;
810     return EXIT_FAILURE;
811     }
812   if( compositeTransform->GetNthTransformToOptimize(0) != opt1 )
813     {
814     std::cout << "ERROR: TransformsToOptimizeFlags[0] is not " << opt1 << std::endl;
815     return EXIT_FAILURE;
816     }
817   if( compositeTransform->GetNthTransformToOptimize(1) != opt2 )
818     {
819     std::cout << "ERROR: TransformsToOptimizeFlags[1] is not " << opt2 << std::endl;
820     return EXIT_FAILURE;
821     }
822 
823   /*
824    * Test flattening transform queue in the case of nested composite
825    * transforms
826    */
827 
828   CompositeType::Pointer nestedCompositeTransform = CompositeType::New();
829   CompositeType::Pointer compositeTransform1 = CompositeType::New();
830   CompositeType::Pointer compositeTransform2 = CompositeType::New();
831   CompositeType::Pointer compositeTransform3 = CompositeType::New();
832   CompositeType::Pointer compositeTransform4 = CompositeType::New();
833 
834   using TranslationTransformType = itk::TranslationTransform<double, NDimensions>;
835   using TranslationTransformPointer = TranslationTransformType::Pointer;
836   using TranslationTransformVector = std::vector<TranslationTransformPointer>;
837   TranslationTransformVector  translationTransformVector(12);
838   for( itk::SizeValueType n=0; n < 12; n++ )
839     {
840     translationTransformVector[n] = TranslationTransformType::New();
841     TranslationTransformType::ParametersType params(NDimensions);
842     params.Fill(n);
843     translationTransformVector[n]->SetParameters( params );
844     }
845 
846   compositeTransform1->AddTransform( translationTransformVector[0] );
847   compositeTransform1->AddTransform( translationTransformVector[1] );
848   compositeTransform1->AddTransform( translationTransformVector[2] );
849 
850   compositeTransform2->AddTransform( translationTransformVector[3] );
851   compositeTransform2->AddTransform( translationTransformVector[4] );
852 
853   compositeTransform3->AddTransform( translationTransformVector[5] );
854   compositeTransform3->AddTransform( translationTransformVector[6] );
855 
856   compositeTransform4->AddTransform( translationTransformVector[7] );
857   compositeTransform4->AddTransform( translationTransformVector[8] );
858   compositeTransform4->AddTransform( translationTransformVector[9] );
859   compositeTransform4->AddTransform( compositeTransform3 );
860 
861   nestedCompositeTransform->AddTransform( compositeTransform1 );
862   nestedCompositeTransform->AddTransform( translationTransformVector[10] );
863   nestedCompositeTransform->AddTransform( compositeTransform2 );
864   nestedCompositeTransform->AddTransform( compositeTransform4 );
865   nestedCompositeTransform->AddTransform( translationTransformVector[11] );
866 
867   std::cout << "Number of transforms before flattening = " << nestedCompositeTransform->GetNumberOfTransforms() << std::endl;
868   if( nestedCompositeTransform->GetNumberOfTransforms() != 5 )
869     {
870     std::cerr << "Error.  Should be 5." << std::endl;
871     return EXIT_FAILURE;
872     }
873 
874   nestedCompositeTransform->FlattenTransformQueue();
875   std::cout << "Number of transforms after flattening = " << nestedCompositeTransform->GetNumberOfTransforms() << std::endl;
876   if( nestedCompositeTransform->GetNumberOfTransforms() != 12 )
877     {
878     std::cerr << "Error.  Should be 12." << std::endl;
879     return EXIT_FAILURE;
880     }
881 
882   /* Verify the transform order */
883   bool passed = true;
884   for( itk::SizeValueType n=0; n < 12; n++ )
885     {
886     const TranslationTransformType::ParametersType & params = translationTransformVector[n]->GetParameters();
887     if( itk::Math::NotExactlyEquals(params[0], n) )
888       {
889       passed = false;
890       }
891     }
892   if( !passed )
893     {
894     std::cout << "Transform are not in correct order after flattening: " << std::endl;
895     for( itk::SizeValueType n=0; n < 12; n++ )
896       {
897       const TranslationTransformType::ParametersType & params = translationTransformVector[n]->GetParameters();
898       std::cout << " " << params[0];
899       }
900     std::cout << std::endl;
901     return EXIT_FAILURE;
902     }
903 
904   /* Test SetParameters with wrong size array */
905   std::cout << "Test SetParameters with wrong size array." << std::endl;
906   parametersTruth.SetSize(1);
907   bool caught = false;
908   try
909     {
910     compositeTransform->SetParameters( parametersTruth );
911     }
912   catch( itk::ExceptionObject & excp )
913     {
914     caught = true;
915     std::cout << "\nCaught expected exception:" << std::endl;
916     std::cout << excp << std::endl;
917     }
918   if( !caught )
919     {
920     std::cerr << "Expected exception calling SetParameters with wrong size"
921               << std::endl;
922 
923     return EXIT_FAILURE;
924     }
925 
926   /* Test printing */
927   compositeTransform->Print(std::cout);
928 
929   std::cout << "Passed test!" << std::endl;
930   return EXIT_SUCCESS;
931 
932 }
933