1 /*=========================================================================
2
3 Program: Visualization Toolkit
4 Module: vtkIterativeClosestPointTransform.cxx
5
6 Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
7 All rights reserved.
8 See Copyright.txt or http://www.kitware.com/Copyright.htm for details.
9
10 This software is distributed WITHOUT ANY WARRANTY; without even
11 the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
12 PURPOSE. See the above copyright notice for more information.
13
14 =========================================================================*/
15 #include "vtkIterativeClosestPointTransform.h"
16
17 #include "vtkCellLocator.h"
18 #include "vtkDataSet.h"
19 #include "vtkLandmarkTransform.h"
20 #include "vtkMath.h"
21 #include "vtkObjectFactory.h"
22 #include "vtkPoints.h"
23 #include "vtkTransform.h"
24
25 vtkStandardNewMacro(vtkIterativeClosestPointTransform);
26
27 //----------------------------------------------------------------------------
28
vtkIterativeClosestPointTransform()29 vtkIterativeClosestPointTransform::vtkIterativeClosestPointTransform()
30 : vtkLinearTransform()
31 {
32 this->Source = nullptr;
33 this->Target = nullptr;
34 this->Locator = nullptr;
35 this->LandmarkTransform = vtkLandmarkTransform::New();
36 this->MaximumNumberOfIterations = 50;
37 this->CheckMeanDistance = 0;
38 this->MeanDistanceMode = VTK_ICP_MODE_RMS;
39 this->MaximumMeanDistance = 0.01;
40 this->MaximumNumberOfLandmarks = 200;
41 this->StartByMatchingCentroids = 0;
42
43 this->NumberOfIterations = 0;
44 this->MeanDistance = 0.0;
45 }
46
47 //----------------------------------------------------------------------------
48
GetMeanDistanceModeAsString()49 const char *vtkIterativeClosestPointTransform::GetMeanDistanceModeAsString()
50 {
51 if ( this->MeanDistanceMode == VTK_ICP_MODE_RMS )
52 {
53 return "RMS";
54 }
55 else
56 {
57 return "AbsoluteValue";
58 }
59 }
60
61 //----------------------------------------------------------------------------
62
~vtkIterativeClosestPointTransform()63 vtkIterativeClosestPointTransform::~vtkIterativeClosestPointTransform()
64 {
65 ReleaseSource();
66 ReleaseTarget();
67 ReleaseLocator();
68 this->LandmarkTransform->Delete();
69 }
70
71 //----------------------------------------------------------------------------
72
SetSource(vtkDataSet * source)73 void vtkIterativeClosestPointTransform::SetSource(vtkDataSet *source)
74 {
75 if (this->Source == source)
76 {
77 return;
78 }
79
80 if (this->Source)
81 {
82 this->ReleaseSource();
83 }
84
85 if (source)
86 {
87 source->Register(this);
88 }
89
90 this->Source = source;
91 this->Modified();
92 }
93
94 //----------------------------------------------------------------------------
95
ReleaseSource(void)96 void vtkIterativeClosestPointTransform::ReleaseSource(void) {
97 if (this->Source)
98 {
99 this->Source->UnRegister(this);
100 this->Source = nullptr;
101 }
102 }
103
104 //----------------------------------------------------------------------------
105
SetTarget(vtkDataSet * target)106 void vtkIterativeClosestPointTransform::SetTarget(vtkDataSet *target)
107 {
108 if (this->Target == target)
109 {
110 return;
111 }
112
113 if (this->Target)
114 {
115 this->ReleaseTarget();
116 }
117
118 if (target)
119 {
120 target->Register(this);
121 }
122
123 this->Target = target;
124 this->Modified();
125 }
126
127 //----------------------------------------------------------------------------
128
ReleaseTarget(void)129 void vtkIterativeClosestPointTransform::ReleaseTarget(void) {
130 if (this->Target)
131 {
132 this->Target->UnRegister(this);
133 this->Target = nullptr;
134 }
135 }
136
137 //----------------------------------------------------------------------------
138
SetLocator(vtkCellLocator * locator)139 void vtkIterativeClosestPointTransform::SetLocator(vtkCellLocator *locator)
140 {
141 if (this->Locator == locator)
142 {
143 return;
144 }
145
146 if (this->Locator)
147 {
148 this->ReleaseLocator();
149 }
150
151 if (locator)
152 {
153 locator->Register(this);
154 }
155
156 this->Locator = locator;
157 this->Modified();
158 }
159
160 //----------------------------------------------------------------------------
161
ReleaseLocator(void)162 void vtkIterativeClosestPointTransform::ReleaseLocator(void) {
163 if (this->Locator)
164 {
165 this->Locator->UnRegister(this);
166 this->Locator = nullptr;
167 }
168 }
169
170 //----------------------------------------------------------------------------
171
CreateDefaultLocator()172 void vtkIterativeClosestPointTransform::CreateDefaultLocator() {
173 if (this->Locator)
174 {
175 this->ReleaseLocator();
176 }
177
178 this->Locator = vtkCellLocator::New();
179 }
180
181 //------------------------------------------------------------------------
182
GetMTime()183 vtkMTimeType vtkIterativeClosestPointTransform::GetMTime()
184 {
185 vtkMTimeType result = this->vtkLinearTransform::GetMTime();
186 vtkMTimeType mtime;
187
188 if (this->Source)
189 {
190 mtime = this->Source->GetMTime();
191 if (mtime > result)
192 {
193 result = mtime;
194 }
195 }
196
197 if (this->Target)
198 {
199 mtime = this->Target->GetMTime();
200 if (mtime > result)
201 {
202 result = mtime;
203 }
204 }
205
206 if (this->Locator)
207 {
208 mtime = this->Locator->GetMTime();
209 if (mtime > result)
210 {
211 result = mtime;
212 }
213 }
214
215 if (this->LandmarkTransform)
216 {
217 mtime = this->LandmarkTransform->GetMTime();
218 if (mtime > result)
219 {
220 result = mtime;
221 }
222 }
223
224 return result;
225 }
226
227 //----------------------------------------------------------------------------
228
Inverse()229 void vtkIterativeClosestPointTransform::Inverse()
230 {
231 vtkDataSet *tmp1 = this->Source;
232 this->Source = this->Target;
233 this->Target = tmp1;
234 this->Modified();
235 }
236
237 //----------------------------------------------------------------------------
238
MakeTransform()239 vtkAbstractTransform *vtkIterativeClosestPointTransform::MakeTransform()
240 {
241 return vtkIterativeClosestPointTransform::New();
242 }
243
244 //----------------------------------------------------------------------------
245
InternalDeepCopy(vtkAbstractTransform * transform)246 void vtkIterativeClosestPointTransform::InternalDeepCopy(vtkAbstractTransform *transform)
247 {
248 vtkIterativeClosestPointTransform *t = (vtkIterativeClosestPointTransform *)transform;
249
250 this->SetSource(t->GetSource());
251 this->SetTarget(t->GetTarget());
252 this->SetLocator(t->GetLocator());
253 this->SetMaximumNumberOfIterations(t->GetMaximumNumberOfIterations());
254 this->SetCheckMeanDistance(t->GetCheckMeanDistance());
255 this->SetMeanDistanceMode(t->GetMeanDistanceMode());
256 this->SetMaximumMeanDistance(t->GetMaximumMeanDistance());
257 this->SetMaximumNumberOfLandmarks(t->GetMaximumNumberOfLandmarks());
258
259 this->Modified();
260 }
261
262 //----------------------------------------------------------------------------
263
InternalUpdate()264 void vtkIterativeClosestPointTransform::InternalUpdate()
265 {
266 // Check source, target
267
268 if (this->Source == nullptr || !this->Source->GetNumberOfPoints())
269 {
270 vtkErrorMacro(<<"Can't execute with nullptr or empty input");
271 return;
272 }
273
274 if (this->Target == nullptr || !this->Target->GetNumberOfPoints())
275 {
276 vtkErrorMacro(<<"Can't execute with nullptr or empty target");
277 return;
278 }
279
280 // Create locator
281
282 this->CreateDefaultLocator();
283 this->Locator->SetDataSet(this->Target);
284 this->Locator->SetNumberOfCellsPerBucket(1);
285 this->Locator->BuildLocator();
286
287 // Create two sets of points to handle iteration
288
289 int step = 1;
290 if (this->Source->GetNumberOfPoints() > this->MaximumNumberOfLandmarks)
291 {
292 step = this->Source->GetNumberOfPoints() / this->MaximumNumberOfLandmarks;
293 vtkDebugMacro(<< "Landmarks step is now : " << step);
294 }
295
296 vtkIdType nb_points = this->Source->GetNumberOfPoints() / step;
297
298 // Allocate some points.
299 // - closestp is used so that the internal state of LandmarkTransform remains
300 // correct whenever the iteration process is stopped (hence its source
301 // and landmark points might be used in a vtkThinPlateSplineTransform).
302 // - points2 could have been avoided, but do not ask me why
303 // InternalTransformPoint is not working correctly on my computer when
304 // in and out are the same pointer.
305
306 vtkPoints *points1 = vtkPoints::New();
307 points1->SetNumberOfPoints(nb_points);
308
309 vtkPoints *closestp = vtkPoints::New();
310 closestp->SetNumberOfPoints(nb_points);
311
312 vtkPoints *points2 = vtkPoints::New();
313 points2->SetNumberOfPoints(nb_points);
314
315 // Fill with initial positions (sample dataset using step)
316
317 vtkTransform *accumulate = vtkTransform::New();
318 accumulate->PostMultiply();
319
320 vtkIdType i;
321 int j;
322 double p1[3], p2[3];
323
324 if (StartByMatchingCentroids)
325 {
326 double source_centroid[3] = {0,0,0};
327 for (i = 0; i < this->Source->GetNumberOfPoints(); i++)
328 {
329 this->Source->GetPoint(i, p1);
330 source_centroid[0] += p1[0];
331 source_centroid[1] += p1[1];
332 source_centroid[2] += p1[2];
333 }
334 source_centroid[0] /= this->Source->GetNumberOfPoints();
335 source_centroid[1] /= this->Source->GetNumberOfPoints();
336 source_centroid[2] /= this->Source->GetNumberOfPoints();
337
338 double target_centroid[3] = {0,0,0};
339 for (i = 0; i < this->Target->GetNumberOfPoints(); i++)
340 {
341 this->Target->GetPoint(i, p2);
342 target_centroid[0] += p2[0];
343 target_centroid[1] += p2[1];
344 target_centroid[2] += p2[2];
345 }
346 target_centroid[0] /= this->Target->GetNumberOfPoints();
347 target_centroid[1] /= this->Target->GetNumberOfPoints();
348 target_centroid[2] /= this->Target->GetNumberOfPoints();
349
350 accumulate->Translate(target_centroid[0] - source_centroid[0],
351 target_centroid[1] - source_centroid[1],
352 target_centroid[2] - source_centroid[2]);
353 accumulate->Update();
354
355 for (i = 0, j = 0; i < nb_points; i++, j += step)
356 {
357 double outPoint[3];
358 accumulate->InternalTransformPoint(this->Source->GetPoint(j),
359 outPoint);
360 points1->SetPoint(i, outPoint);
361 }
362 }
363 else
364 {
365 for (i = 0, j = 0; i < nb_points; i++, j += step)
366 {
367 points1->SetPoint(i, this->Source->GetPoint(j));
368 }
369 }
370
371 // Go
372
373 vtkIdType cell_id;
374 int sub_id;
375 double dist2, totaldist = 0;
376 double outPoint[3];
377
378 vtkPoints *temp, *a = points1, *b = points2;
379
380 this->NumberOfIterations = 0;
381
382 do
383 {
384 // Fill points with the closest points to each vertex in input
385
386 for(i = 0; i < nb_points; i++)
387 {
388 this->Locator->FindClosestPoint(a->GetPoint(i),
389 outPoint,
390 cell_id,
391 sub_id,
392 dist2);
393 closestp->SetPoint(i, outPoint);
394 }
395
396 // Build the landmark transform
397
398 this->LandmarkTransform->SetSourceLandmarks(a);
399 this->LandmarkTransform->SetTargetLandmarks(closestp);
400 this->LandmarkTransform->Update();
401
402 // Concatenate (can't use this->Concatenate directly)
403
404 accumulate->Concatenate(this->LandmarkTransform->GetMatrix());
405
406 this->NumberOfIterations++;
407 vtkDebugMacro(<< "Iteration: " << this->NumberOfIterations);
408 if (this->NumberOfIterations >= this->MaximumNumberOfIterations)
409 {
410 break;
411 }
412
413 // Move mesh and compute mean distance if needed
414
415 if (this->CheckMeanDistance)
416 {
417 totaldist = 0.0;
418 }
419
420 for(i = 0; i < nb_points; i++)
421 {
422 a->GetPoint(i, p1);
423 this->LandmarkTransform->InternalTransformPoint(p1, p2);
424 b->SetPoint(i, p2);
425 if (this->CheckMeanDistance)
426 {
427 if (this->MeanDistanceMode == VTK_ICP_MODE_RMS)
428 {
429 totaldist += vtkMath::Distance2BetweenPoints(p1, p2);
430 } else {
431 totaldist += sqrt(vtkMath::Distance2BetweenPoints(p1, p2));
432 }
433 }
434 }
435
436 if (this->CheckMeanDistance)
437 {
438 if (this->MeanDistanceMode == VTK_ICP_MODE_RMS)
439 {
440 this->MeanDistance = sqrt(totaldist / (double)nb_points);
441 } else {
442 this->MeanDistance = totaldist / (double)nb_points;
443 }
444 vtkDebugMacro("Mean distance: " << this->MeanDistance);
445 if (this->MeanDistance <= this->MaximumMeanDistance)
446 {
447 break;
448 }
449 }
450
451 temp = a;
452 a = b;
453 b = temp;
454
455 }
456 while (1);
457
458 // Now recover accumulated result
459
460 this->Matrix->DeepCopy(accumulate->GetMatrix());
461
462 accumulate->Delete();
463 points1->Delete();
464 closestp->Delete();
465 points2->Delete();
466 }
467
468 //----------------------------------------------------------------------------
469
PrintSelf(ostream & os,vtkIndent indent)470 void vtkIterativeClosestPointTransform::PrintSelf(ostream& os, vtkIndent indent)
471 {
472 this->Superclass::PrintSelf(os,indent);
473
474 if ( this->Source )
475 {
476 os << indent << "Source: " << this->Source << "\n";
477 }
478 else
479 {
480 os << indent << "Source: (none)\n";
481 }
482
483 if ( this->Target )
484 {
485 os << indent << "Target: " << this->Target << "\n";
486 }
487 else
488 {
489 os << indent << "Target: (none)\n";
490 }
491
492 if ( this->Locator )
493 {
494 os << indent << "Locator: " << this->Locator << "\n";
495 }
496 else
497 {
498 os << indent << "Locator: (none)\n";
499 }
500
501 os << indent << "MaximumNumberOfIterations: " << this->MaximumNumberOfIterations << "\n";
502 os << indent << "CheckMeanDistance: " << this->CheckMeanDistance << "\n";
503 os << indent << "MeanDistanceMode: " << this->GetMeanDistanceModeAsString() << "\n";
504 os << indent << "MaximumMeanDistance: " << this->MaximumMeanDistance << "\n";
505 os << indent << "MaximumNumberOfLandmarks: " << this->MaximumNumberOfLandmarks << "\n";
506 os << indent << "StartByMatchingCentroids: " << this->StartByMatchingCentroids << "\n";
507 os << indent << "NumberOfIterations: " << this->NumberOfIterations << "\n";
508 os << indent << "MeanDistance: " << this->MeanDistance << "\n";
509 if(this->LandmarkTransform)
510 {
511 os << indent << "LandmarkTransform:\n";
512 this->LandmarkTransform->PrintSelf(os, indent.GetNextIndent());
513 }
514 }
515