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 // Software Guide : BeginLatex
20 //
21 // This example illustrates how to perform Iterative Closest Point (ICP)
22 // registration in ITK using sets of 3D points.
23 //
24 // Software Guide : EndLatex
25
26 // Software Guide : BeginLatex
27 //
28 // The first step is to include the relevant headers.
29 //
30 // Software Guide : EndLatex
31
32 // Software Guide : BeginCodeSnippet
33 #include "itkEuler3DTransform.h"
34 #include "itkEuclideanDistancePointMetric.h"
35 #include "itkLevenbergMarquardtOptimizer.h"
36 #include "itkPointSetToPointSetRegistrationMethod.h"
37 #include <iostream>
38 #include <fstream>
39 // Software Guide : EndCodeSnippet
40
41 class CommandIterationUpdate : public itk::Command
42 {
43 public:
44 using Self = CommandIterationUpdate;
45 using Superclass = itk::Command;
46 using Pointer = itk::SmartPointer<Self>;
47 itkNewMacro( Self );
48
49 protected:
50 CommandIterationUpdate() = default;
51
52 public:
53 using OptimizerType = itk::LevenbergMarquardtOptimizer;
54 using OptimizerPointer = const OptimizerType *;
55
Execute(itk::Object * caller,const itk::EventObject & event)56 void Execute(itk::Object *caller, const itk::EventObject & event) override
57 {
58 Execute( (const itk::Object *)caller, event);
59 }
60
Execute(const itk::Object * object,const itk::EventObject & event)61 void Execute(const itk::Object * object, const itk::EventObject & event) override
62 {
63 auto optimizer = dynamic_cast< OptimizerPointer >( object );
64 if( optimizer == nullptr )
65 {
66 itkExceptionMacro( "Could not cast optimizer." );
67 }
68
69 if( ! itk::IterationEvent().CheckEvent( &event ) )
70 {
71 return;
72 }
73
74 std::cout << "Value = " << optimizer->GetCachedValue() << std::endl;
75 std::cout << "Position = " << optimizer->GetCachedCurrentPosition();
76 std::cout << std::endl << std::endl;
77 }
78 };
79
80
main(int argc,char * argv[])81 int main(int argc, char * argv[] )
82 {
83
84 if( argc < 3 )
85 {
86 std::cerr << "Arguments Missing. " << std::endl;
87 std::cerr <<
88 "Usage: IterativeClosestPoint2 fixedPointsFile movingPointsFile "
89 << std::endl;
90 return EXIT_FAILURE;
91 }
92
93 constexpr unsigned int Dimension = 3;
94
95 // Software Guide : BeginLatex
96 //
97 // First, define the necessary types for the moving and fixed point sets.
98 //
99 // Software Guide : EndLatex
100
101 // Software Guide : BeginCodeSnippet
102 using PointSetType = itk::PointSet< float, Dimension >;
103
104 PointSetType::Pointer fixedPointSet = PointSetType::New();
105 PointSetType::Pointer movingPointSet = PointSetType::New();
106
107 using PointType = PointSetType::PointType;
108
109 using PointsContainer = PointSetType::PointsContainer;
110
111 PointsContainer::Pointer fixedPointContainer = PointsContainer::New();
112 PointsContainer::Pointer movingPointContainer = PointsContainer::New();
113
114 PointType fixedPoint;
115 PointType movingPoint;
116 // Software Guide : EndCodeSnippet
117
118 // Read the file containing coordinates of fixed points.
119 std::ifstream fixedFile;
120 fixedFile.open( argv[1] );
121 if( fixedFile.fail() )
122 {
123 std::cerr << "Error opening points file with name : " << std::endl;
124 std::cerr << argv[1] << std::endl;
125 return EXIT_FAILURE;
126 }
127
128 unsigned int pointId = 0;
129 fixedFile >> fixedPoint;
130 while( !fixedFile.eof() )
131 {
132 fixedPointContainer->InsertElement( pointId, fixedPoint );
133 fixedFile >> fixedPoint;
134 pointId++;
135 }
136 fixedPointSet->SetPoints( fixedPointContainer );
137 std::cout <<
138 "Number of fixed Points = " << fixedPointSet->GetNumberOfPoints()
139 << std::endl;
140
141 // Read the file containing coordinates of moving points.
142 std::ifstream movingFile;
143 movingFile.open( argv[2] );
144 if( movingFile.fail() )
145 {
146 std::cerr << "Error opening points file with name : " << std::endl;
147 std::cerr << argv[2] << std::endl;
148 return EXIT_FAILURE;
149 }
150
151 pointId = 0;
152 movingFile >> movingPoint;
153 while( !movingFile.eof() )
154 {
155 movingPointContainer->InsertElement( pointId, movingPoint );
156 movingFile >> movingPoint;
157 pointId++;
158 }
159 movingPointSet->SetPoints( movingPointContainer );
160 std::cout <<
161 "Number of moving Points = "
162 << movingPointSet->GetNumberOfPoints() << std::endl;
163
164
165 // Software Guide : BeginLatex
166 //
167 // After the points are read in from files, setup the metric to be used
168 // later by the registration.
169 //
170 // Software Guide : EndLatex
171
172 // Software Guide : BeginCodeSnippet
173 using MetricType = itk::EuclideanDistancePointMetric<
174 PointSetType, PointSetType >;
175
176 MetricType::Pointer metric = MetricType::New();
177 // Software Guide : EndCodeSnippet
178
179 // Software Guide : BeginLatex
180 //
181 // Next, setup the tranform, optimizers, and registration.
182 //
183 // Software Guide : EndLatex
184
185 // Software Guide : BeginCodeSnippet
186 using TransformType = itk::Euler3DTransform< double >;
187
188 TransformType::Pointer transform = TransformType::New();
189
190
191 // Optimizer Type
192 using OptimizerType = itk::LevenbergMarquardtOptimizer;
193
194 OptimizerType::Pointer optimizer = OptimizerType::New();
195 optimizer->SetUseCostFunctionGradient(false);
196
197 // Registration Method
198 using RegistrationType = itk::PointSetToPointSetRegistrationMethod<
199 PointSetType, PointSetType >;
200
201
202 RegistrationType::Pointer registration = RegistrationType::New();
203 // Software Guide : EndCodeSnippet
204
205 // Software Guide : BeginLatex
206 //
207 // Scale the translation components of the Transform in the Optimizer
208 //
209 // Software Guide : EndLatex
210
211 // Software Guide : BeginCodeSnippet
212 OptimizerType::ScalesType scales( transform->GetNumberOfParameters() );
213 // Software Guide : EndCodeSnippet
214
215 // Software Guide : BeginLatex
216 //
217 // Next, set the scales and ranges for translations and rotations in the
218 // transform. Also, set the convergence criteria and number of iterations
219 // to be used by the optimizer.
220 //
221 // Software Guide : EndLatex
222
223 // Software Guide : BeginCodeSnippet
224 constexpr double translationScale = 1000.0; // dynamic range of translations
225 constexpr double rotationScale = 1.0; // dynamic range of rotations
226
227 scales[0] = 1.0 / rotationScale;
228 scales[1] = 1.0 / rotationScale;
229 scales[2] = 1.0 / rotationScale;
230 scales[3] = 1.0 / translationScale;
231 scales[4] = 1.0 / translationScale;
232 scales[5] = 1.0 / translationScale;
233
234 unsigned long numberOfIterations = 2000;
235 double gradientTolerance = 1e-4; // convergence criterion
236 double valueTolerance = 1e-4; // convergence criterion
237 double epsilonFunction = 1e-5; // convergence criterion
238
239
240 optimizer->SetScales( scales );
241 optimizer->SetNumberOfIterations( numberOfIterations );
242 optimizer->SetValueTolerance( valueTolerance );
243 optimizer->SetGradientTolerance( gradientTolerance );
244 optimizer->SetEpsilonFunction( epsilonFunction );
245 // Software Guide : EndCodeSnippet
246
247 // Software Guide : BeginLatex
248 //
249 // Here we start with an identity transform, although the user will usually
250 // be able to provide a better guess than this.
251 //
252 // Software Guide : EndLatex
253
254 // Software Guide : BeginCodeSnippet
255 transform->SetIdentity();
256 // Software Guide : EndCodeSnippet
257
258 registration->SetInitialTransformParameters( transform->GetParameters() );
259
260 // Software Guide : BeginLatex
261 //
262 // Connect all the components required for the registration.
263 //
264 // Software Guide : EndLatex
265
266 // Software Guide : BeginCodeSnippet
267 registration->SetMetric( metric );
268 registration->SetOptimizer( optimizer );
269 registration->SetTransform( transform );
270 registration->SetFixedPointSet( fixedPointSet );
271 registration->SetMovingPointSet( movingPointSet );
272 // Software Guide : EndCodeSnippet
273 //
274 // Connect an observer
275 CommandIterationUpdate::Pointer observer = CommandIterationUpdate::New();
276 optimizer->AddObserver( itk::IterationEvent(), observer );
277
278 try
279 {
280 registration->Update();
281 }
282 catch( itk::ExceptionObject & e )
283 {
284 std::cerr << e << std::endl;
285 return EXIT_FAILURE;
286 }
287
288 std::cout << "Solution = " << transform->GetParameters() << std::endl;
289 std::cout << "Stopping condition: " << optimizer->GetStopConditionDescription() << std::endl;
290
291 return EXIT_SUCCESS;
292 }
293