PlusLib  2.9.0
Software library for tracked ultrasound image acquisition, calibration, and processing.
vtkPlusProbeCalibrationOptimizerAlgo.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 
9 #include "PlusMath.h"
10 
11 #include "vtkObjectFactory.h"
12 #include "vtkMath.h"
15 #include "vtkTransform.h"
16 #include "vtkIGSIOTransformRepository.h"
17 #include "vtkXMLUtilities.h"
18 
19 #include "vtksys/SystemTools.hxx"
20 
21 #include "itkPowellOptimizer.h"
22 #include "itkScaleVersor3DTransform.h"
23 #include "itkSimilarity3DTransform.h"
24 
25 typedef itk::PowellOptimizer OptimizerType;
26 
27 //-----------------------------------------------------------------------------
28 class DistanceToWiresCostFunction : public itk::SingleValuedCostFunction
29 {
30 public:
31 
32  typedef DistanceToWiresCostFunction Self;
33  typedef itk::SingleValuedCostFunction Superclass;
34  typedef itk::SmartPointer<Self> Pointer;
35  typedef itk::SmartPointer<const Self> ConstPointer;
36  itkNewMacro( Self );
37  itkTypeMacro( DistanceToWiresCostFunction, SingleValuedCostFunction );
38 
39  typedef Superclass::ParametersType ParametersType;
40  typedef Superclass::DerivativeType DerivativeType;
41  typedef itk::VersorRigid3DTransform< double > RigidTransformType;
42 
43  DistanceToWiresCostFunction()
44  : m_CalibrationOptimizer(NULL)
45  {
46  }
47 
48  DistanceToWiresCostFunction(vtkPlusProbeCalibrationOptimizerAlgo* calibrationOptimizer)
49  {
50  m_CalibrationOptimizer=calibrationOptimizer;
51  }
52 
53  static PlusStatus GetTransformMatrix(vnl_matrix_fixed<double,4,4>& imageToProbeTransform_vnl, const ParametersType & imageToProbeTransformParameters)
54  {
55  imageToProbeTransform_vnl.set_identity();
56 
57  if (imageToProbeTransformParameters.GetSize()!=7 && imageToProbeTransformParameters.GetSize()!=8)
58  {
59  LOG_ERROR("GetTransformMatrix expects 7 or 8 parameters");
60  return PLUS_FAIL;
61  }
62 
63  auto rigidTransform=RigidTransformType::New();
64  rigidTransform->SetParameters(imageToProbeTransformParameters);
65  imageToProbeTransform_vnl.update(rigidTransform->GetMatrix().GetVnlMatrix());
66  imageToProbeTransform_vnl.put(0,3,rigidTransform->GetOffset()[0]);
67  imageToProbeTransform_vnl.put(1,3,rigidTransform->GetOffset()[1]);
68  imageToProbeTransform_vnl.put(2,3,rigidTransform->GetOffset()[2]);
69 
70  bool isotropicPixelSpacing=(imageToProbeTransformParameters.GetSize()==7);
71  if (isotropicPixelSpacing)
72  {
73  // X and Y pixel spacing are the same
74  imageToProbeTransform_vnl.scale_column(0,imageToProbeTransformParameters(6));
75  imageToProbeTransform_vnl.scale_column(1,imageToProbeTransformParameters(6));
76  imageToProbeTransform_vnl.scale_column(2,imageToProbeTransformParameters(6));
77  }
78  else
79  {
80  // X and Y pixel spacing are different
81  imageToProbeTransform_vnl.scale_column(0,imageToProbeTransformParameters(6));
82  imageToProbeTransform_vnl.scale_column(1,imageToProbeTransformParameters(7));
83  imageToProbeTransform_vnl.scale_column(2,(imageToProbeTransformParameters(6)+imageToProbeTransformParameters(7))/2);
84  }
85  return PLUS_SUCCESS;
86  }
87 
88  static PlusStatus GetTransformParameters(ParametersType& imageToProbeTransformParameters, const vnl_matrix_fixed<double,4,4>& imageToProbeTransform_vnl)
89  {
90  if (imageToProbeTransformParameters.GetSize()!=7 && imageToProbeTransformParameters.GetSize()!=8)
91  {
92  LOG_ERROR("GetTransformMatrix expects 7 or 8 parameters");
93  return PLUS_FAIL;
94  }
95 
96  // Decompose the initial calibration matrix to an orthogonal transformation matrix, scaling vector, and translation vector
97  vnl_matrix_fixed<double,3,3> rotationMatrix= imageToProbeTransform_vnl.extract(3,3);
98  vnl_svd<double> svd(rotationMatrix);
99  itk::Matrix<double,3,3> orthogonalizedRotationMatrix;
100  orthogonalizedRotationMatrix = svd.U() * svd.V().transpose();
101  double scale[3] = { svd.W(0), svd.W(1), svd.W(2) };
102 
103  // Rotation versor (unit quaternion parameters)
104  auto imageToProbeTransformRigid = RigidTransformType::New();
105  imageToProbeTransformRigid->SetMatrix(orthogonalizedRotationMatrix );
106  RigidTransformType::ParametersType rigidParameters=imageToProbeTransformRigid->GetParameters();
107  imageToProbeTransformParameters[0]=rigidParameters[0];
108  imageToProbeTransformParameters[1]=rigidParameters[1];
109  imageToProbeTransformParameters[2]=rigidParameters[2];
110 
111  // Translation
112  imageToProbeTransformParameters[3]=imageToProbeTransform_vnl.get(0,3);
113  imageToProbeTransformParameters[4]=imageToProbeTransform_vnl.get(1,3);
114  imageToProbeTransformParameters[5]=imageToProbeTransform_vnl.get(2,3);
115 
116  // Scaling
117  bool isotropicPixelSpacing=(imageToProbeTransformParameters.GetSize()==7);
118  if (isotropicPixelSpacing)
119  {
120  imageToProbeTransformParameters[6]=(scale[0]+scale[1])/2.0;
121  }
122  else
123  {
124  imageToProbeTransformParameters[6]=scale[0];
125  imageToProbeTransformParameters[7]=scale[1];
126  }
127  return PLUS_SUCCESS;
128  }
129 
130  double GetValue( const ParametersType & imageToProbeTransformParameters ) const
131  {
132  vnl_matrix_fixed<double,4,4> imageToProbeTransform_vnl;
133  GetTransformMatrix(imageToProbeTransform_vnl, imageToProbeTransformParameters);
134  double errorMean=0.0;
135  double errorStDev=0.0;
136  double errorRms=0.0;
137  m_CalibrationOptimizer->ComputeError(imageToProbeTransform_vnl, errorMean, errorStDev, errorRms);
138  return errorRms;
139  }
140 
141  void GetDerivative( const ParametersType & parameters, DerivativeType & derivative ) const
142  {
143  LOG_ERROR("GetDerivative is not implemented");
144  }
145 
146  unsigned int GetNumberOfParameters(void) const
147  {
148  if (m_CalibrationOptimizer->GetIsotropicPixelSpacing())
149  {
150  return 7; // 3 rotation + 3 translation + 1 scaling
151  }
152  else
153  {
154  return 8; // 3 rotation + 3 translation + 2 scaling
155  }
156  }
157 
158 private:
159  vtkPlusProbeCalibrationOptimizerAlgo* m_CalibrationOptimizer;
160 };
161 
162 //-----------------------------------------------------------------------------
164 
165 //-----------------------------------------------------------------------------
167 : IsotropicPixelSpacing(true)
168 , ProbeCalibrationAlgo(NULL)
169 {
170 }
171 
172 //-----------------------------------------------------------------------------
174 {
175 }
176 
177 //-----------------------------------------------------------------------------
179 {
180  this->ProbeCalibrationAlgo=probeCalibrationAlgo;
181 }
182 
183 //--------------------------------------------------------------------------------
184 void vtkPlusProbeCalibrationOptimizerAlgo::ComputeError(const vnl_matrix_fixed<double,4,4> &imageToProbeTransformationMatrix, double &errorMean, double &errorStDev, double &errorRms)
185 {
186  switch (this->OptimizationMethod)
187  {
189  this->ProbeCalibrationAlgo->ComputeError3d(imageToProbeTransformationMatrix, errorMean, errorStDev, errorRms);
190  break;
192  this->ProbeCalibrationAlgo->ComputeError2d(imageToProbeTransformationMatrix, errorMean, errorStDev, errorRms);
193  break;
194  default:
195  LOG_ERROR("Invalid cost function");
196  }
197 }
198 
199 //-----------------------------------------------------------------------------
200 PlusStatus vtkPlusProbeCalibrationOptimizerAlgo::ShowTransformation(const vnl_matrix_fixed<double,4,4> &imageToProbeTransformationMatrix)
201 {
202  LOG_INFO("Translation = [" << imageToProbeTransformationMatrix.get(0,3) << " " << imageToProbeTransformationMatrix.get(1,3) << " " << imageToProbeTransformationMatrix.get(2,3) << " ]");
203 
204  vnl_matrix_fixed<double,3,3> rotationMatrix=imageToProbeTransformationMatrix.extract(3,3);
205  vnl_svd<double> svd(rotationMatrix);
206  vnl_matrix<double> orthogonalizedRotationMatrix;
207  orthogonalizedRotationMatrix = svd.U() * svd.V().transpose();
208  double scale[3] = { svd.W(0), svd.W(1), svd.W(2) };
209  LOG_INFO("Scale = [" << scale[0] << " " << scale[1] << " " << scale[2] << " ]");
210 
211  vnl_vector<double> xAxis=imageToProbeTransformationMatrix.get_column(0);
212  xAxis.normalize();
213  vnl_vector<double> yAxis=imageToProbeTransformationMatrix.get_column(1);
214  yAxis.normalize();
215  double xyAxesAngleDeg=vtkMath::DegreesFromRadians(acos(dot_product(xAxis,yAxis)));
216  LOG_INFO("XY axes angle = " << xyAxesAngleDeg << " deg");
217 
218  double errorMean=0.0;
219  double errorStDev=0.0;
220  double errorRms=0.0;
221  ComputeError(imageToProbeTransformationMatrix, errorMean, errorStDev, errorRms);
222  LOG_INFO("Error (mm): mean=" << errorMean<< ", stdev="<<errorStDev<<", rms="<<errorRms);
223 
224  return PLUS_SUCCESS;
225 }
226 
227 //----------------------------------------------------------------------------
229 {
230  DistanceToWiresCostFunction::Pointer costFunction = new DistanceToWiresCostFunction(this);
231 
232  DistanceToWiresCostFunction::ParametersType imageToProbeSeedTransformParameters(costFunction->GetNumberOfParameters());
233  DistanceToWiresCostFunction::GetTransformParameters(imageToProbeSeedTransformParameters, this->ImageToProbeSeedTransformMatrix);
234 
235  double errorMean=0.0;
236  double errorStDev=0.0;
237  double errorRms=0.0;
238  ComputeError(this->ImageToProbeSeedTransformMatrix, errorMean, errorStDev, errorRms);
239  LOG_INFO("Initial cost function value with unconstrained matrix = " << errorRms );
240  {
241  vtkSmartPointer<vtkMatrix4x4> vtkMatrix=vtkSmartPointer<vtkMatrix4x4>::New();
243  igsioMath::LogVtkMatrix(vtkMatrix);
244  }
245 
246  double initialError=costFunction->GetValue(imageToProbeSeedTransformParameters);
247  LOG_INFO("Initial cost function value with constrained matrix= " << initialError );
248  {
249  vnl_matrix_fixed<double,4,4> imageToProbeTransform_vnl;
250  DistanceToWiresCostFunction::GetTransformMatrix(imageToProbeTransform_vnl, imageToProbeSeedTransformParameters);
251  vtkSmartPointer<vtkMatrix4x4> vtkMatrix=vtkSmartPointer<vtkMatrix4x4>::New();
252  PlusMath::ConvertVnlMatrixToVtkMatrix(imageToProbeTransform_vnl, vtkMatrix);
253  igsioMath::LogVtkMatrix(vtkMatrix);
254  }
255 
256  auto optimizer = OptimizerType::New();
257  try
258  {
259  optimizer->SetCostFunction( costFunction.GetPointer() );
260  }
261  catch( itk::ExceptionObject & e )
262  {
263  LOG_ERROR("Exception thrown ! An error ocurred during Optimization: "<<e);
264  return PLUS_FAIL;
265  }
266 
267  optimizer->SetStepLength( 10 );
268  optimizer->SetStepTolerance( 1e-8 );
269  optimizer->SetValueTolerance( 1e-8 );
270  optimizer->SetMaximumIteration( 300 );
271 
272 
273  const double rotationParametersScale=1.0;
274  const double translationParametersScale=0.5;
275  const double scalesParametersScale=10.0;
276 
277  // Scale the translation components of the transform in the Optimizer
278  OptimizerType::ScalesType scales( costFunction->GetNumberOfParameters() );
279  switch (costFunction->GetNumberOfParameters())
280  {
281  case 7:
282  scales[0] = rotationParametersScale;
283  scales[1] = rotationParametersScale;
284  scales[2] = rotationParametersScale;
285  scales[3] = translationParametersScale;
286  scales[4] = translationParametersScale;
287  scales[5] = translationParametersScale;
288  scales[6] = scalesParametersScale;
289  break;
290  case 8:
291  scales[0] = rotationParametersScale;
292  scales[1] = rotationParametersScale;
293  scales[2] = rotationParametersScale;
294  scales[3] = translationParametersScale;
295  scales[4] = translationParametersScale;
296  scales[5] = translationParametersScale;
297  scales[6] = scalesParametersScale;
298  scales[7] = scalesParametersScale;
299  break;
300  default:
301  LOG_ERROR("Number of transformation parameters is incorrect");
302  return PLUS_FAIL;
303  }
304  optimizer->SetScales(scales);
305 
306  optimizer->SetInitialPosition(imageToProbeSeedTransformParameters);
307 
308  try
309  {
310  optimizer->StartOptimization();
311  }
312  catch( itk::ExceptionObject & e )
313  {
314  LOG_ERROR("Exception thrown ! An error ocurred during Optimization: Location = " << e.GetLocation() << "Description = " << e.GetDescription());
315  return PLUS_FAIL;
316  }
317 
318  std::string stopCondition=optimizer->GetStopConditionDescription();
319  LOG_INFO("Optimization stopping condition: "<<stopCondition<<". Number of iterations: " << optimizer->GetCurrentIteration());
320 
321  // Store the matrix
322 
323  costFunction->GetTransformMatrix(this->ImageToProbeTransformMatrix, optimizer->GetCurrentPosition());
324  {
325  vtkSmartPointer<vtkMatrix4x4> vtkMatrix=vtkSmartPointer<vtkMatrix4x4>::New();
327  igsioMath::LogVtkMatrix(vtkMatrix);
328  }
329 
330  // Store the optimized parameters and show the results
331  LOG_INFO("Cost function = " << GetOptimizationMethodAsString(this->OptimizationMethod));
332 
333  LOG_INFO("Without optimization:");
335 
336  LOG_INFO("With optimization:");
338 
339  vtkSmartPointer<vtkMatrix4x4> imageToProbeSeedTransformMatrixVtk = vtkSmartPointer<vtkMatrix4x4>::New();
340  vtkSmartPointer<vtkMatrix4x4> imageToProbeTransformMatrixVtk = vtkSmartPointer<vtkMatrix4x4>::New();
341  PlusMath::ConvertVnlMatrixToVtkMatrix(this->ImageToProbeSeedTransformMatrix,imageToProbeSeedTransformMatrixVtk);
342  PlusMath::ConvertVnlMatrixToVtkMatrix(this->ImageToProbeTransformMatrix,imageToProbeTransformMatrixVtk);
343  double angleDifference = igsioMath::GetOrientationDifference(imageToProbeSeedTransformMatrixVtk, imageToProbeTransformMatrixVtk);
344  LOG_INFO("Orientation difference between unoptimized and optimized matrices = " << angleDifference << " deg");
345 
346  return PLUS_SUCCESS;
347 }
348 
349 //----------------------------------------------------------------------------
351 {
352  return this->ImageToProbeTransformMatrix;
353 }
354 
355 //----------------------------------------------------------------------------
356 void vtkPlusProbeCalibrationOptimizerAlgo::SetImageToProbeSeedTransform(const vnl_matrix_fixed<double,4,4> &imageToProbeTransformMatrix)
357 {
358  this->ImageToProbeSeedTransformMatrix=imageToProbeTransformMatrix;
359 }
360 
361 
362 //----------------------------------------------------------------------------
364 {
366  {
367  return false;
368  }
369  return true;
370 }
371 
372 //----------------------------------------------------------------------------
374 {
375  switch (type)
376  {
377  case MINIMIZE_NONE: return "NONE";
378  case MINIMIZE_DISTANCE_OF_MIDDLE_WIRES_IN_3D: return "3D";
379  case MINIMIZE_DISTANCE_OF_ALL_WIRES_IN_2D: return "2D";
380  default:
381  LOG_ERROR("Unknown cost function to perform calibration: "<<type);
382  return "unknown";
383  }
384 }
385 
386 //----------------------------------------------------------------------------
388 {
389  LOG_TRACE("vtkPlusProbeCalibrationOptimizerAlgo::ReadConfiguration");
390  if ( aConfig == NULL )
391  {
392  LOG_ERROR("Unable to read configuration");
393  return PLUS_FAIL;
394  }
395 
396  // vtkPlusProbeCalibrationOptimizerAlgo section
397  const char* optimizationMethod=aConfig->GetAttribute("OptimizationMethod");
398  if (optimizationMethod==NULL)
399  {
400  // no optimization method is defined, assume no optimization is needed
402  }
403  else if (STRCASECMP(optimizationMethod, GetOptimizationMethodAsString(MINIMIZE_NONE)) == 0)
404  {
406  }
407  else if (STRCASECMP(optimizationMethod, GetOptimizationMethodAsString(MINIMIZE_DISTANCE_OF_MIDDLE_WIRES_IN_3D)) == 0)
408  {
410  }
411  else if (STRCASECMP(optimizationMethod, GetOptimizationMethodAsString(MINIMIZE_DISTANCE_OF_ALL_WIRES_IN_2D)) == 0)
412  {
414  }
415 
416  XML_READ_BOOL_ATTRIBUTE_OPTIONAL(IsotropicPixelSpacing, aConfig);
417 
418  return PLUS_SUCCESS;
419 }
Refines the image to probe transform using non-linear optimization.
void ComputeError2d(const vnl_matrix_fixed< double, 4, 4 > &imageToProbeMatrix, double &errorMean, double &errorStDev, double &errorRms)
static const char * GetOptimizationMethodAsString(OptimizationMethodType type)
igsioStatus PlusStatus
Definition: PlusCommon.h:40
void SetImageToProbeSeedTransform(const vnl_matrix_fixed< double, 4, 4 > &imageToProbeTransformMatrix)
void SetProbeCalibrationAlgo(vtkPlusProbeCalibrationAlgo *probeCalibrationAlgo)
#define PLUS_FAIL
Definition: PlusCommon.h:43
PlusStatus ShowTransformation(const vnl_matrix_fixed< double, 4, 4 > &transformationMatrix)
vnl_matrix_fixed< double, 4, 4 > GetOptimizedImageToProbeTransformMatrix()
#define PLUS_SUCCESS
Definition: PlusCommon.h:44
vtkStandardNewMacro(vtkPlusProbeCalibrationOptimizerAlgo)
void ComputeError(const vnl_matrix_fixed< double, 4, 4 > &imageToProbeTransformationMatrix, double &errorMean, double &errorStDev, double &errorRms)
itk::PowellOptimizer OptimizerType
vnl_matrix_fixed< double, 4, 4 > ImageToProbeTransformMatrix
void ComputeError3d(const vnl_matrix_fixed< double, 4, 4 > &imageToProbeMatrix, double &errorMean, double &errorStDev, double &errorRms)
vnl_matrix_fixed< double, 4, 4 > ImageToProbeSeedTransformMatrix
PlusStatus ReadConfiguration(vtkXMLDataElement *aConfig)
static void ConvertVnlMatrixToVtkMatrix(const vnl_matrix_fixed< double, 4, 4 > &inVnlMatrix, vtkMatrix4x4 *outVtkMatrix)
Definition: PlusMath.cxx:361
Probe calibration algorithm class.