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