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