CustusX  22.04-rc5
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 
26 {
27 
28 }
29 
30 
32 {
33  // for (int i = 0; i < mBranches.size(); i++)
34  // mBranches[i]->~Branch();
35 }
36 
38 {
39  mBranches.push_back(b);
40 }
41 
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 
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 
91 {
92  BranchPtr trachea = this->getBranches()[0];
93  if(trachea)
95 }
96 
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 
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 
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 
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 
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 
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 
492 vtkPolyDataPtr BranchList::createVtkPolyDataFromBranches(bool fullyConnected, bool straightBranches) const
493 {
494  vtkPolyDataPtr retval = vtkPolyDataPtr::New();
495  vtkPointsPtr points = vtkPointsPtr::New();
496  vtkCellArrayPtr lines = vtkCellArrayPtr::New();
497 
498  int positionCounter = 0;
499  for (size_t i = 0; i < mBranches.size(); ++i)
500  {
501  Eigen::MatrixXd positions = mBranches[i]->getPositions();
502  if(straightBranches)
503  {
504  ++positionCounter;
505  points->InsertNextPoint(positions(0,0),positions(1,0),positions(2,0));
506  points->InsertNextPoint(positions(0,positions.cols()-1),positions(1,positions.cols()-1),positions(2,positions.cols()-1));
507  vtkIdType connection[2] = {positionCounter-1, positionCounter};
508  lines->InsertNextCell(2, connection);
509  ++positionCounter;
510  }
511  else
512  {
513  for (int j = 0; j < positions.cols(); ++j)
514  {
515  ++positionCounter;
516  points->InsertNextPoint(positions(0,j),positions(1,j),positions(2,j));
517  if (j < positions.cols()-1)
518  {
519  vtkIdType connection[2] = {positionCounter-1, positionCounter};
520  lines->InsertNextCell(2, connection);
521  }
522  }
523  }
524  }
525  if(fullyConnected)
526  {
527  int this_branchs_first_point_in_full_polydata_point_list = 0;
528  for(size_t i = 0; i < mBranches.size(); ++i)
529  {
530  if(i>0)
531  {
532  if(!straightBranches)
533  this_branchs_first_point_in_full_polydata_point_list += mBranches[i-1]->getPositions().cols();
534  else
535  this_branchs_first_point_in_full_polydata_point_list += 2;
536  }
537  int parent_index_in_branch_list = mBranches[i]->findParentIndex(mBranches);
538 
539  if(parent_index_in_branch_list > -1)
540  {
541  int parent_branch_last_point_in_full_polydata = 0;
542  for(int j = 0; j <= parent_index_in_branch_list; ++j)
543  {
544  if(!straightBranches)
545  parent_branch_last_point_in_full_polydata += mBranches[j]->getPositions().cols() - 1;
546  else
547  parent_branch_last_point_in_full_polydata += (1 + j*2);
548  }
549  vtkIdType connection[2] = {parent_branch_last_point_in_full_polydata, this_branchs_first_point_in_full_polydata_point_list};
550  lines->InsertNextCell(2, connection);
551  }
552 
553  }
554 
555  }
556  retval->SetPoints(points);
557  retval->SetLines(lines);
558 
559  return retval;
560 }
561 
562 Eigen::MatrixXd sortMatrix(int rowNumber, Eigen::MatrixXd matrix)
563 {
564  for (int i = 0; i < matrix.cols() - 1; i++) {
565  for (int j = i + 1; j < matrix.cols(); j++) {
566  if (matrix(rowNumber,i) > matrix(rowNumber,j)){
567  matrix.col(i).swap(matrix.col(j));
568  }
569  }
570  }
571  return matrix;
572 }
573 
574 
575 
576 Eigen::MatrixXd eraseCol(int removeIndex, Eigen::MatrixXd positions)
577 {
578  positions.block(0 , removeIndex , positions.rows() , positions.cols() - removeIndex - 1) = positions.rightCols(positions.cols() - removeIndex - 1);
579  positions.conservativeResize(Eigen::NoChange, positions.cols() - 1);
580  return positions;
581 }
582 
583 std::pair<Eigen::MatrixXd::Index, double> dsearch(Eigen::Vector3d p, Eigen::MatrixXd positions)
584 {
585  Eigen::MatrixXd::Index index;
586  // find nearest neighbour
587  (positions.colwise() - p).colwise().squaredNorm().minCoeff(&index);
588  double d = (positions.col(index) - p).norm();
589 
590  return std::make_pair(index , d);
591 }
592 
593 std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd > dsearchn(Eigen::MatrixXd p1, Eigen::MatrixXd p2)
594 {
595  Eigen::MatrixXd::Index index;
596  std::vector<Eigen::MatrixXd::Index> indexVector;
597  Eigen::VectorXd D(p1.cols());
598  for (int i = 0; i < p1.cols(); i++)
599  {
600  // find nearest neighbour
601  (p2.colwise() - p1.col(i)).colwise().squaredNorm().minCoeff(&index);
602  D(i) = (p2.col(index) - p1.col(i)).norm();
603  indexVector.push_back(index);
604  }
605  return std::make_pair(indexVector , D);
606 }
607 
608 std::pair<Eigen::MatrixXd,Eigen::MatrixXd > findConnectedPointsInCT(int startIndex , Eigen::MatrixXd positionsNotUsed)
609 {
610  //Eigen::MatrixXd branchPositions(positionsNotUsed.rows(), positionsNotUsed.cols());
611  Eigen::MatrixXd thisPosition(3,1);
612  std::vector<Eigen::MatrixXd> branchPositionsVector;
613  thisPosition = positionsNotUsed.col(startIndex);
614  branchPositionsVector.push_back(thisPosition); //add first position to branch
615  positionsNotUsed = eraseCol(startIndex,positionsNotUsed);; //remove first position from list of remaining points
616 
617  while (positionsNotUsed.cols() > 0)
618  {
619  std::pair<Eigen::MatrixXd::Index, double > minDistance = dsearch(thisPosition, positionsNotUsed);
620  Eigen::MatrixXd::Index index = minDistance.first;
621  double d = minDistance.second;
622  if (d > 3) // more than 3 mm distance to closest point --> branch is compledted
623  break;
624 
625  thisPosition = positionsNotUsed.col(index);
626  positionsNotUsed = eraseCol(index,positionsNotUsed);
627  //add position to branch
628  branchPositionsVector.push_back(thisPosition);
629 
630  }
631 
632  Eigen::MatrixXd branchPositions(3,branchPositionsVector.size());
633 
634  for (int j = 0; j < branchPositionsVector.size(); j++)
635  {
636  branchPositions.block(0,j,3,1) = branchPositionsVector[j];
637  }
638 
639  return std::make_pair(branchPositions, positionsNotUsed);
640 }
641 
642 /*
643  smoothBranch is smoothing the positions of a centerline branch by using vtkCardinalSpline.
644  The degree of smoothing is dependent on the branch radius and the shape of the branch.
645  First, the method tests if a straight line from start to end of the branch is sufficient by the condition of
646  all positions along the line being within the lumen of the airway (max distance from original centerline
647  is set to branch radius).
648  If this fails, one more control point is added to the spline at the time, until the condition is fulfilled.
649  The control point added for each iteration is the position with the larges deviation from the original/unfiltered
650  centerline.
651 */
652 std::vector< Eigen::Vector3d > smoothBranch(BranchPtr branchPtr, int startIndex, Eigen::MatrixXd startPosition)
653 {
654  vtkCardinalSplinePtr splineX = vtkSmartPointer<vtkCardinalSpline>::New();
655  vtkCardinalSplinePtr splineY = vtkSmartPointer<vtkCardinalSpline>::New();
656  vtkCardinalSplinePtr splineZ = vtkSmartPointer<vtkCardinalSpline>::New();
657 
658  double branchRadius = branchPtr->findBranchRadius()/2;
659 
660  //add control points to spline
661 
662  //add first position
663  Eigen::MatrixXd positions = branchPtr->getPositions();
664  splineX->AddPoint(0,startPosition(0));
665  splineY->AddPoint(0,startPosition(1));
666  splineZ->AddPoint(0,startPosition(2));
667 
668  // Add last position if no parent branch, else add parents position closest to current branch.
669  // Branch positions are stored in order from head to feet (e.g. first position is top of trachea),
670  // while route-to-target is generated from target to top of trachea.
671  if(!branchPtr->getParentBranch())
672  {
673  splineX->AddPoint(startIndex,positions(0,0));
674  splineY->AddPoint(startIndex,positions(1,0));
675  splineZ->AddPoint(startIndex,positions(2,0));
676  }
677  else
678  {
679  Eigen::MatrixXd parentPositions = branchPtr->getParentBranch()->getPositions();
680  splineX->AddPoint(startIndex,parentPositions(0,parentPositions.cols()-1));
681  splineY->AddPoint(startIndex,parentPositions(1,parentPositions.cols()-1));
682  splineZ->AddPoint(startIndex,parentPositions(2,parentPositions.cols()-1));
683  }
684 
685  //Add points until all filtered/smoothed positions are minimum 1 mm inside the airway wall, (within r - 1 mm).
686  //This is to make sure the smoothed centerline is within the lumen of the airways.
687  double maxAcceptedDistanceToOriginalPosition = std::max(branchRadius - 1, 1.0);
688  double maxDistanceToOriginalPosition = maxAcceptedDistanceToOriginalPosition + 1;
689  int maxDistanceIndex = -1;
690  std::vector< Eigen::Vector3d > smoothingResult;
691 
692  //add positions to spline
693  while (maxDistanceToOriginalPosition >= maxAcceptedDistanceToOriginalPosition && splineX->GetNumberOfPoints() < startIndex)
694  {
695  if(maxDistanceIndex > 0)
696  {
697  //add to spline the position with largest distance to original position
698  splineX->AddPoint(maxDistanceIndex,positions(0,startIndex - maxDistanceIndex));
699  splineY->AddPoint(maxDistanceIndex,positions(1,startIndex - maxDistanceIndex));
700  splineZ->AddPoint(maxDistanceIndex,positions(2,startIndex - maxDistanceIndex));
701  }
702 
703  //evaluate spline - get smoothed positions
704  maxDistanceToOriginalPosition = 0.0;
705  smoothingResult.clear();
706  for(int j=0; j<=startIndex; j++)
707  {
708  double splineParameter = j;
709  Eigen::Vector3d tempPoint;
710  tempPoint(0) = splineX->Evaluate(splineParameter);
711  tempPoint(1) = splineY->Evaluate(splineParameter);
712  tempPoint(2) = splineZ->Evaluate(splineParameter);
713  smoothingResult.push_back(tempPoint);
714 
715  //calculate distance to original (non-filtered) position
716  double distance = dsearch(tempPoint, positions).second;
717  //finding the index with largest distance
718  if (distance > maxDistanceToOriginalPosition)
719  {
720  maxDistanceToOriginalPosition = distance;
721  maxDistanceIndex = j;
722  }
723  }
724  }
725 
726  return smoothingResult;
727 }
728 
729 }//namespace cx
std::vector< Eigen::Vector3d > smoothBranch(BranchPtr branchPtr, int startIndex, Eigen::MatrixXd startPosition)
std::pair< Eigen::MatrixXd, Eigen::MatrixXd > findConnectedPointsInCT(int startIndex, Eigen::MatrixXd positionsNotUsed)
virtual ~BranchList()
Vector3D ceil(const Vector3D &a)
Definition: cxVector3D.cpp:84
Eigen::MatrixXd eraseCol(int removeIndex, Eigen::MatrixXd positions)
vtkSmartPointer< class vtkCellArray > vtkCellArrayPtr
double bendingDirectionToBronchoscopeRotation(Vector3D bendingDirection, Vector3D parentBranchOrientation, double parentRotation)
boost::shared_ptr< class BranchList > BranchListPtr
BranchListPtr removePositionsForLocalRegistration(Eigen::MatrixXd trackingPositions, double maxDistance)
vtkSmartPointer< vtkPoints > vtkPointsPtr
void calculateBronchoscopeRotation(BranchPtr branch)
void addBranch(BranchPtr b)
void deleteAllBranches()
boost::shared_ptr< class Branch > BranchPtr
vtkSmartPointer< class vtkCardinalSpline > vtkCardinalSplinePtr
Vector3D cross(const Vector3D &a, const Vector3D &b)
compute cross product of a and b.
Definition: cxVector3D.cpp:41
void findBranchesInCenterline(Eigen::MatrixXd positions_r, bool sortByZindex=true)
void findBronchoscopeRotation()
vtkSmartPointer< class vtkCardinalSpline > vtkCardinalSplinePtr
Definition: cxAccusurf.cpp:17
vtkPolyDataPtr createVtkPolyDataFromBranches(bool fullyConnected=false, bool straightBranches=false) const
BranchList::createVtkPolyDataFromBranches Return a VtkPolyData object created from the branches in th...
#define MAX_ROTATION_TO_TILT_DOWN_DEGREES
Definition: cxBranchList.h:26
void interpolateBranchPositions(double resolution)
Vector3D calculateBronchoscopeBendingDirection(Vector3D A, Vector3D B)
void smoothBranchPositions(int controlPointDistance)
std::vector< BranchPtr > getBranches()
void excludeClosePositionsInCTCenterline(double minPointDistance)
BranchPtr findBranchWithLargestRadius()
void deleteBranch(BranchPtr b)
vtkSmartPointer< vtkPolyData > vtkPolyDataPtr
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:42
class org_custusx_registration_method_bronchoscopy_EXPORT Branch
Definition: cxBranch.h:26
bool similar(const CameraInfo &lhs, const CameraInfo &rhs, double tol)
void selectGenerations(int maxGeneration)
Eigen::MatrixXd sortMatrix(int rowNumber, Eigen::MatrixXd matrix)
std::pair< std::vector< Eigen::MatrixXd::Index >, Eigen::VectorXd > dsearchn(Eigen::MatrixXd p1, Eigen::MatrixXd p2)
void smoothOrientations()
#define M_PI
std::pair< Eigen::MatrixXd::Index, double > dsearch(Eigen::Vector3d p, Eigen::MatrixXd positions)
std::vector< BranchPtr > branchVector
Definition: cxBranch.h:28
Namespace for all CustusX production code.