NorMIT-nav  22.09
An IGT application
cxBranchList.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 =========================================================================*/
11 #include "cxBranchList.h"
12 #include "cxBranch.h"
13 #include "cxMesh.h"
14 #include "cxVector3D.h"
15 #include <vtkPolyData.h>
16 #include <vtkCardinalSpline.h>
17 #include "cxLogger.h"
18 #include <boost/math/special_functions/fpclassify.hpp> // isnan
19 
20 typedef vtkSmartPointer<class vtkCardinalSpline> vtkCardinalSplinePtr;
21 
22 namespace cx
23 {
24 
25 BranchList::BranchList()
26 {
27 
28 }
29 
30 
31 BranchList::~BranchList()
32 {
33  // for (int i = 0; i < mBranches.size(); i++)
34  // mBranches[i]->~Branch();
35 }
36 
37 void BranchList::addBranch(BranchPtr b)
38 {
39  mBranches.push_back(b);
40 }
41 
42 void BranchList::deleteBranch(BranchPtr b)
43 {
44  if(b->getParentBranch())
45  b->getParentBranch()->deleteChildBranches();
46 
47  for( int i = 0; i < mBranches.size(); i++ )
48  {
49  if (b == mBranches[i])
50  {
51  mBranches.erase(mBranches.begin() + i);
52  return;
53  }
54  }
55 }
56 
57 void BranchList::deleteAllBranches()
58 {
59  mBranches.clear();
60 }
61 
62 std::vector<BranchPtr> BranchList::getBranches()
63 {
64  return mBranches;
65 }
66 
67 void BranchList::selectGenerations(int maxGeneration)
68 {
69  std::vector<int> branchNumbersToBeDeleted;
70  for( int i = 0; i < mBranches.size(); i++ )
71  {
72  int generationCounter = 1;
73  BranchPtr currentBranch = mBranches[i];
74  while (currentBranch->getParentBranch()){
75  generationCounter++;
76  currentBranch = currentBranch->getParentBranch();
77  if (generationCounter > maxGeneration)
78  {
79  branchNumbersToBeDeleted.push_back(i);
80  break;
81  }
82  }
83 
84  }
85 
86  for ( int i = branchNumbersToBeDeleted.size() - 1; i >= 0; i-- )
87  deleteBranch(mBranches[branchNumbersToBeDeleted[i]]);
88 }
89 
90 void BranchList::findBronchoscopeRotation()
91 {
92  BranchPtr trachea = this->getBranches()[0];
93  if(trachea)
94  calculateBronchoscopeRotation(trachea);
95 }
96 
97 void BranchList::calculateBronchoscopeRotation(BranchPtr branch)
98 // recursive function on all child branches
99 {
100  if(!branch->getParentBranch())
101  branch->setBronchoscopeRotation(0);
102  else
103  {
104  double parentRotation = branch->getParentBranch()->getBronchoscopeRotation();
105  Eigen::MatrixXd branchOrientations = branch->getOrientations();
106  Vector3D branchOrientationStart = branchOrientations.leftCols(std::min(25, (int) branchOrientations.cols())).rowwise().mean();
107  Eigen::MatrixXd parentBranchOrientations = branch->getParentBranch()->getOrientations();
108  Vector3D parentBranchOrientationEnd = parentBranchOrientations.rightCols(std::min(50, (int) parentBranchOrientations.cols())).rowwise().mean();
109 
110  Vector3D bendingDirection = calculateBronchoscopeBendingDirection(parentBranchOrientationEnd, branchOrientationStart);
111  double bronchoscopeRotation = bendingDirectionToBronchoscopeRotation(bendingDirection, parentBranchOrientationEnd, parentRotation);
112 
113  branch->setBronchoscopeRotation(bronchoscopeRotation);
114  }
115 
116  branchVector childBranches = branch->getChildBranches();
117  for(int i=0; i<childBranches.size(); i++)
118  calculateBronchoscopeRotation(childBranches[i]);
119 }
120 
121 double bendingDirectionToBronchoscopeRotation(Vector3D bendingDirection, Vector3D parentBranchOrientation, double parentRotation)
122 {
123  double bronchoscopeRotation;
124 
125  Vector3D xVector = Vector3D(1,0,0);
126  Vector3D up = cross(parentBranchOrientation, xVector).normalized();
127  if(up(1) > 0)
128  up = -up;
129 
130  bronchoscopeRotation = acos( up.dot(bendingDirection) );
131 
132  Vector3D N = cross(up, bendingDirection).normalized();
133 
134  if(parentBranchOrientation.dot(N) < 0)
135  bronchoscopeRotation = -bronchoscopeRotation;
136 
137  double rotationDifferenceFromParent = bronchoscopeRotation - parentRotation;
138 
139  //Make sure rotation difference is between -180 and 180 deg.
140  if ( rotationDifferenceFromParent > M_PI )
141  rotationDifferenceFromParent = rotationDifferenceFromParent - 2*M_PI;
142  else if ( rotationDifferenceFromParent < -M_PI )
143  rotationDifferenceFromParent = rotationDifferenceFromParent + 2*M_PI;
144 
145  //Tilt down if needed rotation is less than maxRotationToTiltDown
146  if( rotationDifferenceFromParent < (MAX_ROTATION_TO_TILT_DOWN_DEGREES - 180)*M_PI/180 )
147  bronchoscopeRotation = bronchoscopeRotation + M_PI;
148  else if( rotationDifferenceFromParent > (180 - MAX_ROTATION_TO_TILT_DOWN_DEGREES)*M_PI/180 )
149  bronchoscopeRotation = bronchoscopeRotation - M_PI;
150 
151  //Not allowing rotation above 180 deg
152  rotationDifferenceFromParent = bronchoscopeRotation - parentRotation;
153  if( rotationDifferenceFromParent > M_PI )
154  bronchoscopeRotation = bronchoscopeRotation - M_PI;
155  else if( rotationDifferenceFromParent < -M_PI )
156  bronchoscopeRotation = bronchoscopeRotation + M_PI;
157 
158 
159 
160  //Sett rotasjon til samme som parent dersom endring er mindre enn 15?? grader
161 
162  return bronchoscopeRotation;
163 }
164 
165 Vector3D calculateBronchoscopeBendingDirection(Vector3D A, Vector3D B)
166 {
167  A = A.normalized();
168  B = B.normalized();
169  Vector3D N = A.cross(B);
170  N = N.normalized();
171  Vector3D C;
172 
173  C(2) = 1;
174  C(1) = - ( ( N(2) - N(0)*A(2)/A(0) ) / ( N(1) - N(0)*A(1)/A(0) ) ) * C(2);
175  if(boost::math::isnan(C(1)) || boost::math::isinf(C(1)))
176  C(1) = 0;
177  if( similar(A(0),0) )
178  C(0) = 0;
179  else
180  C(0) = - ( A(1)*C(1) + A(2)*C(2) ) / A(0);
181 
182  C = C.normalized();
183 
184  if (B.dot(C) < 0)
185  C = -C;
186 
187  return C;
188 }
189 
190 void BranchList::smoothOrientations()
191 {
192  for (int i = 0; i < mBranches.size(); i++)
193  {
194  Eigen::MatrixXd orientations = mBranches[i]->getOrientations();
195  Eigen::MatrixXd newOrientations(orientations.rows(),orientations.cols());
196  int numberOfColumns = orientations.cols();
197  for (int j = 0; j < numberOfColumns; j++)
198  {
199  newOrientations.col(j) = orientations.block(0,std::max(j-2,0),orientations.rows(),std::min(5,numberOfColumns-j)).rowwise().mean(); //smoothing
200  newOrientations.col(j) = newOrientations.col(j) / newOrientations.col(j).norm(); // normalizing
201  }
202  mBranches[i]->setOrientations(newOrientations);
203  }
204 }
205 
206 void BranchList::smoothRadius()
207 {
208  for (int i = 0; i < mBranches.size(); i++)
209  {
210  Eigen::VectorXd radius = mBranches[i]->getRadius();
211  Eigen::VectorXd newRadius(radius.rows(),radius.cols());
212  int numberOfPoints = radius.size();
213  for (int j = 0; j < numberOfPoints; j++)
214  {
215  newRadius[j] = radius.segment(std::max(j-2,0), std::min(5,numberOfPoints-j)).mean(); //smoothing
216  }
217  mBranches[i]->setRadius(newRadius);
218  }
219 }
220 
221 BranchPtr BranchList::findBranchWithLargestRadius()
222 {
223  BranchPtr branchWithLargestRadius = mBranches[0];
224  double largestRadius = mBranches[0]->getAverageRadius();
225 
226  for (int i = 1; i < mBranches.size(); i++)
227  {
228  if (mBranches[i]->getAverageRadius() > largestRadius)
229  {
230  largestRadius = mBranches[i]->getAverageRadius();
231  branchWithLargestRadius = mBranches[i];
232  }
233  }
234 
235  return branchWithLargestRadius;
236 }
237 
238 void BranchList::interpolateBranchPositions(double resolution /*mm*/)
239 {
240 
241  for (int i = 0; i < mBranches.size(); i++)
242  {
243  Eigen::MatrixXd positions = mBranches[i]->getPositions();
244 
245  if (mBranches[i]->getParentBranch()) // Add parents last position to interpolate between branches
246  {
247  Eigen::MatrixXd parentPositions = mBranches[i]->getParentBranch()->getPositions();
248  Eigen::MatrixXd positionsResized(positions.rows(), positions.cols()+1);
249  positionsResized.col(0) = parentPositions.rightCols(1);
250  positionsResized.rightCols(positions.cols()) = positions;
251  positions = positionsResized;
252  }
253 
254  std::vector<Eigen::Vector3d> interpolatedPositions;
255  for (int j = 0; j < positions.cols()-1; j++)
256  {
257  int interpolationFactor = static_cast<int>(std::ceil( ( positions.col(j+1) - positions.col(j) ).norm() / resolution ));
258  for (int k = 0; k < interpolationFactor; k++){
259  Eigen::Vector3d interpolationPoint;
260  interpolationPoint[0] = (positions(0,j)*(interpolationFactor-k) + positions(0,j+1)*(k) ) / interpolationFactor;
261  interpolationPoint[1] = (positions(1,j)*(interpolationFactor-k) + positions(1,j+1)*(k) ) / interpolationFactor;
262  interpolationPoint[2] = (positions(2,j)*(interpolationFactor-k) + positions(2,j+1)*(k) ) / interpolationFactor;
263  interpolatedPositions.push_back(interpolationPoint);
264  }
265  }
266  if (mBranches[i]->getParentBranch()) // Remove parents last position after interpolation
267  {
268  Eigen::MatrixXd positionsResized(positions.rows(), positions.cols()-1);
269  positionsResized = positions.rightCols(positionsResized.cols());
270  positions = positionsResized;
271  }
272  Eigen::MatrixXd interpolationResult(3 , interpolatedPositions.size());
273  for (int j = 0; j < interpolatedPositions.size(); j++)
274  {
275  interpolationResult(0,j) = interpolatedPositions[j](0);
276  interpolationResult(1,j) = interpolatedPositions[j](1);
277  interpolationResult(2,j) = interpolatedPositions[j](2);
278  }
279  mBranches[i]->setPositions(interpolationResult);
280  }
281 
282 }
283 
284 void BranchList::smoothBranchPositions(int controlPointDistance)
285 {
286  for (int i = 0; i < mBranches.size(); i++)
287  {
288  Eigen::MatrixXd positions = mBranches[i]->getPositions();
289  int numberOfInputPoints = positions.cols();
290  //int controlPointFactor = 10;
291  //int numberOfControlPoints = numberOfInputPoints / controlPointFactor;
292  double branchLength = (positions.rightCols(1) - positions.leftCols(1)).norm();
293  int numberOfControlPoints = std::ceil(branchLength/controlPointDistance);
294  numberOfControlPoints = std::max(numberOfControlPoints, 2); // at least two control points
295 
296  vtkCardinalSplinePtr splineX = vtkSmartPointer<vtkCardinalSpline>::New();
297  vtkCardinalSplinePtr splineY = vtkSmartPointer<vtkCardinalSpline>::New();
298  vtkCardinalSplinePtr splineZ = vtkSmartPointer<vtkCardinalSpline>::New();
299 
300  //add control points to spline
301  for(int j=0; j<numberOfControlPoints; j++)
302  {
303  int indexP = (j*numberOfInputPoints)/numberOfControlPoints;
304 
305  splineX->AddPoint(indexP,positions(0,indexP));
306  splineY->AddPoint(indexP,positions(1,indexP));
307  splineZ->AddPoint(indexP,positions(2,indexP));
308  }
309  //Always add the last point to complete spline
310  splineX->AddPoint(numberOfInputPoints-1,positions(0,numberOfInputPoints-1));
311  splineY->AddPoint(numberOfInputPoints-1,positions(1,numberOfInputPoints-1));
312  splineZ->AddPoint(numberOfInputPoints-1,positions(2,numberOfInputPoints-1));
313 
314  //evaluate spline - get smoothed positions
315  Eigen::MatrixXd smoothingResult(3 , numberOfInputPoints);
316  for(int j=0; j<numberOfInputPoints; j++)
317  {
318  double splineParameter = j;
319  smoothingResult(0,j) = splineX->Evaluate(splineParameter);
320  smoothingResult(1,j) = splineY->Evaluate(splineParameter);
321  smoothingResult(2,j) = splineZ->Evaluate(splineParameter);
322  }
323  mBranches[i]->setPositions(smoothingResult);
324  }
325 }
326 
327 void BranchList::findBranchesInCenterline(Eigen::MatrixXd positions_r, bool sortByZindex)
328 {
329  if (sortByZindex)
330  positions_r = sortMatrix(2,positions_r);
331 
332  Eigen::MatrixXd positionsNotUsed_r = positions_r;
333 
334  // int minIndex;
335  int index;
336  int splitIndex;
337  Eigen::MatrixXd::Index startIndex;
338  BranchPtr branchToSplit;
339  while (positionsNotUsed_r.cols() > 0)
340  {
341  if (!mBranches.empty())
342  {
343  double minDistance = 1000;
344  for (int i = 0; i < mBranches.size(); i++)
345  {
346  std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd> distances;
347  distances = dsearchn(positionsNotUsed_r, mBranches[i]->getPositions());
348  double d = distances.second.minCoeff(&index);
349  if (d < minDistance)
350  {
351  minDistance = d;
352  branchToSplit = mBranches[i];
353  startIndex = index;
354  if (minDistance < 2)
355  break;
356  }
357  }
358  std::pair<Eigen::MatrixXd::Index, double> dsearchResult = dsearch(positionsNotUsed_r.col(startIndex) , branchToSplit->getPositions());
359  splitIndex = dsearchResult.first;
360  }
361  else //if this is the first branch. Select the top position (Trachea).
362  startIndex = positionsNotUsed_r.cols() - 1;
363 
364  std::pair<Eigen::MatrixXd,Eigen::MatrixXd > connectedPointsResult = findConnectedPointsInCT(startIndex , positionsNotUsed_r);
365  Eigen::MatrixXd branchPositions = connectedPointsResult.first;
366  positionsNotUsed_r = connectedPointsResult.second;
367 
368  if (branchPositions.cols() >= 5) //only include brances of length >= 5 points
369  {
370  BranchPtr newBranch = BranchPtr(new Branch());
371  newBranch->setPositions(branchPositions);
372  mBranches.push_back(newBranch);
373 
374  if (mBranches.size() > 1) // do not try to split another branch when the first branch is processed
375  {
376  if ((splitIndex + 1 >= 5) && (branchToSplit->getPositions().cols() - splitIndex - 1 >= 5))
377  //do not split branch if the new branch is close to the edge of the branch
378  //if the new branch is not close to one of the edges of the
379  //connected existing branch: Split the existing branch
380  {
381  BranchPtr newBranchFromSplit = BranchPtr(new Branch());
382  Eigen::MatrixXd branchToSplitPositions = branchToSplit->getPositions();
383  newBranchFromSplit->setPositions(branchToSplitPositions.rightCols(branchToSplitPositions.cols() - splitIndex - 1));
384  branchToSplit->setPositions(branchToSplitPositions.leftCols(splitIndex + 1));
385  mBranches.push_back(newBranchFromSplit);
386  newBranchFromSplit->setParentBranch(branchToSplit);
387  newBranch->setParentBranch(branchToSplit);
388  newBranchFromSplit->setChildBranches(branchToSplit->getChildBranches());
389  branchVector branchToSplitChildren = branchToSplit->getChildBranches();
390  for (int i = 0; i < branchToSplitChildren.size(); i++)
391  branchToSplitChildren[i]->setParentBranch(newBranchFromSplit);
392  branchToSplit->deleteChildBranches();
393  branchToSplit->addChildBranch(newBranchFromSplit);
394  branchToSplit->addChildBranch(newBranch);
395  }
396  else if (splitIndex + 1 < 5)
397  // If the new branch is close to the start of the existing
398  // branch: Connect it to the same position start as the
399  // existing branch
400  {
401  newBranch->setParentBranch(branchToSplit->getParentBranch());
402  if(branchToSplit->getParentBranch())
403  branchToSplit->getParentBranch()->addChildBranch(newBranch);
404  }
405  else if (branchToSplit->getPositions().cols() - splitIndex - 1 < 5)
406  // If the new branch is close to the end of the existing
407  // branch: Connect it to the end of the existing branch
408  {
409  newBranch->setParentBranch(branchToSplit);
410  branchToSplit->addChildBranch(newBranch);
411  }
412 
413  }
414 
415  }
416  }
417 }
418 
419 BranchListPtr BranchList::removePositionsForLocalRegistration(Eigen::MatrixXd trackingPositions, double maxDistance)
420 {
421  BranchListPtr retval = BranchListPtr(new BranchList());
422  BranchPtr b;
423  for (int i = 0; i < mBranches.size(); i++)
424  {
425  b = BranchPtr(new Branch());
426  b->setPositions(mBranches[i]->getPositions());
427  retval->addBranch(b);
428  }
429 
430  std::vector<BranchPtr> branches = retval->getBranches();
431  Eigen::MatrixXd positions;
432  Eigen::MatrixXd orientations;
433  for (int i = 0; i < branches.size(); i++)
434  {
435  positions = branches[i]->getPositions();
436  orientations = branches[i]->getOrientations();
437  std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd> distanceData;
438  distanceData = dsearchn(positions, trackingPositions);
439  Eigen::VectorXd distance = distanceData.second;
440  for (int j = positions.cols() - 1; j >= 0; j--)
441  {
442  if (distance(j) > maxDistance)
443  {
444  positions = eraseCol(j, positions);
445  orientations = eraseCol(j, orientations);
446  }
447  }
448  branches[i]->setPositions(positions);
449  }
450  return retval;
451 }
452 
453 void BranchList::excludeClosePositionsInCTCenterline(double minPointDistance){
454 
455  std::vector<BranchPtr> branchVector = this->getBranches();
456  for (int i = 0; i < branchVector.size(); i++)
457  {
458  Eigen::MatrixXd positions = branchVector[i]->getPositions();
459  Eigen::MatrixXd orientations = branchVector[i]->getOrientations();
460 
461  for (int i = positions.cols()-2; i > 0; i--){
462  double distance = (positions.col(i) - positions.col(i+1)).norm();
463  if ( distance < minPointDistance )
464  {
465  positions = eraseCol(i,positions);
466  orientations = eraseCol(i,orientations);
467  }
468  }
469  branchVector[i]->setPositions(positions);
470  }
471 
472 }
473 
474 void BranchList::markLungLap(QString name, Vector3D position)
475 {
476  BranchPtr branch = findClosestBranch(position);
477  this->setLapName(branch, name);
478 }
479 
480 //Recursive function. Setting lap name to branch and all sub-branches
481 void BranchList::setLapName(BranchPtr branch, QString name)
482 {
483  branch->setLap(name);
484  branchVector bv = branch->getChildBranches();
485  for(int i=0; i<bv.size(); i++)
486  setLapName(bv[i], name);
487 }
488 
489 QString BranchList::findClosestLungLap(Vector3D position)
490 {
491  BranchPtr branch = findClosestBranch(position);
492  return branch->getLap();
493 }
494 
495 double BranchList::findDistance(Vector3D p1, Vector3D p2)
496 {
497  double d0 = p1(0) - p2(0);
498  double d1 = p1(1) - p2(1);
499  double d2 = p1(2) - p2(2);
500 
501  double D = sqrt( d0*d0 + d1*d1 + d2*d2 );
502 
503  return D;
504 }
505 
506 BranchPtr BranchList::findClosestBranch(Vector3D targetCoordinate_r)
507 {
508  double minDistance = 100000;
509  BranchPtr minDistanceBranch;
510  for (int i = 0; i < mBranches.size(); i++)
511  {
512  Eigen::MatrixXd positions = mBranches[i]->getPositions();
513  for (int j = 0; j < positions.cols(); j++)
514  {
515  double D = findDistance(positions.col(j), targetCoordinate_r);
516  if (D < minDistance)
517  {
518  minDistance = D;
519  minDistanceBranch = mBranches[i];
520  }
521  }
522  }
523 
524  return minDistanceBranch;
525 }
526 
545 vtkPolyDataPtr BranchList::createVtkPolyDataFromBranches(bool fullyConnected, bool straightBranches) const
546 {
547  vtkPolyDataPtr retval = vtkPolyDataPtr::New();
548  vtkPointsPtr points = vtkPointsPtr::New();
549  vtkCellArrayPtr lines = vtkCellArrayPtr::New();
550 
551  int positionCounter = 0;
552  for (size_t i = 0; i < mBranches.size(); ++i)
553  {
554  Eigen::MatrixXd positions = mBranches[i]->getPositions();
555  if(straightBranches)
556  {
557  ++positionCounter;
558  points->InsertNextPoint(positions(0,0),positions(1,0),positions(2,0));
559  points->InsertNextPoint(positions(0,positions.cols()-1),positions(1,positions.cols()-1),positions(2,positions.cols()-1));
560  vtkIdType connection[2] = {positionCounter-1, positionCounter};
561  lines->InsertNextCell(2, connection);
562  ++positionCounter;
563  }
564  else
565  {
566  for (int j = 0; j < positions.cols(); ++j)
567  {
568  ++positionCounter;
569  points->InsertNextPoint(positions(0,j),positions(1,j),positions(2,j));
570  if (j < positions.cols()-1)
571  {
572  vtkIdType connection[2] = {positionCounter-1, positionCounter};
573  lines->InsertNextCell(2, connection);
574  }
575  }
576  }
577  }
578  if(fullyConnected)
579  {
580  int this_branchs_first_point_in_full_polydata_point_list = 0;
581  for(size_t i = 0; i < mBranches.size(); ++i)
582  {
583  if(i>0)
584  {
585  if(!straightBranches)
586  this_branchs_first_point_in_full_polydata_point_list += mBranches[i-1]->getPositions().cols();
587  else
588  this_branchs_first_point_in_full_polydata_point_list += 2;
589  }
590  int parent_index_in_branch_list = mBranches[i]->findParentIndex(mBranches);
591 
592  if(parent_index_in_branch_list > -1)
593  {
594  int parent_branch_last_point_in_full_polydata = 0;
595  for(int j = 0; j <= parent_index_in_branch_list; ++j)
596  {
597  if(!straightBranches)
598  parent_branch_last_point_in_full_polydata += mBranches[j]->getPositions().cols() - 1;
599  else
600  parent_branch_last_point_in_full_polydata += (1 + j*2);
601  }
602  vtkIdType connection[2] = {parent_branch_last_point_in_full_polydata, this_branchs_first_point_in_full_polydata_point_list};
603  lines->InsertNextCell(2, connection);
604  }
605 
606  }
607 
608  }
609  retval->SetPoints(points);
610  retval->SetLines(lines);
611 
612  return retval;
613 }
614 
615 Eigen::MatrixXd sortMatrix(int rowNumber, Eigen::MatrixXd matrix)
616 {
617  for (int i = 0; i < matrix.cols() - 1; i++) {
618  for (int j = i + 1; j < matrix.cols(); j++) {
619  if (matrix(rowNumber,i) > matrix(rowNumber,j)){
620  matrix.col(i).swap(matrix.col(j));
621  }
622  }
623  }
624  return matrix;
625 }
626 
627 
628 
629 Eigen::MatrixXd eraseCol(int removeIndex, Eigen::MatrixXd positions)
630 {
631  positions.block(0 , removeIndex , positions.rows() , positions.cols() - removeIndex - 1) = positions.rightCols(positions.cols() - removeIndex - 1);
632  positions.conservativeResize(Eigen::NoChange, positions.cols() - 1);
633  return positions;
634 }
635 
636 std::pair<Eigen::MatrixXd::Index, double> dsearch(Eigen::Vector3d p, Eigen::MatrixXd positions)
637 {
638  Eigen::MatrixXd::Index index;
639  // find nearest neighbour
640  (positions.colwise() - p).colwise().squaredNorm().minCoeff(&index);
641  double d = (positions.col(index) - p).norm();
642 
643  return std::make_pair(index , d);
644 }
645 
646 std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd > dsearchn(Eigen::MatrixXd p1, Eigen::MatrixXd p2)
647 {
648  Eigen::MatrixXd::Index index;
649  std::vector<Eigen::MatrixXd::Index> indexVector;
650  Eigen::VectorXd D(p1.cols());
651  for (int i = 0; i < p1.cols(); i++)
652  {
653  // find nearest neighbour
654  (p2.colwise() - p1.col(i)).colwise().squaredNorm().minCoeff(&index);
655  D(i) = (p2.col(index) - p1.col(i)).norm();
656  indexVector.push_back(index);
657  }
658  return std::make_pair(indexVector , D);
659 }
660 
661 std::pair<Eigen::MatrixXd,Eigen::MatrixXd > findConnectedPointsInCT(int startIndex , Eigen::MatrixXd positionsNotUsed)
662 {
663  //Eigen::MatrixXd branchPositions(positionsNotUsed.rows(), positionsNotUsed.cols());
664  Eigen::MatrixXd thisPosition(3,1);
665  std::vector<Eigen::MatrixXd> branchPositionsVector;
666  thisPosition = positionsNotUsed.col(startIndex);
667  branchPositionsVector.push_back(thisPosition); //add first position to branch
668  positionsNotUsed = eraseCol(startIndex,positionsNotUsed);; //remove first position from list of remaining points
669 
670  while (positionsNotUsed.cols() > 0)
671  {
672  std::pair<Eigen::MatrixXd::Index, double > minDistance = dsearch(thisPosition, positionsNotUsed);
673  Eigen::MatrixXd::Index index = minDistance.first;
674  double d = minDistance.second;
675  if (d > 3) // more than 3 mm distance to closest point --> branch is compledted
676  break;
677 
678  thisPosition = positionsNotUsed.col(index);
679  positionsNotUsed = eraseCol(index,positionsNotUsed);
680  //add position to branch
681  branchPositionsVector.push_back(thisPosition);
682 
683  }
684 
685  Eigen::MatrixXd branchPositions(3,branchPositionsVector.size());
686 
687  for (int j = 0; j < branchPositionsVector.size(); j++)
688  {
689  branchPositions.block(0,j,3,1) = branchPositionsVector[j];
690  }
691 
692  return std::make_pair(branchPositions, positionsNotUsed);
693 }
694 
695 /*
696  smoothBranch is smoothing the positions of a centerline branch by using vtkCardinalSpline.
697  The degree of smoothing is dependent on the branch radius and the shape of the branch.
698  First, the method tests if a straight line from start to end of the branch is sufficient by the condition of
699  all positions along the line being within the lumen of the airway (max distance from original centerline
700  is set to branch radius).
701  If this fails, one more control point is added to the spline at the time, until the condition is fulfilled.
702  The control point added for each iteration is the position with the larges deviation from the original/unfiltered
703  centerline.
704 */
705 std::vector< Eigen::Vector3d > smoothBranch(BranchPtr branchPtr, int startIndex, Eigen::MatrixXd startPosition)
706 {
707  vtkCardinalSplinePtr splineX = vtkSmartPointer<vtkCardinalSpline>::New();
708  vtkCardinalSplinePtr splineY = vtkSmartPointer<vtkCardinalSpline>::New();
709  vtkCardinalSplinePtr splineZ = vtkSmartPointer<vtkCardinalSpline>::New();
710 
711  double branchRadius = branchPtr->findBranchRadius()/2;
712 
713  //add control points to spline
714 
715  //add first position
716  Eigen::MatrixXd positions = branchPtr->getPositions();
717  splineX->AddPoint(0,startPosition(0));
718  splineY->AddPoint(0,startPosition(1));
719  splineZ->AddPoint(0,startPosition(2));
720 
721  // Add last position if no parent branch, else add parents position closest to current branch.
722  // Branch positions are stored in order from head to feet (e.g. first position is top of trachea),
723  // while route-to-target is generated from target to top of trachea.
724  if(!branchPtr->getParentBranch())
725  {
726  splineX->AddPoint(startIndex,positions(0,0));
727  splineY->AddPoint(startIndex,positions(1,0));
728  splineZ->AddPoint(startIndex,positions(2,0));
729  }
730  else
731  {
732  Eigen::MatrixXd parentPositions = branchPtr->getParentBranch()->getPositions();
733  splineX->AddPoint(startIndex,parentPositions(0,parentPositions.cols()-1));
734  splineY->AddPoint(startIndex,parentPositions(1,parentPositions.cols()-1));
735  splineZ->AddPoint(startIndex,parentPositions(2,parentPositions.cols()-1));
736  }
737 
738  //Add points until all filtered/smoothed positions are minimum 1 mm inside the airway wall, (within r - 1 mm).
739  //This is to make sure the smoothed centerline is within the lumen of the airways.
740  double maxAcceptedDistanceToOriginalPosition = std::max(branchRadius - 1, 1.0);
741  double maxDistanceToOriginalPosition = maxAcceptedDistanceToOriginalPosition + 1;
742  int maxDistanceIndex = -1;
743  std::vector< Eigen::Vector3d > smoothingResult;
744 
745  //add positions to spline
746  while (maxDistanceToOriginalPosition >= maxAcceptedDistanceToOriginalPosition && splineX->GetNumberOfPoints() < startIndex)
747  {
748  if(maxDistanceIndex > 0)
749  {
750  //add to spline the position with largest distance to original position
751  splineX->AddPoint(maxDistanceIndex,positions(0,startIndex - maxDistanceIndex));
752  splineY->AddPoint(maxDistanceIndex,positions(1,startIndex - maxDistanceIndex));
753  splineZ->AddPoint(maxDistanceIndex,positions(2,startIndex - maxDistanceIndex));
754  }
755 
756  //evaluate spline - get smoothed positions
757  maxDistanceToOriginalPosition = 0.0;
758  smoothingResult.clear();
759  for(int j=0; j<=startIndex; j++)
760  {
761  double splineParameter = j;
762  Eigen::Vector3d tempPoint;
763  tempPoint(0) = splineX->Evaluate(splineParameter);
764  tempPoint(1) = splineY->Evaluate(splineParameter);
765  tempPoint(2) = splineZ->Evaluate(splineParameter);
766  smoothingResult.push_back(tempPoint);
767 
768  //calculate distance to original (non-filtered) position
769  double distance = dsearch(tempPoint, positions).second;
770  //finding the index with largest distance
771  if (distance > maxDistanceToOriginalPosition)
772  {
773  maxDistanceToOriginalPosition = distance;
774  maxDistanceIndex = j;
775  }
776  }
777  }
778 
779  return smoothingResult;
780 }
781 
782 }//namespace cx
cx::BranchList::deleteBranch
void deleteBranch(BranchPtr b)
Definition: cxBranchList.cpp:42
vtkCellArrayPtr
vtkSmartPointer< class vtkCellArray > vtkCellArrayPtr
Definition: vtkForwardDeclarations.h:41
cxLogger.h
cx::BranchList::interpolateBranchPositions
void interpolateBranchPositions(double resolution)
Definition: cxBranchList.cpp:238
cx::BranchList::setLapName
void setLapName(BranchPtr branch, QString name)
Definition: cxBranchList.cpp:481
cx::Branch
class org_custusx_registration_method_bronchoscopy_EXPORT Branch
Definition: cxBranch.h:26
cx::BranchList::smoothRadius
void smoothRadius()
Definition: cxBranchList.cpp:206
cx
Namespace for all CustusX production code.
Definition: cx_dev_group_definitions.h:13
MAX_ROTATION_TO_TILT_DOWN_DEGREES
#define MAX_ROTATION_TO_TILT_DOWN_DEGREES
Definition: cxBranchList.h:26
cx::dsearch
std::pair< Eigen::MatrixXd::Index, double > dsearch(Eigen::Vector3d p, Eigen::MatrixXd positions)
Definition: cxBranchList.cpp:636
cx::BranchList::BranchList
BranchList()
Definition: cxBranchList.cpp:25
cx::BranchList::smoothOrientations
void smoothOrientations()
Definition: cxBranchList.cpp:190
vtkCardinalSplinePtr
vtkSmartPointer< class vtkCardinalSpline > vtkCardinalSplinePtr
Definition: cxAccusurf.cpp:17
cxBranchList.h
cx::BranchList::findClosestBranch
BranchPtr findClosestBranch(Vector3D targetCoordinate_r)
Definition: cxBranchList.cpp:506
cx::BranchList::findDistance
double findDistance(Vector3D p1, Vector3D p2)
Definition: cxBranchList.cpp:495
cxVector3D.h
cx::BranchList::deleteAllBranches
void deleteAllBranches()
Definition: cxBranchList.cpp:57
cx::calculateBronchoscopeBendingDirection
Vector3D calculateBronchoscopeBendingDirection(Vector3D A, Vector3D B)
Definition: cxBranchList.cpp:165
cx::BranchList::smoothBranchPositions
void smoothBranchPositions(int controlPointDistance)
Definition: cxBranchList.cpp:284
cx::cross
Vector3D cross(const Vector3D &a, const Vector3D &b)
compute cross product of a and b.
Definition: cxVector3D.cpp:41
cx::BranchList::getBranches
std::vector< BranchPtr > getBranches()
Definition: cxBranchList.cpp:62
cx::BranchList::~BranchList
virtual ~BranchList()
Definition: cxBranchList.cpp:31
vtkCardinalSplinePtr
vtkSmartPointer< class vtkCardinalSpline > vtkCardinalSplinePtr
Definition: cxBranchList.cpp:20
cx::vtkPointsPtr
vtkSmartPointer< vtkPoints > vtkPointsPtr
Definition: cxCenterlineRegistration.h:41
cx::smoothBranch
std::vector< Eigen::Vector3d > smoothBranch(BranchPtr branchPtr, int startIndex, Eigen::MatrixXd startPosition)
Definition: cxBranchList.cpp:705
cx::BranchList::addBranch
void addBranch(BranchPtr b)
Definition: cxBranchList.cpp:37
cx::BranchList::createVtkPolyDataFromBranches
vtkPolyDataPtr createVtkPolyDataFromBranches(bool fullyConnected=false, bool straightBranches=false) const
BranchList::createVtkPolyDataFromBranches Return a VtkPolyData object created from the branches in th...
Definition: cxBranchList.cpp:545
cx::BranchList::calculateBronchoscopeRotation
void calculateBronchoscopeRotation(BranchPtr branch)
Definition: cxBranchList.cpp:97
cx::BranchList::findBranchWithLargestRadius
BranchPtr findBranchWithLargestRadius()
Definition: cxBranchList.cpp:221
cx::dsearchn
std::pair< std::vector< Eigen::MatrixXd::Index >, Eigen::VectorXd > dsearchn(Eigen::MatrixXd p1, Eigen::MatrixXd p2)
Definition: cxBranchList.cpp:646
cx::BranchList::selectGenerations
void selectGenerations(int maxGeneration)
Definition: cxBranchList.cpp:67
cxBranch.h
cx::findConnectedPointsInCT
std::pair< Eigen::MatrixXd, Eigen::MatrixXd > findConnectedPointsInCT(int startIndex, Eigen::MatrixXd positionsNotUsed)
Definition: cxBranchList.cpp:661
cx::BranchList::excludeClosePositionsInCTCenterline
void excludeClosePositionsInCTCenterline(double minPointDistance)
Definition: cxBranchList.cpp:453
cx::vtkPolyDataPtr
vtkSmartPointer< vtkPolyData > vtkPolyDataPtr
Definition: cxCenterlineRegistration.h:42
cx::BranchList::markLungLap
void markLungLap(QString name, Vector3D position)
Definition: cxBranchList.cpp:474
cx::ceil
Vector3D ceil(const Vector3D &a)
Definition: cxVector3D.cpp:84
cx::BranchList::findClosestLungLap
QString findClosestLungLap(Vector3D position)
Definition: cxBranchList.cpp:489
M_PI
#define M_PI
Definition: cxReconstructParams.cpp:25
cx::branchVector
std::vector< BranchPtr > branchVector
Definition: cxBranch.h:28
cx::BranchListPtr
boost::shared_ptr< class BranchList > BranchListPtr
Definition: cxBronchoscopePositionProjection.h:14
cx::eraseCol
Eigen::MatrixXd eraseCol(int removeIndex, Eigen::MatrixXd positions)
Definition: cxBranchList.cpp:629
cx::BranchList::findBranchesInCenterline
void findBranchesInCenterline(Eigen::MatrixXd positions_r, bool sortByZindex=true)
Definition: cxBranchList.cpp:327
cx::BranchPtr
boost::shared_ptr< class Branch > BranchPtr
Definition: cxBronchoscopePositionProjection.h:15
cx::BranchList::removePositionsForLocalRegistration
BranchListPtr removePositionsForLocalRegistration(Eigen::MatrixXd trackingPositions, double maxDistance)
Definition: cxBranchList.cpp:419
cx::similar
bool similar(const CameraInfo &lhs, const CameraInfo &rhs, double tol)
Definition: cxCameraStyleForView.cpp:506
cxMesh.h
cx::Vector3D
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:42
cx::bendingDirectionToBronchoscopeRotation
double bendingDirectionToBronchoscopeRotation(Vector3D bendingDirection, Vector3D parentBranchOrientation, double parentRotation)
Definition: cxBranchList.cpp:121
cx::sortMatrix
Eigen::MatrixXd sortMatrix(int rowNumber, Eigen::MatrixXd matrix)
Definition: cxBranchList.cpp:615
cx::BranchList::findBronchoscopeRotation
void findBronchoscopeRotation()
Definition: cxBranchList.cpp:90