CustusX  2023.01.05-dev+develop.0da12
An IGT application
cxCenterlineRegistration.cpp
Go to the documentation of this file.
1 /*=========================================================================
2 This file is part of CustusX, an Image Guided Therapy Application.
3 
4 Copyright (c) SINTEF Department of Medical Technology.
5 All rights reserved.
6 
7 CustusX is released under a BSD 3-Clause license.
8 
9 See Lisence.txt (https://github.com/SINTEFMedtek/CustusX/blob/master/License.txt) for details.
10 =========================================================================*/
12 #include <vtkPointData.h>
13 #include <vtkPolyData.h>
14 #include <vtkPolyDataWriter.h>
15 #include <vtkCellArray.h>
16 #include <vtkMatrix4x4.h>
17 #include <vtkLinearTransform.h>
18 #include <vtkLandmarkTransform.h>
19 #include "cxTransform3D.h"
20 #include "cxVector3D.h"
21 #include "cxLogger.h"
22 #include <boost/math/special_functions/fpclassify.hpp> // isnan
23 #include "vtkCardinalSpline.h"
24 
25 typedef vtkSmartPointer<class vtkCardinalSpline> vtkCardinalSplinePtr;
26 
27 namespace cx
28 {
29 
30 
32 {
33  mFixedPointSet = PointSetType::New();
34  mMovingPointSet = PointSetType::New();
35  mRegistration = RegistrationType::New();
36  mTransform = TransformType::New();
37  MetricType::Pointer metric = MetricType::New();
38  mOptimizer = OptimizerType::New();
39  mOptimizer->SetUseCostFunctionGradient(false);
40  OptimizerType::ScalesType scales(mTransform->GetNumberOfParameters());
41 
42  unsigned long numberOfIterations = 2000;
43  double gradientTolerance = 1e-4;
44  double valueTolerance = 1e-4;
45  double epsilonFunction = 1e-5;
46 
47  UpdateScales(true,true,true,true,true,true);
48  mOptimizer->SetNumberOfIterations(numberOfIterations);
49  mOptimizer->SetValueTolerance(valueTolerance);
50  mOptimizer->SetGradientTolerance(gradientTolerance);
51  mOptimizer->SetEpsilonFunction(epsilonFunction);
52 
53  // Initialize transform
54  mTransform->SetIdentity();
55  mRegistration->SetInitialTransformParameters(mTransform->GetParameters());
56 
57  // Setup framework
58  mRegistration->SetMetric(metric);
59  mRegistration->SetOptimizer(mOptimizer);
60  mRegistration->SetTransform(mTransform);
61 
62 }
63 
64 
65 
66 vtkPointsPtr convertTovtkPoints(Eigen::MatrixXd positions)
67 {
68  vtkPointsPtr retval = vtkPointsPtr::New();
69 
70  for (unsigned i=0; i<positions.cols(); ++i)
71  {
72  retval->InsertNextPoint(positions(0,i), positions(1,i), positions(2,i));
73  }
74  return retval;
75 }
76 
77 void CenterlineRegistration::UpdateScales(bool xRot, bool yRot, bool zRot, bool xTrans, bool yTrans, bool zTrans)
78 {
79 
80  OptimizerType::ScalesType scales(mTransform->GetNumberOfParameters());
81  const double translationScale = 40.0; // dynamic range of translations
82  const double rotationScale = 0.3; // dynamic range of rotations
83 
84  if (xRot)
85  scales[0] = 1.0 / rotationScale;
86  else
87  scales[0] = 1e50;
88  if (yRot)
89  scales[1] = 1.0 / rotationScale;
90  else
91  scales[1] = 1e50;
92  if (zRot)
93  scales[2] = 1.0 / rotationScale;
94  else
95  scales[2] = 1e50;
96  if (xTrans)
97  scales[3] = 1.0 / translationScale;
98  else
99  scales[3] = 1e50;
100  if (yTrans)
101  scales[4] = 1.0 / translationScale;
102  else
103  scales[4] = 1e50;
104  if (zTrans)
105  scales[5] = 1.0 / translationScale;
106  else
107  scales[5] = 1e50;
108 
109  mOptimizer->SetScales(scales);
110  mRegistration->SetOptimizer(mOptimizer);
111 
112  std::cout << "Update scales: "
113  << mRegistration->GetOptimizer()->GetScales()
114  << scales
115  << std::endl;
116 
117 }
118 
119 
121 {
122  int numberOfInputPoints = centerline->GetNumberOfPoints();
123  int controlPointFactor = 10;
124  int numberOfControlPoints = numberOfInputPoints / controlPointFactor;
125  if (numberOfControlPoints < 10)
126  numberOfControlPoints = std::min(numberOfInputPoints,10);
127 
128  int numberOfOutputPoints = std::max(numberOfInputPoints, 100); //interpolate to at least 100 output points
129  vtkPointsPtr newCenterline = vtkPointsPtr::New();;
130 
131  vtkCardinalSplinePtr splineX = vtkSmartPointer<vtkCardinalSpline>::New();
132  vtkCardinalSplinePtr splineY = vtkSmartPointer<vtkCardinalSpline>::New();
133  vtkCardinalSplinePtr splineZ = vtkSmartPointer<vtkCardinalSpline>::New();
134 
135  if (numberOfControlPoints >= 2)
136  {
137  //add control points to spline
138  for(int i=0; i<numberOfControlPoints; i++)
139  {
140  int indexP = (i*numberOfInputPoints)/numberOfControlPoints;
141  double p[3];
142  centerline->GetPoint(indexP,p);
143  int indexSpline = (i*numberOfOutputPoints)/numberOfControlPoints;
144  splineX->AddPoint( indexSpline, p[0] );
145  splineY->AddPoint( indexSpline, p[1] );
146  splineZ->AddPoint( indexSpline, p[2] );
147  //std::cout << "spline point: " << "(" << indexSpline << ") " << p[0] << " " << p[1] << " " << p[2] << std::endl;
148  }
149 
150  //Always add the last point to complete spline
151  double p[3];
152  centerline->GetPoint( numberOfInputPoints-1, p );
153  splineX->AddPoint( numberOfOutputPoints-1, p[0] );
154  splineY->AddPoint( numberOfOutputPoints-1, p[1] );
155  splineZ->AddPoint( numberOfOutputPoints-1, p[2] );
156  //std::cout << "spline point: " << "(" << numberOfOutputPoints-1 << ") " << p[0] << " " << p[1] << " " << p[2] << std::endl;
157 
158  //evaluate spline - get smoothed positions
159  for(int i=0; i<numberOfOutputPoints; i++)
160  {
161  double splineParameter = i;
162  newCenterline->InsertNextPoint(splineX->Evaluate(splineParameter),
163  splineY->Evaluate(splineParameter),
164  splineZ->Evaluate(splineParameter));
165  }
166  }
167  else
168  return centerline;
169 
170  return newCenterline;
171 }
172 
174 {
175 
176  vtkPointsPtr processedPositions = vtkPointsPtr::New();
177  int N = centerline->GetNumberOfPoints();
178  Eigen::MatrixXd CLpoints(3,N);
179  for(vtkIdType i = 0; i < N; i++)
180  {
181  double p[3];
182  centerline->GetPoint(i,p);
183  Vector3D pos;
184  pos(0) = p[0]; pos(1) = p[1]; pos(2) = p[2];
185  pos = rMd.coord(pos);
186  processedPositions->InsertNextPoint(pos[0],pos[1],pos[2]);
187  }
188 
189  //sort positions?
190  processedPositions = smoothPositions(processedPositions);
191 
192  return processedPositions;
193 }
194 
196 {
197  vtkPointsPtr loggedPositions = vtkPointsPtr::New();
198 
199  for(TimedTransformMap::iterator iter=trackingData_prMt.begin();
200  iter!=trackingData_prMt.end();++iter)
201  {
202  Transform3D prMt = iter->second;
203  Vector3D pos = prMt.matrix().block<3,1> (0, 3);
204  pos = rMpr.coord(pos);
205  loggedPositions->InsertNextPoint(pos[0],pos[1],pos[2]);
206  }
207 
208  return loggedPositions;
209 }
210 
212 {
213  mFixedPointSet->Initialize(); // Clear previous pointset
214 // PointsContainerPtr itkPoints = mFixedPointSet->GetPoints();
215  PointsContainerPtr itkPoints = PointsContainer::New();
216  PointType point;
217 
218  for(vtkIdType n=0;n<vtkPoints->GetNumberOfPoints();n++)
219  {
220  double temp_Point[3];
221  vtkPoints->GetPoint(n, temp_Point);
222  point[0] = temp_Point[0];
223  point[1] = temp_Point[1];
224  point[2] = temp_Point[2];
225  Vector3D vPoint(temp_Point[0], temp_Point[1], temp_Point[2]);
226  itkPoints->InsertElement(n,point);
227  }
228  mFixedPointSet->SetPoints(itkPoints);
229 }
230 
232 {
233  mMovingPointSet->Initialize();
234  PointsContainerPtr itkPoints = PointsContainer::New();
235  PointType point;
236 
237  for(vtkIdType n=0;n<vtkPoints->GetNumberOfPoints();n++)
238  {
239  double temp_Point[3];
240  vtkPoints->GetPoint(n, temp_Point);
241  Vector3D vPoint(temp_Point[0], temp_Point[1], temp_Point[2]);
242  point[0] = temp_Point[0];
243  point[1] = temp_Point[1];
244  point[2] = temp_Point[2];
245  itkPoints->InsertElement(n,point);
246  }
247  mMovingPointSet->SetPoints(itkPoints);
248 }
249 
251 {
252  mRegistration->SetFixedPointSet(mFixedPointSet);
253  mRegistration->SetMovingPointSet(mMovingPointSet);
254 
255  typedef itk::Matrix<double,3,3> MatrixType;
256  typedef TransformType::OffsetType TranslateVector;
257  MatrixType initMatrix;
258  TranslateVector initTranslation;
259 
260  // Set initial rotation
261  for(int i=0; i<3; i++)
262  for(int j=0;j<3;j++) {
263  double element = init_transform(i,j);
264  initMatrix(i,j) = element;
265  }
266 
267  // Set initial translation
268  initTranslation[0] = init_transform(0,3);
269  initTranslation[1] = init_transform(1,3);
270  initTranslation[2] = init_transform(2,3);
271 
272  mTransform->SetMatrix(initMatrix, 1e-5);
273  mTransform->SetTranslation(initTranslation);
274 
275  mRegistration->SetInitialTransformParameters(mTransform->GetParameters());
276 
277  try
278  {
279  mRegistration->Update();
280  }
281  catch (itk::ExceptionObject &exp)
282  {
283  std::cout << "CenterlineRegMethod - Exception caught ! " << std::endl;
284  std::cout << exp << std::endl;
285  }
286 
287  RegistrationType::ParametersType finalParameters =
288  mRegistration->GetLastTransformParameters();
289 
290  TransformType::MatrixType regMatrix = mTransform->GetMatrix();
291 
292  for(int i=0; i<3; i++)
293  for(int j=0;j<3;j++) {
294  double element = regMatrix(i,j);
295  mResultTransform(i,j) = element;
296  }
297 
298  mResultTransform(0,3) = finalParameters(3);
299  mResultTransform(1,3) = finalParameters(4);
300  mResultTransform(2,3) = finalParameters(5);
301 
302  mRegistrationUpdated = true;
303 
304  return Transform3D(mResultTransform);
305 }
306 
307 
309 {
310 
311  vtkPointsPtr centerlinePoints = processCenterline(centerline, rMd);
312 
313  SetFixedPoints( centerlinePoints );
314  SetMovingPoints( ConvertTrackingDataToVTK(trackingData_prMt, old_rMpr) );
315 
316  Transform3D rMpr = FullRegisterMoving(Transform3D::Identity());
317 
318  std::cout << "rMpr : " << std::endl;
319  std::cout << rMpr << std::endl;
320 
321  return rMpr;
322 }
323 
325 {
326 
327 }
328 
329 
330 
331 }//namespace cx
vtkPointsPtr processCenterline(vtkPolyDataPtr centerline, Transform3D rMd)
vtkPointsPtr ConvertTrackingDataToVTK(TimedTransformMap trackingData_prMt, Transform3D rMpr)
PointSetType::PointType PointType
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
void SetMovingPoints(vtkPointsPtr points)
vtkSmartPointer< vtkPoints > vtkPointsPtr
Transform3D runCenterlineRegistration(vtkPolyDataPtr centerline, Transform3D rMd, TimedTransformMap trackingData_prMt, Transform3D old_rMpr)
vtkSmartPointer< class vtkCardinalSpline > vtkCardinalSplinePtr
Definition: cxAccusurf.cpp:17
vtkPointsPtr smoothPositions(vtkPointsPtr centerline)
vtkPointsPtr convertTovtkPoints(Eigen::MatrixXd positions)
vtkSmartPointer< class vtkCardinalSpline > vtkCardinalSplinePtr
Transform3D FullRegisterMoving(Transform3D init_transform)
vtkSmartPointer< vtkPolyData > vtkPolyDataPtr
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:42
PointSetType::PointsContainerPointer PointsContainerPtr
void UpdateScales(bool xRot, bool yRot, bool zRot, bool xTrans, bool yTrans, bool zTrans)
void SetFixedPoints(vtkPointsPtr points)
std::map< double, Transform3D > TimedTransformMap
Namespace for all CustusX production code.