CustusX  22.04-rc5
An IGT application
cxBronchoscopyRegistration.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 
24 namespace cx
25 {
26 
27 
29  mCenterlineProcessed(false),
30  mBranchListPtr(new BranchList)
31 {
32 
33 }
34 
36 // To avoid having too many tracking data samples in the registration,
37 // we are only including tracking data with a distance to last included
38 // tracking position of more than 1 mm.
39 {
40  M4Vector TnavigationIncluded;
41  if(Tnavigation.empty())
42  return TnavigationIncluded;
43  TnavigationIncluded.push_back(Tnavigation[0]); // first position is always included
44  int numberOfIncluded = 0;
45  size_t numberOfPos = Tnavigation.size();
46  for ( size_t index = 1; index < numberOfPos; index++)
47  {
48  double xDistance = (TnavigationIncluded[numberOfIncluded](0,3) - Tnavigation[index](0,3) );
49  double xDistanceSquared = xDistance * xDistance;
50  double yDistance = (TnavigationIncluded[numberOfIncluded](1,3) - Tnavigation[index](1,3) );
51  double yDistanceSquared = yDistance * yDistance;
52  double zDistance = (TnavigationIncluded[numberOfIncluded](2,3) - Tnavigation[index](2,3) );
53  double zDistanceSquared = zDistance * zDistance;
54  double distanceToLastIncluded = sqrt (xDistanceSquared + yDistanceSquared + zDistanceSquared);
55 
56  if (distanceToLastIncluded > 1) //condition of at least movement of 1 mm from last included sample
57  {
58  numberOfIncluded ++;
59  TnavigationIncluded.push_back(Tnavigation[index]);
60  }
61  }
62 
63  return TnavigationIncluded;
64 }
65 
66 
67 
68 Eigen::VectorXd sortVector(Eigen::VectorXd v)
69 {
70  for (int i = 0; i < v.size() - 1; i++) {
71  for (int j = i + 1; j < v.size(); j++) {
72  if (v(i) > v(j)){
73  std::swap(v(i) , v(j));
74  }
75  }
76  }
77  return v;
78 }
79 
80 
81 Eigen::VectorXd findMedian(Eigen::MatrixXd matrix)
82 {
83  Eigen::VectorXd medianValues(matrix.rows());
84  for (int i = 0; i < matrix.rows(); i++) {
85  Eigen::MatrixXd sortedMatrix = sortMatrix(i, matrix);
86  if (sortedMatrix.cols()%2==1) {// odd number
87  medianValues(i) = (sortedMatrix(i,(sortedMatrix.cols()+1)/2) );
88  }
89  else { // even number
90  medianValues(i) = ( sortedMatrix(i,sortedMatrix.cols()/2) + sortedMatrix(i,sortedMatrix.cols()/2 - 1) ) / 2;
91  }
92  }
93  return medianValues;
94 }
95 
96 
97 std::pair<Eigen::MatrixXd , Eigen::MatrixXd> findPositionsWithSmallesAngleDifference(int percentage , Eigen::VectorXd DAngle , Eigen::MatrixXd trackingPositions , Eigen::MatrixXd nearestCTPositions)
98 {
99  Eigen::VectorXd DAngleSorted = sortVector(DAngle);
100  int numberOfPositionsIncluded = floor((double)(DAngle.size() * percentage/100));
101  Eigen::MatrixXd trackingPositionsIncluded(3 , numberOfPositionsIncluded );
102  Eigen::MatrixXd nearestCTPositionsIncluded(3 , numberOfPositionsIncluded );
103  float maxDAngle = DAngleSorted( numberOfPositionsIncluded );
104  int counter = 0;
105  for (int i = 0; i < DAngle.size(); i++)
106  {
107  if ((DAngle(i) <= maxDAngle) && (counter < numberOfPositionsIncluded))
108  {
109  trackingPositionsIncluded.col(counter) = trackingPositions.col(i);
110  nearestCTPositionsIncluded.col(counter) = nearestCTPositions.col(i);
111  counter++;
112  }
113  }
114 
115  return std::make_pair(trackingPositionsIncluded , nearestCTPositionsIncluded);
116 }
117 
118 
119 vtkPointsPtr convertTovtkPoints(Eigen::MatrixXd positions)
120 {
121  vtkPointsPtr retval = vtkPointsPtr::New();
122 
123  for (unsigned i=0; i<positions.cols(); ++i)
124  {
125  retval->InsertNextPoint(positions(0,i), positions(1,i), positions(2,i));
126  }
127  return retval;
128 }
129 
133 Eigen::Matrix4d performLandmarkRegistration(vtkPointsPtr source, vtkPointsPtr target /*, bool* ok */)
134 {
135  //*ok = false;
136 
137  // too few data samples: ignore
138  if (source->GetNumberOfPoints() < 3)
139  {
140  std::cout << "Warning in performLandmarkRegistration: Need >3 positions, returning identity matrix." << std::endl;
141  return Eigen::Matrix4d::Identity();
142  }
143 
144  vtkLandmarkTransformPtr landmarktransform = vtkLandmarkTransformPtr::New();
145  landmarktransform->SetSourceLandmarks(source);
146  landmarktransform->SetTargetLandmarks(target);
147  landmarktransform->SetModeToRigidBody();
148  source->Modified();
149  target->Modified();
150  //landmarktransform->Update();
151 
152  vtkMatrix4x4* temp = landmarktransform->GetMatrix();
153  Eigen::Matrix4d tar_M_src;
154  for (int i = 0; i < 4; i++)
155  {
156  for (int j = 0; j < 4; j++)
157  {
158  tar_M_src(i,j) = temp->GetElement(i,j);
159  }
160  }
161 
162  if ( boost::math::isnan(tar_M_src.sum()) )
163  {
164  std::cout << "Warning in performLandmarkRegistration: Returning identity matrix due to nan." << std::endl;
165  return Eigen::Matrix4d::Identity();
166  }
167 
168  return tar_M_src;
169 }
170 
171 
172 
173 
174 std::vector<Eigen::MatrixXd::Index> dsearch2n(Eigen::MatrixXd pos1, Eigen::MatrixXd pos2, Eigen::MatrixXd ori1, Eigen::MatrixXd ori2)
175 {
176  Eigen::MatrixXd::Index index;
177  std::vector<Eigen::MatrixXd::Index> indexVector;
178 
179  for (int i = 0; i < pos1.cols(); i++)
180  {
181  Eigen::VectorXd D(pos2.cols());
182  Eigen::VectorXd P(pos2.cols());
183  Eigen::VectorXd O(pos2.cols());
184  Eigen::VectorXd R(pos2.cols());
185 
186  for (int j = 0; j < pos2.cols(); j++)
187  {
188  float p0 = ( pos2(0,j) - pos1(0,i) );
189  float p1 = ( pos2(1,j) - pos1(1,i) );
190  float p2 = ( pos2(2,j) - pos1(2,i) );
191  float o0 = fmod( ori2(0,j) - ori1(0,i) , 2 );
192  float o1 = fmod( ori2(1,j) - ori1(1,i) , 2 );
193  float o2 = fmod( ori2(2,j) - ori1(2,i) , 2 );
194 
195  P(j) = sqrt( p0*p0 + p1*p1 + p2*p2 );
196  O(j) = sqrt( o0*o0 + o1*o1 + o2*o2 );
197 
198  if (boost::math::isnan( O(j) ))
199  O(j) = 4;
200 
201  if ( (o0>2) || (o1>2) || (o2>2) )
202  std::cout << "Warning in bronchoscopyRegistration.cpp: Error on oriantation calculation in dsearch2n. Orientation > 2." << std::endl;
203 
204  R(j) = P(j) / O(j);
205 
206  }
207  float alpha = sqrt( R.mean() );
208  if (boost::math::isnan( alpha ))
209  alpha = 0;
210 
211  D = P + alpha * O;
212  D.minCoeff(&index);
213  indexVector.push_back(index);
214  }
215  return indexVector;
216 }
217 
218 std::pair<Eigen::MatrixXd , Eigen::MatrixXd> RemoveInvalidData(Eigen::MatrixXd positionData, Eigen::MatrixXd orientationData)
219 {
220  std::vector<int> indicesToBeDeleted;
221 
222  for (int i = 0; i < fmin(positionData.cols(), orientationData.cols()); i++)
223  {
224  if ( boost::math::isinf( positionData.block(0 , i , 3 , 1).sum() ) || boost::math::isinf( orientationData.block(0 , i , 3 , 1).sum() ) )
225  indicesToBeDeleted.push_back(i);
226  else if (boost::math::isnan( positionData.block(0 , i , 3 , 1).sum() ) || boost::math::isnan( orientationData.block(0 , i , 3 , 1).sum() ))
227  indicesToBeDeleted.push_back(i);
228  else if ( (positionData.block(0 , i , 1 , 1).sum() == 0 && positionData.block(1 , i , 1 , 1).sum() == 0 && positionData.block(2 , i , 1 , 1).sum() == 0) ||
229  (orientationData.block(0 , i , 1 , 1).sum() == 0 && orientationData.block(1 , i , 1 , 1).sum() == 0 && orientationData.block(2 , i , 1 , 1).sum() == 0))
230  indicesToBeDeleted.push_back(i);
231  }
232 
233  for ( int i = indicesToBeDeleted.size() - 1; i >= 0; i-- )
234  {
235  std::cout << "Warning in bronchoscopyRegistration: Removed corrupted data: " << positionData.block(0 , indicesToBeDeleted[i] , 3 , 1) << " " << orientationData.block(0 , indicesToBeDeleted[i] , 3 , 1) << std::endl;
236  positionData = eraseCol(indicesToBeDeleted[i],positionData);
237  orientationData = eraseCol(indicesToBeDeleted[i],orientationData);
238  }
239  return std::make_pair(positionData , orientationData);
240 }
241 
243 {
244  Eigen::Vector3d position;
245  Eigen::Vector3d orientation;
246  std::vector<int> indicesToBeDeleted;
247 
248  for (int i = 0; i < T_vector.size(); i++)
249  {
250  position = T_vector[i].topRightCorner(3 , 1);
251  orientation = T_vector[i].block(0 , 2 , 3 , 1);
252  if ( boost::math::isinf( position.sum() ) || boost::math::isinf( orientation.sum() ) )
253  indicesToBeDeleted.push_back(i);
254  else if (boost::math::isnan( position.sum() ) || boost::math::isnan( orientation.sum() ))
255  indicesToBeDeleted.push_back(i);
256  else if ( (position[0] == 0 && position[1] == 0 && position[2] == 0) ||
257  (orientation[0] == 0 && orientation[1] == 0 && orientation[2] == 0) )
258  indicesToBeDeleted.push_back(i);
259  }
260 
261  for ( int i = indicesToBeDeleted.size() - 1; i >= 0; i-- )
262  {
263  std::cout << "Warning in bronchoscopyRegistration: Removed corrupted data: " << T_vector[i].topRightCorner(3 , 1) << " " << T_vector[i].block(0 , 2 , 3 , 1) << std::endl;
264  T_vector.erase(T_vector.begin() + indicesToBeDeleted[i]);
265  }
266  return T_vector;
267 }
268 
269 Eigen::Matrix4d registrationAlgorithm(BranchListPtr branches, M4Vector Tnavigation, Transform3D old_rMpr)
270 {
271  Eigen::Matrix4d registrationMatrix;
272  Eigen::MatrixXd CTPositions;
273  Eigen::MatrixXd CTOrientations;
274  Eigen::MatrixXd trackingPositions(3 , Tnavigation.size());
275  Eigen::MatrixXd trackingOrientations(3, Tnavigation.size());
276 
277  std::vector<BranchPtr> branchVector = branches->getBranches();
278  CTPositions = branchVector[0]->getPositions();
279  CTOrientations = branchVector[0]->getOrientations();
280 
281  if (trackingPositions.cols() < 10)
282  {
283  std::cout << "Warning: Too few positions in tracking data to perform registration." << std::endl;
284  return Eigen::Matrix4d::Identity();
285  }
286 
287  for (int i = 1; i < branchVector.size(); i++)
288  {
289  Eigen::MatrixXd CTPositionsNew(CTPositions.rows() , CTPositions.cols() + branchVector[i]->getPositions().cols());
290  Eigen::MatrixXd CTOrientationsNew(CTOrientations.rows() , CTOrientations.cols() + branchVector[i]->getOrientations().cols());
291  CTPositionsNew.leftCols(CTPositions.cols()) = CTPositions;
292  CTPositionsNew.rightCols(branchVector[i]->getPositions().cols()) = branchVector[i]->getPositions();
293  CTOrientationsNew.leftCols(CTOrientations.cols()) = CTOrientations;
294  CTOrientationsNew.rightCols(branchVector[i]->getOrientations().cols()) = branchVector[i]->getOrientations();
295  CTPositions.swap(CTPositionsNew);
296  CTOrientations.swap(CTOrientationsNew);
297  }
298 
299  if (CTPositions.cols() < 10)
300  {
301  std::cout << "Warning: Too few positions in centerline to perform registration." << std::endl;
302  return Eigen::Matrix4d::Identity();
303  }
304 
305  std::pair<Eigen::MatrixXd , Eigen::MatrixXd> qualityCheckedData = RemoveInvalidData(CTPositions, CTOrientations);
306  CTPositions = qualityCheckedData.first;
307  CTOrientations = qualityCheckedData.second;
308 
309  for (int i = 0; i < Tnavigation.size(); i++)
310  Tnavigation[i] = old_rMpr * Tnavigation[i];
311 
312  Tnavigation = RemoveInvalidData(Tnavigation);
313 
314  for (int i = 0; i < Tnavigation.size(); i++)
315  {
316  trackingPositions.block(0 , i , 3 , 1) = Tnavigation[i].topRightCorner(3 , 1);
317  trackingOrientations.block(0 , i , 3 , 1) = Tnavigation[i].block(0 , 2 , 3 , 1);
318  }
319 
320  //Adjusting points to initially match top positoins in CT and tracking data
321  Eigen::MatrixXd::Index maxIndex;
322  trackingPositions.row(2).maxCoeff( &maxIndex );
323  Eigen::Vector3d translation = CTPositions.col(0) - trackingPositions.col(maxIndex);
324  //Eigen::Vector3d translation = findMedian(CTPositions) - findMedian(trackingPositions);
325  //trackingPositions = trackingPositions.colwise() + translation;
326 
327 
328  registrationMatrix << 1, 0, 0, translation(0),
329  0, 1, 0, translation(1),
330  0, 0, 1, translation(2),
331  0, 0, 0, 1;
332 
333  for (int i = 0; i < Tnavigation.size(); i++)
334  {
335  Tnavigation[i] = registrationMatrix * Tnavigation[i];
336  }
337 
338  int iterationNumber = 0;
339  int maxIterations = 50;
340  while ( translation.array().abs().sum() > 1 && iterationNumber < maxIterations)
341  {
342  for (int i = 0; i < Tnavigation.size(); i++)
343  {
344  trackingPositions.block(0 , i , 3 , 1) = Tnavigation[i].topRightCorner(3 , 1);
345  trackingOrientations.block(0 , i , 3 , 1) = Tnavigation[i].block(0 , 2 , 3 , 1);
346  }
347 
348 
349  iterationNumber++;
350  std::vector<Eigen::MatrixXd::Index> indexVector = dsearch2n( trackingPositions, CTPositions, trackingOrientations, CTOrientations );
351  Eigen::MatrixXd nearestCTPositions(3,indexVector.size());
352  Eigen::MatrixXd nearestCTOrientations(3,indexVector.size());
353  Eigen::VectorXd DAngle(indexVector.size());
354  for (int i = 0; i < indexVector.size(); i++)
355  {
356  nearestCTPositions.col(i) = CTPositions.col(indexVector[i]);
357  nearestCTOrientations.col(i) = CTOrientations.col(indexVector[i]);
358  float o0 = fmod( trackingOrientations(0,i) - nearestCTOrientations(0,i) , 2 );
359  float o1 = fmod( trackingOrientations(1,i) - nearestCTOrientations(1,i) , 2 );
360  float o2 = fmod( trackingOrientations(2,i) - nearestCTOrientations(2,i) , 2 );
361  DAngle(i) = sqrt(o0*o0+o1*o1+o2*o2);
362  }
363 
364  std::pair<Eigen::MatrixXd , Eigen::MatrixXd> result = findPositionsWithSmallesAngleDifference(70 , DAngle , trackingPositions , nearestCTPositions);
365  vtkPointsPtr trackingPositions_vtk = convertTovtkPoints(result.first);
366  vtkPointsPtr CTPositions_vtk = convertTovtkPoints(result.second);
367 
368  Eigen::Matrix4d tempMatrix = performLandmarkRegistration(trackingPositions_vtk, CTPositions_vtk);
369 
370  registrationMatrix = tempMatrix * registrationMatrix;
371 
372  for (int i = 0; i < Tnavigation.size(); i++)
373  {
374  Tnavigation[i] = tempMatrix * Tnavigation[i];
375  }
376 
377  translation << tempMatrix(0,3), tempMatrix(1,3), tempMatrix(2,3);
378 
379  std::cout << "Iteration nr " << iterationNumber << " translation: " << translation.array().abs().sum() << std::endl;
380  }
381 
382  if (translation.array().abs().sum() > 1)
383  std::cout << "Warning: Registration did not converge within " << maxIterations <<" iterations, which is max number of iterations." << std::endl;
384 
385  return registrationMatrix;
386 }
387 
388 Eigen::Matrix4d registrationAlgorithmImage2Image(BranchListPtr branchesFixed, BranchListPtr branchesMoving)
389 {
390  Eigen::Matrix4d registrationMatrix;
391  Eigen::MatrixXd CTPositionsFixed;
392  Eigen::MatrixXd CTOrientationsFixed;
393  Eigen::MatrixXd CTPositionsMoving;
394  Eigen::MatrixXd CTOrientationsMoving;
395 
396  std::vector<BranchPtr> branchVectorFixed = branchesFixed->getBranches();
397  CTPositionsFixed = branchVectorFixed[0]->getPositions();
398  CTOrientationsFixed = branchVectorFixed[0]->getOrientations();
399 
400  std::vector<BranchPtr> branchVectorMoving = branchesMoving->getBranches();
401  CTPositionsMoving = branchVectorMoving[0]->getPositions();
402  CTOrientationsMoving = branchVectorMoving[0]->getOrientations();
403 
404  for (int i = 1; i < branchVectorFixed.size(); i++)
405  {
406  Eigen::MatrixXd CTPositionsFixedNew(CTPositionsFixed.rows() , CTPositionsFixed.cols() + branchVectorFixed[i]->getPositions().cols());
407  Eigen::MatrixXd CTOrientationsFixedNew(CTOrientationsFixed.rows() , CTOrientationsFixed.cols() + branchVectorFixed[i]->getOrientations().cols());
408  CTPositionsFixedNew.leftCols(CTPositionsFixed.cols()) = CTPositionsFixed;
409  CTPositionsFixedNew.rightCols(branchVectorFixed[i]->getPositions().cols()) = branchVectorFixed[i]->getPositions();
410  CTOrientationsFixedNew.leftCols(CTOrientationsFixed.cols()) = CTOrientationsFixed;
411  CTOrientationsFixedNew.rightCols(branchVectorFixed[i]->getOrientations().cols()) = branchVectorFixed[i]->getOrientations();
412  CTPositionsFixed.swap(CTPositionsFixedNew);
413  CTOrientationsFixed.swap(CTOrientationsFixedNew);
414  }
415 
416  for (int i = 1; i < branchVectorMoving.size(); i++)
417  {
418  Eigen::MatrixXd CTPositionsMovingNew(CTPositionsMoving.rows() , CTPositionsMoving.cols() + branchVectorMoving[i]->getPositions().cols());
419  Eigen::MatrixXd CTOrientationsMovingNew(CTOrientationsMoving.rows() , CTOrientationsMoving.cols() + branchVectorMoving[i]->getOrientations().cols());
420  CTPositionsMovingNew.leftCols(CTPositionsMoving.cols()) = CTPositionsMoving;
421  CTPositionsMovingNew.rightCols(branchVectorMoving[i]->getPositions().cols()) = branchVectorMoving[i]->getPositions();
422  CTOrientationsMovingNew.leftCols(CTOrientationsMoving.cols()) = CTOrientationsMoving;
423  CTOrientationsMovingNew.rightCols(branchVectorMoving[i]->getOrientations().cols()) = branchVectorMoving[i]->getOrientations();
424  CTPositionsMoving.swap(CTPositionsMovingNew);
425  CTOrientationsMoving.swap(CTOrientationsMovingNew);
426  }
427 
428  if (CTPositionsFixed.cols() < 10 || CTPositionsMoving.cols() < 10)
429  {
430  CX_LOG_WARNING() << "Too few positions in centerline to perform registration.";
431  return Eigen::Matrix4d::Identity();
432  }
433 
434  std::pair<Eigen::MatrixXd , Eigen::MatrixXd> qualityCheckedDataFixed = RemoveInvalidData(CTPositionsFixed, CTOrientationsFixed);
435  CTPositionsFixed = qualityCheckedDataFixed.first;
436  CTOrientationsFixed = qualityCheckedDataFixed.second;
437 
438  std::pair<Eigen::MatrixXd , Eigen::MatrixXd> qualityCheckedDataMoving = RemoveInvalidData(CTPositionsMoving, CTOrientationsMoving);
439  CTPositionsMoving = qualityCheckedDataMoving.first;
440  CTOrientationsMoving = qualityCheckedDataMoving.second;
441 
442  //Adjusting points for centeroids
443  Eigen::Vector3d translation = findMedian(CTPositionsFixed) - findMedian(CTPositionsMoving);
444 
445  registrationMatrix << 1, 0, 0, translation(0),
446  0, 1, 0, translation(1),
447  0, 0, 1, translation(2),
448  0, 0, 0, 1;
449 
450  for (int i = 0; i < CTPositionsMoving.cols(); i++)
451  {
452  CTPositionsMoving.col(i) = CTPositionsMoving.col(i) + translation;
453  }
454 
455  int iterationNumber = 0;
456  int maxIterations = 200;
457  while ( translation.array().abs().sum() > 0.5 && iterationNumber < maxIterations)
458  {
459 
460  iterationNumber++;
461  std::vector<Eigen::MatrixXd::Index> indexVector = dsearch2n( CTPositionsMoving, CTPositionsFixed, CTOrientationsMoving, CTOrientationsFixed );
462  Eigen::MatrixXd nearestCTPositions(3,indexVector.size());
463  Eigen::MatrixXd nearestCTOrientations(3,indexVector.size());
464  Eigen::VectorXd DAngle(indexVector.size());
465  for (int i = 0; i < indexVector.size(); i++)
466  {
467  nearestCTPositions.col(i) = CTPositionsFixed.col(indexVector[i]);
468  nearestCTOrientations.col(i) = CTOrientationsFixed.col(indexVector[i]);
469  float o0 = fmod( CTOrientationsMoving(0,i) - nearestCTOrientations(0,i) , 2 );
470  float o1 = fmod( CTOrientationsMoving(1,i) - nearestCTOrientations(1,i) , 2 );
471  float o2 = fmod( CTOrientationsMoving(2,i) - nearestCTOrientations(2,i) , 2 );
472  DAngle(i) = sqrt(o0*o0+o1*o1+o2*o2);
473  }
474 
475  std::pair<Eigen::MatrixXd , Eigen::MatrixXd> result = findPositionsWithSmallesAngleDifference(70 , DAngle , CTPositionsMoving , nearestCTPositions);
476  vtkPointsPtr CTPositionsMoving_vtk = convertTovtkPoints(result.first);
477  vtkPointsPtr CTPositionsFixed_vtk = convertTovtkPoints(result.second);
478 
479  Eigen::Matrix4d tempMatrix = performLandmarkRegistration(CTPositionsMoving_vtk, CTPositionsFixed_vtk);
480 
481  registrationMatrix = tempMatrix * registrationMatrix;
482 
483  for (int i = 0; i < CTPositionsMoving.cols(); i++)
484  {
485  CTPositionsMoving.col(i) = tempMatrix.topLeftCorner(3,3) * CTPositionsMoving.col(i) + tempMatrix.topRightCorner(3,1);
486  }
487 
488  translation << tempMatrix(0,3), tempMatrix(1,3), tempMatrix(2,3);
489 
490  }
491 
492  if (translation.array().abs().sum() > 1)
493  CX_LOG_WARNING() << "Registration did not converge within " << maxIterations <<" iterations, which is max number of iterations.";
494 
495  return registrationMatrix;
496 }
497 
499 {
500  if (mBranchListPtr)
501  mBranchListPtr->deleteAllBranches();
502 
503  Eigen::MatrixXd CLpoints = makeTransformedMatrix(centerline, rMd);
504  mBranchListPtr->findBranchesInCenterline(CLpoints);
505  if (numberOfGenerations != 0)
506  {
507  mBranchListPtr->selectGenerations(numberOfGenerations);
508  }
509 
510  mBranchListPtr->smoothBranchPositions(10);
511  mBranchListPtr->smoothOrientations();
512 
513  double minPointDistance = 0.5; //mm
514  mBranchListPtr->excludeClosePositionsInCTCenterline(minPointDistance); // to increase speed in registration
515 
516  vtkPolyDataPtr retval = mBranchListPtr->createVtkPolyDataFromBranches();
517 
518  std::cout << "Number of branches in CT centerline: " << mBranchListPtr->getBranches().size() << std::endl;
519 
520  mCenterlineProcessed = true;
521 
522  return retval;
523 }
524 
525 //Can be used instead of processCenterline(...) if you have a preprosessed branchList to be used in the registration process.
526 void BronchoscopyRegistration::setBranchList(BranchListPtr branchList, int numberOfGenerations)
527 {
528  if (!branchList)
529  return;
530 
531  mBranchListPtr = branchList;
532 
533  if (numberOfGenerations != 0)
534  {
535  mBranchListPtr->selectGenerations(numberOfGenerations);
536  }
537 
538  mCenterlineProcessed = true;
539 }
540 
542 {
543  BranchListPtr branchListPtr;
544  branchListPtr = BranchListPtr(new BranchList());
545  Eigen::MatrixXd CLpoints = makeTransformedMatrix(centerline);
546 
547  branchListPtr->findBranchesInCenterline(CLpoints);
548 
549  if (numberOfGenerations != 0)
550  {
551  branchListPtr->selectGenerations(numberOfGenerations);
552  }
553 
554  branchListPtr->smoothBranchPositions(10);
555  branchListPtr->smoothOrientations();
556 
557  return branchListPtr;
558 }
559 
560 Eigen::Matrix4d BronchoscopyRegistration::runBronchoscopyRegistration(TimedTransformMap trackingData_prMt, Transform3D old_rMpr, double maxDistanceForLocalRegistration)
561 {
562  if(trackingData_prMt.empty())
563  reportError("BronchoscopyRegistration::runBronchoscopyRegistration(): No tracking data");
564 
565  M4Vector Tnavigation;
566  for(TimedTransformMap::iterator iter=trackingData_prMt.begin(); iter!=trackingData_prMt.end(); ++iter)
567  {
568  Tnavigation.push_back(iter->second. matrix());
569  }
570 
571  Tnavigation = excludeClosePositions(Tnavigation);
572 
573  Eigen::Matrix4d regMatrix;
574  if (maxDistanceForLocalRegistration != 0)
575  {
576  Eigen::MatrixXd trackingPositions_temp(3 , Tnavigation.size());
577  M4Vector Tnavigation_temp = Tnavigation;
578  for (int i = 0; i < Tnavigation.size(); i++)
579  {
580  Tnavigation_temp[i] = old_rMpr * Tnavigation[i];
581  trackingPositions_temp.block(0 , i , 3 , 1) = Tnavigation_temp[i].topRightCorner(3 , 1);
582  }
583  BranchListPtr tempPtr = mBranchListPtr->removePositionsForLocalRegistration(trackingPositions_temp, maxDistanceForLocalRegistration);
584  regMatrix = registrationAlgorithm(tempPtr, Tnavigation, old_rMpr);
585  }
586  else
587  regMatrix = registrationAlgorithm(mBranchListPtr, Tnavigation, old_rMpr);
588 
589 
590  if ( boost::math::isnan(regMatrix.sum()) )
591  {
592  std::cout << "Warning: Registration matrix contains 'nan' number, using identity matrix." << std::endl;
593  return Eigen::Matrix4d::Identity();
594  }
595 
596  if ( boost::math::isinf(regMatrix.sum()) )
597  {
598  std::cout << "Warning: Registration matrix contains 'inf' number, using identity matrix." << std::endl;
599  return Eigen::Matrix4d::Identity();
600  }
601 
602  std::cout << "prMt from bronchoscopyRegistration: " << std::endl;
603  for (int i = 0; i < 4; i++)
604  std::cout << regMatrix.row(i) << std::endl;
605 
606  return regMatrix;
607 }
608 
610 {
611 
612  int numberOfGenerations = 4;
613  Eigen::Matrix4d regMatrix;
614 
615  BranchListPtr branchesFixed;
616  BranchListPtr branchesMoving;
617  branchesFixed = processCenterlineImage2Image(centerlineFixed, numberOfGenerations);
618  branchesMoving = processCenterlineImage2Image(centerlineMoving, numberOfGenerations);
619 
620  regMatrix = registrationAlgorithmImage2Image(branchesFixed, branchesMoving);
621 
622 
623  if ( boost::math::isnan(regMatrix.sum()) )
624  {
625  CX_LOG_WARNING() << "Registration matrix contains 'nan' number, using identity matrix.";
626  return Eigen::Matrix4d::Identity();
627  }
628 
629  if ( boost::math::isinf(regMatrix.sum()) )
630  {
631  CX_LOG_WARNING() << "Registration matrix contains 'inf' number, using identity matrix.";
632  return Eigen::Matrix4d::Identity();
633  }
634 
635  return regMatrix;
636 }
637 
639 {
640  return mCenterlineProcessed;
641 }
642 
643 
645 {
646 
647 }
648 
664 Eigen::MatrixXd makeTransformedMatrix(vtkPolyDataPtr linesPolyData, Transform3D rMd)
665 {
666  vtkIdType N = linesPolyData->GetNumberOfPoints();
667  Eigen::MatrixXd CLpoints(3,N);
668 
669  for(vtkIdType i = 0; i < N; i++)
670  {
671  double p[3];
672  linesPolyData->GetPoint(i,p);
673  Eigen::Vector3d position;
674  position(0) = p[0]; position(1) = p[1]; position(2) = p[2];
675  CLpoints.block(0 , i , 3 , 1) = rMd.coord(position);
676  }
677 
678  return CLpoints;
679 }
680 
681 
682 
683 }//namespace cx
void reportError(QString msg)
Definition: cxLogger.cpp:71
Eigen::MatrixXd eraseCol(int removeIndex, Eigen::MatrixXd positions)
vtkSmartPointer< class vtkLandmarkTransform > vtkLandmarkTransformPtr
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
boost::shared_ptr< class BranchList > BranchListPtr
vtkSmartPointer< vtkPoints > vtkPointsPtr
std::pair< Eigen::MatrixXd, Eigen::MatrixXd > RemoveInvalidData(Eigen::MatrixXd positionData, Eigen::MatrixXd orientationData)
vtkPointsPtr convertTovtkPoints(Eigen::MatrixXd positions)
Eigen::Matrix4d registrationAlgorithmImage2Image(BranchListPtr branchesFixed, BranchListPtr branchesMoving)
std::vector< Eigen::MatrixXd::Index > dsearch2n(Eigen::MatrixXd pos1, Eigen::MatrixXd pos2, Eigen::MatrixXd ori1, Eigen::MatrixXd ori2)
BranchListPtr processCenterlineImage2Image(vtkPolyDataPtr centerline, int numberOfGenerations=0)
M4Vector excludeClosePositions(M4Vector Tnavigation)
Eigen::Matrix4d runBronchoscopyRegistration(TimedTransformMap trackingData_prMt, Transform3D old_rMpr, double maxDistanceForLocalRegistration)
std::pair< Eigen::MatrixXd, Eigen::MatrixXd > findPositionsWithSmallesAngleDifference(int percentage, Eigen::VectorXd DAngle, Eigen::MatrixXd trackingPositions, Eigen::MatrixXd nearestCTPositions)
vtkPolyDataPtr processCenterline(vtkPolyDataPtr centerline, Transform3D rMd, int numberOfGenerations=0)
vtkSmartPointer< vtkPolyData > vtkPolyDataPtr
void setBranchList(BranchListPtr branchList, int numberOfGenerations=0)
Eigen::Matrix4d runBronchoscopyRegistrationImage2Image(vtkPolyDataPtr centerlineFixed, vtkPolyDataPtr centerlineMoving)
Eigen::Matrix4d performLandmarkRegistration(vtkPointsPtr source, vtkPointsPtr target)
#define CX_LOG_WARNING
Definition: cxLogger.h:98
Eigen::MatrixXd sortMatrix(int rowNumber, Eigen::MatrixXd matrix)
std::vector< Eigen::Matrix4d > M4Vector
Eigen::VectorXd findMedian(Eigen::MatrixXd matrix)
Eigen::MatrixXd makeTransformedMatrix(vtkPolyDataPtr linesPolyData, Transform3D rMd)
makeTransformedMatrix This method takes an vtkpolydata as input, runs it through a transform and retu...
Eigen::VectorXd sortVector(Eigen::VectorXd v)
Eigen::Matrix4d registrationAlgorithm(BranchListPtr branches, M4Vector Tnavigation, Transform3D old_rMpr)
std::map< double, Transform3D > TimedTransformMap
std::vector< BranchPtr > branchVector
Definition: cxBranch.h:28
Namespace for all CustusX production code.