PlusLib  2.9.0
Software library for tracked ultrasound image acquisition, calibration, and processing.
vtkPlusPhantomLandmarkRegistrationAlgo.cxx
Go to the documentation of this file.
1 /*=Plus=header=begin======================================================
2  Program: Plus
3  Copyright (c) Laboratory for Percutaneous Surgery. All rights reserved.
4  See License.txt for details.
5 =========================================================Plus=header=end*/
6 
7 #include "PlusConfigure.h"
8 
10 
11 #include "itkImage.h"
12 #include "itkLandmarkBasedTransformInitializer.h"
13 
14 #include "vtkMath.h"
15 #include "vtkObjectFactory.h"
16 #include "vtkIGSIOTransformRepository.h"
17 #include "vtkXMLUtilities.h"
18 
19 //-----------------------------------------------------------------------------
20 
22 
23 std::string vtkPlusPhantomLandmarkRegistrationAlgo::ConfigurationElementName = "vtkPlusPhantomLandmarkRegistrationAlgo";
24 
25 //-----------------------------------------------------------------------------
26 
28 {
29  this->RegistrationErrorMm = -1.0;
30 
32  this->DefinedLandmarks_Phantom = NULL;
33  this->RecordedLandmarks_Reference = NULL;
34  this->PhantomCoordinateFrame = NULL;
35  this->ReferenceCoordinateFrame = NULL;
36  this->StylusTipCoordinateFrame = NULL;
37 
38  this->DefinedLandmarkNames.clear();
39 
40  vtkSmartPointer<vtkPoints> definedLandmarks = vtkSmartPointer<vtkPoints>::New();
41  this->SetDefinedLandmarks_Phantom( definedLandmarks );
42 
43  vtkSmartPointer<vtkPoints> recordedLandmarks = vtkSmartPointer<vtkPoints>::New();
44  this->SetRecordedLandmarks_Reference( recordedLandmarks );
45 }
46 
47 //-----------------------------------------------------------------------------
48 
50 {
52  this->SetDefinedLandmarks_Phantom( NULL );
53  this->SetRecordedLandmarks_Reference( NULL );
54  this->PhantomCoordinateFrame = NULL;
55  this->ReferenceCoordinateFrame = NULL;
56  this->StylusTipCoordinateFrame = NULL;
57 }
58 
59 //-----------------------------------------------------------------------------
60 
61 PlusStatus vtkPlusPhantomLandmarkRegistrationAlgo::LandmarkRegister( vtkIGSIOTransformRepository* aTransformRepository/* = NULL*/ )
62 {
63  LOG_TRACE( "vtkPlusPhantomLandmarkRegistrationAlgo::Register" );
64 
65  // Create input point vectors
66  std::vector< itk::Point<double, 3> > fixedPoints;
67  std::vector< itk::Point<double, 3> > movingPoints;
68 
69  for ( int i = 0; i < this->RecordedLandmarks_Reference->GetNumberOfPoints(); ++i )
70  {
71  // Defined landmarks from xml are in the phantom coordinate system
72  double* fixedPointArray = this->DefinedLandmarks_Phantom->GetPoint( i );
73  itk::Point<double, 3> fixedPoint( fixedPointArray );
74 
75  // Recorded landmarks are in the tracker coordinate system
76  double* movingPointArray = this->RecordedLandmarks_Reference->GetPoint( i );
77  itk::Point<double, 3> movingPoint( movingPointArray );
78 
79  fixedPoints.push_back( fixedPoint );
80  movingPoints.push_back( movingPoint );
81  }
82 
83  for ( int i = 0; i < this->RecordedLandmarks_Reference->GetNumberOfPoints(); ++i )
84  {
85  LOG_DEBUG( "Phantom point " << i << ": Defined: " << fixedPoints[i] << " Recorded: " << movingPoints[i] );
86  }
87 
88  // Initialize ITK transform
89  auto transform = itk::VersorRigid3DTransform<double>::New();
90  transform->SetIdentity();
91 
92  auto initializer = itk::LandmarkBasedTransformInitializer< itk::VersorRigid3DTransform<double>, itk::Image<short, 3>, itk::Image<short, 3> >::New();
93  initializer->SetTransform( transform );
94  initializer->SetFixedLandmarks( fixedPoints );
95  initializer->SetMovingLandmarks( movingPoints );
96  initializer->InitializeTransform();
97 
98  // Get result (do the registration)
99  vtkSmartPointer<vtkMatrix4x4> phantomToReferenceTransformMatrix = vtkSmartPointer<vtkMatrix4x4>::New();
100  phantomToReferenceTransformMatrix->Identity();
101 
102  itk::Matrix<double, 3, 3> transformMatrix = transform->GetMatrix();
103  for ( int i = 0; i < transformMatrix.RowDimensions; ++i )
104  {
105  for ( int j = 0; j < transformMatrix.ColumnDimensions; ++j )
106  {
107  phantomToReferenceTransformMatrix->SetElement( i, j, transformMatrix[i][j] );
108  }
109  }
110  itk::Vector<double, 3> transformOffset = transform->GetOffset();
111  for ( unsigned int j = 0; j < transformOffset.GetNumberOfComponents(); ++j )
112  {
113  phantomToReferenceTransformMatrix->SetElement( j, 3, transformOffset[j] );
114  }
115 
116  std::ostringstream osPhantomToReferenceTransformMatrix;
117  phantomToReferenceTransformMatrix->Print( osPhantomToReferenceTransformMatrix );
118 
119  LOG_DEBUG( "PhantomToReferenceTransformMatrix:\n" << osPhantomToReferenceTransformMatrix.str().c_str() );
120 
121  this->SetPhantomToReferenceTransformMatrix( phantomToReferenceTransformMatrix );
122 
123  if ( ComputeError() != PLUS_SUCCESS )
124  {
125  LOG_ERROR( "Failed to compute registration error!" );
126  return PLUS_FAIL;
127  }
128 
129  // Save result
130  if ( aTransformRepository )
131  {
132  igsioTransformName phantomToReferenceTransformName( this->PhantomCoordinateFrame, this->ReferenceCoordinateFrame );
133  aTransformRepository->SetTransform( phantomToReferenceTransformName, this->PhantomToReferenceTransformMatrix );
134  aTransformRepository->SetTransformPersistent( phantomToReferenceTransformName, true );
135  aTransformRepository->SetTransformDate( phantomToReferenceTransformName, vtkIGSIOAccurateTimer::GetInstance()->GetDateAndTimeString().c_str() );
136  aTransformRepository->SetTransformError( phantomToReferenceTransformName, this->RegistrationErrorMm );
137  }
138  else
139  {
140  LOG_INFO( "Transform repository object is NULL, cannot save results into it" );
141  }
142 
143  return PLUS_SUCCESS;
144 }
145 
146 
147 //-----------------------------------------------------------------------------
148 
150 {
151  LOG_TRACE( "vtkPlusPhantomLandmarkRegistrationAlgo::ReadConfiguration" );
152 
153  XML_FIND_NESTED_ELEMENT_REQUIRED( phantomDefinition, aConfig, "PhantomDefinition" );
154 
155  this->DefinedLandmarks_Phantom->Reset();
156  this->RecordedLandmarks_Reference->Reset();
157  this->DefinedLandmarkNames.clear();
158 
159  // Load geometry
160  XML_FIND_NESTED_ELEMENT_REQUIRED( geometry, phantomDefinition, "Geometry" );
161 
162  // Read landmarks (NWires are not interesting at this point, it is only parsed if segmentation is needed)
163  vtkXMLDataElement* landmarks = geometry->FindNestedElementWithName( "Landmarks" );
164  if ( landmarks == NULL )
165  {
166  if( geometry->FindNestedElementWithName( "Planes" ) == NULL )
167  {
168  LOG_ERROR( "No Planes or Landmarks found in configuration file found, registration is not possible!" );
169  return PLUS_FAIL;
170  }
171  else
172  {
173  LOG_WARNING( "No Landmarks found in configuration file found, perform Landmark Registration" );
174  return PLUS_FAIL;
175  }
176  }
177  else
178  {
179  int numberOfLandmarks = landmarks->GetNumberOfNestedElements();
180  this->DefinedLandmarkNames.resize( numberOfLandmarks );
181 
182  for ( int i = 0; i < numberOfLandmarks; ++i )
183  {
184  vtkXMLDataElement* landmark = landmarks->GetNestedElement( i );
185  if ( ( landmark == NULL ) || ( STRCASECMP( "Landmark", landmark->GetName() ) ) )
186  {
187  LOG_WARNING( "Invalid landmark definition found" );
188  continue;
189  }
190 
191  const char* landmarkName = landmark->GetAttribute( "Name" );
192  if ( landmarkName == NULL )
193  {
194  LOG_WARNING( "Invalid landmark name (landmark #" << i + 1 << ")" );
195  continue;
196  }
197 
198  double landmarkPosition[4] = {0, 0, 0, 1};
199  if ( ! landmark->GetVectorAttribute( "Position", 3, landmarkPosition ) )
200  {
201  LOG_WARNING( "Invalid landmark position (landmark #" << i + 1 << ")" );
202  continue;
203  }
204 
205  this->DefinedLandmarks_Phantom->InsertPoint( i, landmarkPosition );
206  this->DefinedLandmarkNames[i] = landmarkName;
207  }
208  }
209 
210  if ( this->DefinedLandmarks_Phantom->GetNumberOfPoints() == 0 )
211  {
212  LOG_ERROR( "No valid landmarks were found!" );
213  return PLUS_FAIL;
214  }
215 
216  // vtkPlusPhantomLandmarkRegistrationAlgo section
217  XML_FIND_NESTED_ELEMENT_REQUIRED( phantomRegistrationElement, aConfig, vtkPlusPhantomLandmarkRegistrationAlgo::ConfigurationElementName.c_str() );
218  XML_READ_CSTRING_ATTRIBUTE_REQUIRED( PhantomCoordinateFrame, phantomRegistrationElement );
219  XML_READ_CSTRING_ATTRIBUTE_REQUIRED( ReferenceCoordinateFrame, phantomRegistrationElement );
220  XML_READ_CSTRING_ATTRIBUTE_REQUIRED( StylusTipCoordinateFrame, phantomRegistrationElement );
221 
222  return PLUS_SUCCESS;
223 }
224 
225 //-----------------------------------------------------------------------------
227 {
228  LOG_TRACE( "vtkPlusPhantomLandmarkRegistrationAlgo::ComputeError" );
229 
230  double sumDistance = 0.0;
231 
232  for ( int i = 0; i < this->RecordedLandmarks_Reference->GetNumberOfPoints(); ++i )
233  {
234  // Defined landmarks from xml are in the phantom coordinate system
235  double definedLandmark_Phantom[4] = {0, 0, 0, 1.0};
236  this->DefinedLandmarks_Phantom->GetPoint( i, definedLandmark_Phantom );
237 
238  double* definedLandmark_Reference = this->PhantomToReferenceTransformMatrix->MultiplyDoublePoint( definedLandmark_Phantom );
239  sumDistance += sqrt( vtkMath::Distance2BetweenPoints( definedLandmark_Reference, this->RecordedLandmarks_Reference->GetPoint( i ) ) );
240  }
241 
242  this->RegistrationErrorMm = sumDistance / this->RecordedLandmarks_Reference->GetNumberOfPoints();
243 
244  LOG_DEBUG( "Registration error = " << this->RegistrationErrorMm );
245 
246  return PLUS_SUCCESS;
247 }
248 
249 //-----------------------------------------------------------------------------
251 {
252  // Defined landmarks from xml are in the phantom coordinate system
253  double referencePointArray_Phantom[4] = {0, 0, 0, 1};
254  double otherPointArray_Phantom[4] = {0, 0, 0, 1};
255  double minimumDistanceMm = DBL_MAX;
256 
257  for ( int i = 0; i < this->DefinedLandmarks_Phantom->GetNumberOfPoints(); ++i )
258  {
259  this->DefinedLandmarks_Phantom->GetPoint( i, referencePointArray_Phantom );
260  for( int j = i + 1; j < this->DefinedLandmarks_Phantom->GetNumberOfPoints(); ++j )
261  {
262  this->DefinedLandmarks_Phantom->GetPoint( j, otherPointArray_Phantom );
263  // Defined landmarks from xml are in the phantom coordinate system
264  double distanceMm = sqrt( vtkMath::Distance2BetweenPoints( referencePointArray_Phantom, otherPointArray_Phantom ) );
265  if( distanceMm < minimumDistanceMm )
266  {
267  minimumDistanceMm = distanceMm;
268  }
269  }
270  }
271  return minimumDistanceMm;
272 }
273 
274 //-----------------------------------------------------------------------------
276 {
277  // Defined landmarks from xml are in the phantom coordinate system
278  double landmarksAverage_Phantom[4] = {0, 0, 0, 1};
279  GetDefinedLandmarksCentroid_Phantom( landmarksAverage_Phantom );
280  // store it in a temporary variable because MultiplyDoublePoint would overwrite the 4th component of landmarksAverage_Reference, too
281  double* tempPtr = this->PhantomToReferenceTransformMatrix->MultiplyDoublePoint( landmarksAverage_Phantom );
282  landmarksAverage_Reference[0] = tempPtr[0];
283  landmarksAverage_Reference[1] = tempPtr[1];
284  landmarksAverage_Reference[2] = tempPtr[2];
285 }
286 
287 //-----------------------------------------------------------------------------
289 {
290  // Defined landmarks from xml are in the phantom coordinate system
291  double definedLandmarkPoint_Phantom[4] = {0, 0, 0, 1};
292  int numberOfLandmarks = this->DefinedLandmarks_Phantom->GetNumberOfPoints();
293  for ( int landmarkIndex = 0; landmarkIndex < numberOfLandmarks; landmarkIndex++ )
294  {
295  this->DefinedLandmarks_Phantom->GetPoint( landmarkIndex, definedLandmarkPoint_Phantom );
296  landmarksCentroid_Phantom[0] += definedLandmarkPoint_Phantom[0];
297  landmarksCentroid_Phantom[1] += definedLandmarkPoint_Phantom[1];
298  landmarksCentroid_Phantom[2] += definedLandmarkPoint_Phantom[2];
299  }
300  landmarksCentroid_Phantom[0] = landmarksCentroid_Phantom[0] / numberOfLandmarks;
301  landmarksCentroid_Phantom[1] = landmarksCentroid_Phantom[1] / numberOfLandmarks;
302  landmarksCentroid_Phantom[2] = landmarksCentroid_Phantom[2] / numberOfLandmarks;
303  return;
304 }
305 
306 //-----------------------------------------------------------------------------
307 void vtkPlusPhantomLandmarkRegistrationAlgo::GetLandmarkCameraPosition_Reference( int index, double cameraPosition_Reference[4] )
308 {
309  this->DefinedLandmarks_Phantom->GetPoint( index, cameraPosition_Reference );
310  double centroid_Phantom[4] = {0, 0, 0, 1};
311  double viewNormalVector_Reference[4] = {0, 0, 0, 1};
312 
313  this->GetDefinedLandmarksCentroid_Phantom( centroid_Phantom );
314  viewNormalVector_Reference[0] = cameraPosition_Reference[0] - centroid_Phantom[0];
315  viewNormalVector_Reference[1] = cameraPosition_Reference[1] - centroid_Phantom[1];
316  viewNormalVector_Reference[2] = cameraPosition_Reference[2] - centroid_Phantom[2];
317 
318  vtkMath::Normalize( viewNormalVector_Reference );
319  const double CAMERA_DISTANCE_FROM_CENTROID = 500; // in general, 50cm from the centroid gives a good view of the phantom
320  centroid_Phantom[0] = centroid_Phantom[0] + CAMERA_DISTANCE_FROM_CENTROID * viewNormalVector_Reference[0];
321  centroid_Phantom[1] = centroid_Phantom[1] + CAMERA_DISTANCE_FROM_CENTROID * viewNormalVector_Reference[1];
322  centroid_Phantom[2] = centroid_Phantom[2] + CAMERA_DISTANCE_FROM_CENTROID * viewNormalVector_Reference[2];
323 
324  // store it in a temporary variable because MultiplyDoublePoint would overwrite the 4th component of cameraPosition_Reference, too
325  double* tempPtr = this->PhantomToReferenceTransformMatrix->MultiplyDoublePoint( centroid_Phantom );
326  cameraPosition_Reference[0] = tempPtr[0];
327  cameraPosition_Reference[1] = tempPtr[1];
328  cameraPosition_Reference[2] = tempPtr[2];
329 }
330 
331 //-----------------------------------------------------------------------------
332 void vtkPlusPhantomLandmarkRegistrationAlgo::GetDefinedLandmark_Reference( int index, double cameraPosition_Reference[4] )
333 {
334  this->DefinedLandmarks_Phantom->GetPoint( index, cameraPosition_Reference );
335 
336  // store it in a temporary variable because MultiplyDoublePoint would overwrite the 4th component of cameraPosition_Reference, too
337  double* tempPtr = this->PhantomToReferenceTransformMatrix->MultiplyDoublePoint( cameraPosition_Reference );
338  cameraPosition_Reference[0] = tempPtr[0];
339  cameraPosition_Reference[1] = tempPtr[1];
340  cameraPosition_Reference[2] = tempPtr[2];
341 }
342 
343 //-----------------------------------------------------------------------------
345 {
346  // Defined landmarks from xml are in the phantom coordinate system
347  double recordedLandmarkPoint_Phantom[4] = {0, 0, 0, 1};
348  double* tempPtr;
349 
350  vtkSmartPointer<vtkMatrix4x4> referenceToPhantomTransformMatrix = vtkSmartPointer<vtkMatrix4x4>::New();
351  vtkMatrix4x4::Invert( this->PhantomToReferenceTransformMatrix, referenceToPhantomTransformMatrix );
352  int numberOfLandmarks = this->DefinedLandmarks_Phantom->GetNumberOfPoints();
353  for ( int landmarkIndex = 0; landmarkIndex < numberOfLandmarks; landmarkIndex++ )
354  {
355  this->RecordedLandmarks_Reference->GetPoint( landmarkIndex, recordedLandmarkPoint_Phantom );
356  // store it in a temporary variable because MultiplyDoublePoint would overwrite the 4th component of cameraPosition_Reference, too
357  tempPtr = referenceToPhantomTransformMatrix->MultiplyDoublePoint( recordedLandmarkPoint_Phantom );
358  LOG_INFO( "RecordedLandmark_Phantom " << landmarkIndex << " (" << tempPtr[0] << ", " << tempPtr[1] << ", " << tempPtr[2] << ")" );
359  }
360 }
void GetDefinedLandmark_Reference(int index, double cameraPosition_Reference[4])
igsioStatus PlusStatus
Definition: PlusCommon.h:40
virtual void SetRecordedLandmarks_Reference(vtkPoints *)
virtual void SetDefinedLandmarks_Phantom(vtkPoints *)
for i
static vtkPlusPhantomLandmarkRegistrationAlgo * New()
#define PLUS_FAIL
Definition: PlusCommon.h:43
vtkStandardNewMacro(vtkPlusPhantomLandmarkRegistrationAlgo)
void GetLandmarkCameraPosition_Reference(int index, double cameraPosition_Reference[4])
#define PLUS_SUCCESS
Definition: PlusCommon.h:44
void GetDefinedLandmarksCentroid_Reference(double landmarksAverage_Reference[4])
Landmark registration to determine the Phantom pose relative to the attached marker (PhantomReference...
PlusStatus LandmarkRegister(vtkIGSIOTransformRepository *aTransformRepository=NULL)
void GetDefinedLandmarksCentroid_Phantom(double landmarksCentroid_Phantom[4])
virtual void SetPhantomToReferenceTransformMatrix(vtkMatrix4x4 *)