NorMIT-nav  16.5
An IGT application
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
cxRouteToTarget.cpp
Go to the documentation of this file.
1 
2 
3 #include "cxRouteToTarget.h"
4 #include <vtkPolyData.h>
5 //#include "cxDoubleDataAdapterXml.h"
6 #include "cxBranchList.h"
7 #include "cxBranch.h"
8 #include <vtkCellArray.h>
9 #include "vtkCardinalSpline.h"
10 
11 typedef vtkSmartPointer<class vtkCardinalSpline> vtkCardinalSplinePtr;
12 
13 namespace cx
14 {
15 
17  mBranchListPtr(new BranchList),
18  mProjectedIndex(0)
19 {
20 }
21 
22 
24 {
25 }
26 
28 {
29  mCLpoints = this->getCenterlinePositions(centerline);
30 }
31 
33 {
34 
35  int N = centerline->GetNumberOfPoints();
36  Eigen::MatrixXd CLpoints(3,N);
37  for(vtkIdType i = 0; i < N; i++)
38  {
39  double p[3];
40  centerline->GetPoint(i,p);
41  Eigen::Vector3d position;
42  position(0) = p[0]; position(1) = p[1]; position(2) = p[2];
43  CLpoints.block(0 , i , 3 , 1) = position;
44  }
45  return CLpoints;
46 }
47 
49 {
50  if (mBranchListPtr)
51  mBranchListPtr->deleteAllBranches();
52 
53  Eigen::MatrixXd CLpoints = getCenterlinePositions(centerline);
54 
55  mBranchListPtr->findBranchesInCenterline(CLpoints);
56 
57  mBranchListPtr->calculateOrientations();
58  mBranchListPtr->smoothOrientations();
59 
60  std::cout << "Number of branches in CT centerline: " << mBranchListPtr->getBranches().size() << std::endl;
61 }
62 
63 
65 {
66 
67  double minDistance = 100000;
68  int minDistancePositionIndex;
69  BranchPtr minDistanceBranch;
70  std::vector<BranchPtr> branches = mBranchListPtr->getBranches();
71  for (int i = 0; i < branches.size(); i++)
72  {
73  Eigen::MatrixXd positions = branches[i]->getPositions();
74  for (int j = 0; j < positions.cols(); j++)
75  {
76  double D = findDistance(positions.col(j), targetCoordinate);
77  if (D < minDistance)
78  {
79  minDistance = D;
80  minDistanceBranch = branches[i];
81  minDistancePositionIndex = j;
82  }
83  }
84  }
85 
86  mProjectedBranchPtr = minDistanceBranch;
87  mProjectedIndex = minDistancePositionIndex;
88 }
89 
90 
92 {
93  mRoutePositions.clear();
94 
95  searchBranchUp(mProjectedBranchPtr, mProjectedIndex);
96 }
97 
98 void RouteToTarget::searchBranchUp(BranchPtr searchBranchPtr, int startIndex)
99 {
100  Eigen::MatrixXd positions = searchBranchPtr->getPositions();
101 
102  for (int i = startIndex; i>=0; i--)
103  mRoutePositions.push_back(positions.col(i));
104 
105  BranchPtr parentBranchPtr = searchBranchPtr->getParentBranch();
106  if (parentBranchPtr)
107  searchBranchUp(parentBranchPtr, parentBranchPtr->getPositions().cols()-1);
108 }
109 
110 
112 {
113 
114  findClosestPointInBranches(targetCoordinate);
116 
117  smoothPositions();
118 
119  vtkPolyDataPtr retval = addVTKPoints();
120 
121  return retval;
122 }
123 
125 {
126  vtkPolyDataPtr retval = vtkPolyDataPtr::New();
127  vtkPointsPtr points = vtkPointsPtr::New();
128  vtkCellArrayPtr lines = vtkCellArrayPtr::New();
129  int numberOfPositions = mRoutePositions.size();
130  for (int j = numberOfPositions - 1; j >= 0; j--)
131  {
132  points->InsertNextPoint(mRoutePositions[j](0),mRoutePositions[j](1),mRoutePositions[j](2));
133  }
134  for (int j = 0; j < numberOfPositions-1; j++)
135  {
136  vtkIdType connection[2] = {j, j+1};
137  lines->InsertNextCell(2, connection);
138  }
139  retval->SetPoints(points);
140  retval->SetLines(lines);
141  return retval;
142 }
143 
144 void RouteToTarget::smoothPositions()
145 {
146  int numberOfInputPoints = mRoutePositions.size();
147  int controlPointFactor = 10;
148  int numberOfControlPoints = numberOfInputPoints / controlPointFactor;
149 
150  vtkCardinalSplinePtr splineX = vtkSmartPointer<vtkCardinalSpline>::New();
151  vtkCardinalSplinePtr splineY = vtkSmartPointer<vtkCardinalSpline>::New();
152  vtkCardinalSplinePtr splineZ = vtkSmartPointer<vtkCardinalSpline>::New();
153 
154  if (numberOfControlPoints >= 2)
155  {
156  //add control points to spline
157  for(int j=0; j<numberOfControlPoints; j++)
158  {
159  int indexP = (j*numberOfInputPoints)/numberOfControlPoints;
160  splineX->AddPoint(indexP,mRoutePositions[indexP](0));
161  splineY->AddPoint(indexP,mRoutePositions[indexP](1));
162  splineZ->AddPoint(indexP,mRoutePositions[indexP](2));
163  }
164  //Always add the last point to complete spline
165  splineX->AddPoint(numberOfInputPoints-1,mRoutePositions[numberOfInputPoints-1](0));
166  splineY->AddPoint(numberOfInputPoints-1,mRoutePositions[numberOfInputPoints-1](1));
167  splineZ->AddPoint(numberOfInputPoints-1,mRoutePositions[numberOfInputPoints-1](2));
168 
169 
170  //evaluate spline - get smoothed positions
171  std::vector< Eigen::Vector3d > smoothingResult;
172  for(int j=0; j<numberOfInputPoints; j++)
173  {
174  double splineParameter = j;
175  Eigen::Vector3d tempPoint;
176  tempPoint(0) = splineX->Evaluate(splineParameter);
177  tempPoint(1) = splineY->Evaluate(splineParameter);
178  tempPoint(2) = splineZ->Evaluate(splineParameter);
179  smoothingResult.push_back(tempPoint);
180  }
181  mRoutePositions.clear();
182  mRoutePositions = smoothingResult;
183  }
184 }
185 
186 
187 double findDistance(Eigen::MatrixXd p1, Eigen::MatrixXd p2)
188 {
189  double d0 = p1(0) - p2(0);
190  double d1 = p1(1) - p2(1);
191  double d2 = p1(2) - p2(2);
192 
193  double D = sqrt( d0*d0 + d1*d1 + d2*d2 );
194 
195  return D;
196 }
197 
198 
199 } /* namespace cx */
void findClosestPointInBranches(Vector3D targetCoordinate)
vtkSmartPointer< class vtkCellArray > vtkCellArrayPtr
void setCenterline(vtkPolyDataPtr centerline)
void processCenterline(vtkPolyDataPtr centerline)
boost::shared_ptr< class Branch > BranchPtr
double findDistance(Eigen::MatrixXd p1, Eigen::MatrixXd p2)
vtkSmartPointer< class vtkPolyData > vtkPolyDataPtr
void searchBranchUp(BranchPtr searchBranchPtr, int startIndex)
vtkSmartPointer< class vtkCardinalSpline > vtkCardinalSplinePtr
vtkPolyDataPtr findRouteToTarget(Vector3D targetCoordinate)
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:63
vtkPolyDataPtr addVTKPoints()
vtkSmartPointer< class vtkPoints > vtkPointsPtr
Eigen::MatrixXd getCenterlinePositions(vtkPolyDataPtr centerline)