NorMIT-nav  16.5
An IGT application
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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  //std::cout << tar_M_src(i,j) << " ";
181  }
182  //std::cout << std::endl;
183  }
184 
185  if ( boost::math::isnan(tar_M_src.sum()) )
186  {
187  std::cout << "Warning in performLandmarkRegistration: Returning identity matrix due to nan." << std::endl;
188  return Eigen::Matrix4d::Identity();
189  }
190 
191  return tar_M_src;
192 }
193 
194 
195 
196 
197  std::vector<Eigen::MatrixXd::Index> dsearch2n(Eigen::MatrixXd pos1, Eigen::MatrixXd pos2, Eigen::MatrixXd ori1, Eigen::MatrixXd ori2)
198  {
199  Eigen::MatrixXd::Index index;
200  std::vector<Eigen::MatrixXd::Index> indexVector;
201 
202  for (int i = 0; i < pos1.cols(); i++)
203  {
204  Eigen::VectorXd D(pos2.cols());
205  Eigen::VectorXd P(pos2.cols());
206  Eigen::VectorXd O(pos2.cols());
207  Eigen::VectorXd R(pos2.cols());
208 
209  for (int j = 0; j < pos2.cols(); j++)
210  {
211  float p0 = ( pos2(0,j) - pos1(0,i) );
212  float p1 = ( pos2(1,j) - pos1(1,i) );
213  float p2 = ( pos2(2,j) - pos1(2,i) );
214  float o0 = fmod( ori2(0,j) - ori1(0,i) , 2 );
215  float o1 = fmod( ori2(1,j) - ori1(1,i) , 2 );
216  float o2 = fmod( ori2(2,j) - ori1(2,i) , 2 );
217 
218  P(j) = sqrt( p0*p0 + p1*p1 + p2*p2 );
219  O(j) = sqrt( o0*o0 + o1*o1 + o2*o2 );
220 
221  if (boost::math::isnan( O(j) ))
222  O(j) = 4;
223 
224  if ( (o0>2) || (o1>2) || (o2>2) )
225  std::cout << "Warning in bronchoscopyRegistration.cpp: Error on oriantation calculation in dsearch2n. Orientation > 2." << std::endl;
226 
227  R(j) = P(j) / O(j);
228 
229  }
230  float alpha = sqrt( R.mean() );
231  if (boost::math::isnan( alpha ))
232  alpha = 0;
233 
234  D = P + alpha * O;
235  D.minCoeff(&index);
236  //std::cout << "index: " << index << std::endl;
237  indexVector.push_back(index);
238  }
239  return indexVector;
240  }
241 
242 std::pair<Eigen::MatrixXd , Eigen::MatrixXd> RemoveInvalidData(Eigen::MatrixXd positionData, Eigen::MatrixXd orientationData)
243 {
244  std::vector<int> indicesToBeDeleted;
245 
246  for (int i = 0; i < fmin(positionData.cols(), orientationData.cols()); i++)
247  {
248  if ( boost::math::isinf( positionData.block(0 , i , 3 , 1).sum() ) || boost::math::isinf( orientationData.block(0 , i , 3 , 1).sum() ) )
249  indicesToBeDeleted.push_back(i);
250  else if (boost::math::isnan( positionData.block(0 , i , 3 , 1).sum() ) || boost::math::isnan( orientationData.block(0 , i , 3 , 1).sum() ))
251  indicesToBeDeleted.push_back(i);
252  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) ||
253  (orientationData.block(0 , i , 1 , 1).sum() == 0 && orientationData.block(1 , i , 1 , 1).sum() == 0 && orientationData.block(2 , i , 1 , 1).sum() == 0))
254  indicesToBeDeleted.push_back(i);
255  }
256 
257  for ( int i = indicesToBeDeleted.size() - 1; i >= 0; i-- )
258  {
259  std::cout << "Warning in bronchoscopyRegistration: Removed corrupted data: " << positionData.block(0 , indicesToBeDeleted[i] , 3 , 1) << " " << orientationData.block(0 , indicesToBeDeleted[i] , 3 , 1) << std::endl;
260  positionData = eraseCol(indicesToBeDeleted[i],positionData);
261  orientationData = eraseCol(indicesToBeDeleted[i],orientationData);
262  }
263  return std::make_pair(positionData , orientationData);
264 }
265 
267 {
268  Eigen::Vector3d position;
269  Eigen::Vector3d orientation;
270  std::vector<int> indicesToBeDeleted;
271 
272  for (int i = 0; i < T_vector.size(); i++)
273  {
274  position = T_vector[i].topRightCorner(3 , 1);
275  orientation = T_vector[i].block(0 , 2 , 3 , 1);
276  if ( boost::math::isinf( position.sum() ) || boost::math::isinf( orientation.sum() ) )
277  indicesToBeDeleted.push_back(i);
278  else if (boost::math::isnan( position.sum() ) || boost::math::isnan( orientation.sum() ))
279  indicesToBeDeleted.push_back(i);
280  else if ( (position[0] == 0 && position[1] == 0 && position[2] == 0) ||
281  (orientation[0] == 0 && orientation[1] == 0 && orientation[2] == 0) )
282  indicesToBeDeleted.push_back(i);
283  }
284 
285  for ( int i = indicesToBeDeleted.size() - 1; i >= 0; i-- )
286  {
287  std::cout << "Warning in bronchoscopyRegistration: Removed corrupted data: " << T_vector[i].topRightCorner(3 , 1) << " " << T_vector[i].block(0 , 2 , 3 , 1) << std::endl;
288  T_vector.erase(T_vector.begin() + indicesToBeDeleted[i]);
289  }
290  return T_vector;
291 }
292 
293 Eigen::Matrix4d registrationAlgorithm(BranchListPtr branches, M4Vector Tnavigation, Transform3D old_rMpr)
294 {
295  Eigen::Matrix4d registrationMatrix;
296  Eigen::MatrixXd CTPositions;
297  Eigen::MatrixXd CTOrientations;
298  Eigen::MatrixXd trackingPositions(3 , Tnavigation.size());
299  Eigen::MatrixXd trackingOrientations(3, Tnavigation.size());
300 
301  std::vector<BranchPtr> branchVector = branches->getBranches();
302  CTPositions = branchVector[0]->getPositions();
303  CTOrientations = branchVector[0]->getOrientations();
304 
305  std::cout << "Positions in centerline:" << CTPositions.cols() << std::endl;
306  std::cout << "Positions in tracking data:" << trackingPositions.cols() << std::endl;
307 
308  if (trackingPositions.cols() < 10)
309  {
310  std::cout << "Warning: Too few positions in tracking data to perform registration." << std::endl;
311  return Eigen::Matrix4d::Identity();
312  }
313 
314  for (int i = 1; i < branchVector.size(); i++)
315  {
316  Eigen::MatrixXd CTPositionsNew(CTPositions.rows() , CTPositions.cols() + branchVector[i]->getPositions().cols());
317  Eigen::MatrixXd CTOrientationsNew(CTOrientations.rows() , CTOrientations.cols() + branchVector[i]->getOrientations().cols());
318  CTPositionsNew.leftCols(CTPositions.cols()) = CTPositions;
319  CTPositionsNew.rightCols(branchVector[i]->getPositions().cols()) = branchVector[i]->getPositions();
320  CTOrientationsNew.leftCols(CTOrientations.cols()) = CTOrientations;
321  CTOrientationsNew.rightCols(branchVector[i]->getOrientations().cols()) = branchVector[i]->getOrientations();
322  CTPositions.swap(CTPositionsNew);
323  CTOrientations.swap(CTOrientationsNew);
324  }
325 
326  if (CTPositions.cols() < 10)
327  {
328  std::cout << "Warning: Too few positions in centerline to perform registration." << std::endl;
329  return Eigen::Matrix4d::Identity();
330  }
331 
332  std::pair<Eigen::MatrixXd , Eigen::MatrixXd> qualityCheckedData = RemoveInvalidData(CTPositions, CTOrientations);
333  CTPositions = qualityCheckedData.first;
334  CTOrientations = qualityCheckedData.second;
335 
336  for (int i = 0; i < Tnavigation.size(); i++)
337  Tnavigation[i] = old_rMpr * Tnavigation[i];
338 
339  Tnavigation = RemoveInvalidData(Tnavigation);
340 
341  for (int i = 0; i < Tnavigation.size(); i++)
342  {
343  trackingPositions.block(0 , i , 3 , 1) = Tnavigation[i].topRightCorner(3 , 1);
344  trackingOrientations.block(0 , i , 3 , 1) = Tnavigation[i].block(0 , 2 , 3 , 1);
345  }
346 
347  //Adjusting points for centeroids
348  Eigen::MatrixXd::Index maxIndex;
349  trackingPositions.row(2).maxCoeff( &maxIndex );
350  //std::cout << "maxIndex: " << maxIndex << std::endl;
351  //Eigen::Vector3d translation = CTPositions.col(0) - trackingPositions.col(maxIndex);
352  //std::cout << "CTPositions.col(0): " << CTPositions.col(0) << std::endl;
353  Eigen::Vector3d translation = findMedian(CTPositions) - findMedian(trackingPositions);
354  //Eigen::Matrix3d rotation;
355  //trackingPositions = trackingPositions.colwise() + translation;
356  //std::cout << "trackingPositions.col(maxIndex): " << trackingPositions.col(maxIndex) << std::endl;
357 
358 
359  registrationMatrix << 1, 0, 0, translation(0),
360  0, 1, 0, translation(1),
361  0, 0, 1, translation(2),
362  0, 0, 0, 1;
363 
364  for (int i = 0; i < Tnavigation.size(); i++)
365  {
366  Tnavigation[i] = registrationMatrix * Tnavigation[i];
367  }
368  //std::cout << "Tracking data 1 after initial translation: " << Tnavigation[0] << std::endl;
369  //std::cout << "Tracking data maxIndex after initial translation: " << Tnavigation[maxIndex] << std::endl;
370 
371  int iterationNumber = 0;
372  int maxIterations = 50;
373  while ( translation.array().abs().sum() > 1 && iterationNumber < maxIterations)
374  {
375  for (int i = 0; i < Tnavigation.size(); i++)
376  {
377  trackingPositions.block(0 , i , 3 , 1) = Tnavigation[i].topRightCorner(3 , 1);
378  trackingOrientations.block(0 , i , 3 , 1) = Tnavigation[i].block(0 , 2 , 3 , 1);
379  }
380 
381 
382  iterationNumber++;
383  std::vector<Eigen::MatrixXd::Index> indexVector = dsearch2n( trackingPositions, CTPositions, trackingOrientations, CTOrientations );
384  Eigen::MatrixXd nearestCTPositions(3,indexVector.size());
385  Eigen::MatrixXd nearestCTOrientations(3,indexVector.size());
386  Eigen::VectorXd DAngle(indexVector.size());
387  for (int i = 0; i < indexVector.size(); i++)
388  {
389  nearestCTPositions.col(i) = CTPositions.col(indexVector[i]);
390  nearestCTOrientations.col(i) = CTOrientations.col(indexVector[i]);
391  float o0 = fmod( trackingOrientations(0,i) - nearestCTOrientations(0,i) , 2 );
392  float o1 = fmod( trackingOrientations(1,i) - nearestCTOrientations(1,i) , 2 );
393  float o2 = fmod( trackingOrientations(2,i) - nearestCTOrientations(2,i) , 2 );
394  DAngle(i) = sqrt(o0*o0+o1*o1+o2*o2);
395  }
396 
397  std::pair<Eigen::MatrixXd , Eigen::MatrixXd> result = findPositionsWithSmallesAngleDifference(70 , DAngle , trackingPositions , nearestCTPositions);
398  vtkPointsPtr trackingPositions_vtk = convertTovtkPoints(result.first);
399  vtkPointsPtr CTPositions_vtk = convertTovtkPoints(result.second);
400 
401  Eigen::Matrix4d tempMatrix = performLandmarkRegistration(trackingPositions_vtk, CTPositions_vtk);
402 
403  registrationMatrix = tempMatrix * registrationMatrix;
404 
405  for (int i = 0; i < Tnavigation.size(); i++)
406  {
407  Tnavigation[i] = tempMatrix * Tnavigation[i];
408  }
409 
410  translation << tempMatrix(0,3), tempMatrix(1,3), tempMatrix(2,3);
411 
412  std::cout << "Iteration nr " << iterationNumber << " translation: " << translation.array().abs().sum() << std::endl;
413  //for (int i = 0; i < 4; i++)
414  // std::cout << tempMatrix.row(i) << std::endl;
415  }
416 
417  if (translation.array().abs().sum() > 1)
418  std::cout << "Warning: Registration did not converge within " << maxIterations <<" iterations, which is max number of iterations." << std::endl;
419 
420  return registrationMatrix;
421 }
422 
424 {
425  if (mBranchListPtr)
426  mBranchListPtr->deleteAllBranches();
427 
428  int N = centerline->GetNumberOfPoints();
429  Eigen::MatrixXd CLpoints(3,N);
430  for(vtkIdType i = 0; i < N; i++)
431  {
432  double p[3];
433  centerline->GetPoint(i,p);
434  Eigen::Vector3d position;
435  position(0) = p[0]; position(1) = p[1]; position(2) = p[2];
436  CLpoints.block(0 , i , 3 , 1) = rMd.coord(position);
437  }
438  mBranchListPtr->findBranchesInCenterline(CLpoints);
439  if (numberOfGenerations != 0)
440  {
441  mBranchListPtr->selectGenerations(numberOfGenerations);
442  }
443 
444  mBranchListPtr->smoothBranchPositions();
445  mBranchListPtr->calculateOrientations();
446  mBranchListPtr->smoothOrientations();
447 
448  vtkPolyDataPtr retval = vtkPolyDataPtr::New();
449  vtkPointsPtr points = vtkPointsPtr::New();
450  vtkCellArrayPtr lines = vtkCellArrayPtr::New();
451 
452  std::vector<BranchPtr> branches = mBranchListPtr->getBranches();
453  int positionCounter = 0;
454  for (int i = 0; i < branches.size(); i++)
455  {
456  Eigen::MatrixXd positions = branches[i]->getPositions();
457  for (int j = 0; j < positions.cols(); j++)
458  {
459  positionCounter ++;
460  points->InsertNextPoint(positions(0,j),positions(1,j),positions(2,j));
461  if (j < positions.cols()-1)
462  {
463  vtkIdType connection[2] = {positionCounter-1, positionCounter};
464  lines->InsertNextCell(2, connection);
465  }
466  }
467  }
468  retval->SetPoints(points);
469  retval->SetLines(lines);
470 
471  std::cout << "Number of branches in CT centerline: " << mBranchListPtr->getBranches().size() << std::endl;
472 
473  mCenterlineProcessed = true;
474 
475  return retval;
476 
477 }
478 
479 
480 Eigen::Matrix4d BronchoscopyRegistration::runBronchoscopyRegistration(TimedTransformMap trackingData_prMt, Transform3D old_rMpr, double maxDistanceForLocalRegistration)
481 {
482  if(trackingData_prMt.empty())
483  reportError("BronchoscopyRegistration::runBronchoscopyRegistration(): No tracking data");
484 
485  M4Vector Tnavigation;
486  for(TimedTransformMap::iterator iter=trackingData_prMt.begin(); iter!=trackingData_prMt.end(); ++iter)
487  {
488  Tnavigation.push_back(iter->second. matrix());
489  }
490 
491  //vtkPointsPtr points = centerline->GetPoints();
492 
493  Tnavigation = excludeClosePositions(Tnavigation);
494 
495  Eigen::Matrix4d regMatrix;
496  if (maxDistanceForLocalRegistration != 0)
497  {
498  Eigen::MatrixXd trackingPositions_temp(3 , Tnavigation.size());
499  M4Vector Tnavigation_temp = Tnavigation;
500  for (int i = 0; i < Tnavigation.size(); i++)
501  {
502  Tnavigation_temp[i] = old_rMpr * Tnavigation[i];
503  trackingPositions_temp.block(0 , i , 3 , 1) = Tnavigation_temp[i].topRightCorner(3 , 1);
504  }
505  BranchListPtr tempPtr = mBranchListPtr->removePositionsForLocalRegistration(trackingPositions_temp, maxDistanceForLocalRegistration);
506  regMatrix = registrationAlgorithm(tempPtr, Tnavigation, old_rMpr);
507  }
508  else
509  regMatrix = registrationAlgorithm(mBranchListPtr, Tnavigation, old_rMpr);
510 
511 
512  if ( boost::math::isnan(regMatrix.sum()) )
513  {
514  std::cout << "Warning: Registration matrix contains 'nan' number, using identity matrix." << std::endl;
515  return Eigen::Matrix4d::Identity();
516  }
517 
518  if ( boost::math::isinf(regMatrix.sum()) )
519  {
520  std::cout << "Warning: Registration matrix contains 'inf' number, using identity matrix." << std::endl;
521  return Eigen::Matrix4d::Identity();
522  }
523 
524  std::cout << "prMt from bronchoscopyRegistration: " << std::endl;
525  for (int i = 0; i < 4; i++)
526  std::cout << regMatrix.row(i) << std::endl;
527 
528  return regMatrix;
529 }
530 
531 
533 {
534  return mCenterlineProcessed;
535 }
536 
537 
539 {
540 
541 }
542 
543 
544 
545 }//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.
vtkSmartPointer< class vtkCellArray > vtkCellArrayPtr
boost::shared_ptr< class BranchList > BranchListPtr
std::pair< Eigen::MatrixXd, Eigen::MatrixXd > RemoveInvalidData(Eigen::MatrixXd positionData, Eigen::MatrixXd orientationData)
vtkPointsPtr convertTovtkPoints(Eigen::MatrixXd positions)
vtkSmartPointer< class vtkPolyData > vtkPolyDataPtr
std::vector< Eigen::MatrixXd::Index > dsearch2n(Eigen::MatrixXd pos1, Eigen::MatrixXd pos2, Eigen::MatrixXd ori1, Eigen::MatrixXd ori2)
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)
Eigen::Matrix4d performLandmarkRegistration(vtkPointsPtr source, vtkPointsPtr target)
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::VectorXd sortVector(Eigen::VectorXd v)
Eigen::Matrix4d registrationAlgorithm(BranchListPtr branches, M4Vector Tnavigation, Transform3D old_rMpr)
std::map< double, Transform3D > TimedTransformMap
vtkSmartPointer< class vtkPoints > vtkPointsPtr
std::vector< BranchPtr > branchVector
Definition: cxBranch.h:49