NorMIT-nav  16.5
An IGT application
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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) 2008-2014, SINTEF Department of Medical Technology
5 All rights reserved.
6 
7 Redistribution and use in source and binary forms, with or without
8 modification, are permitted provided that the following conditions are met:
9 
10 1. Redistributions of source code must retain the above copyright notice,
11  this list of conditions and the following disclaimer.
12 
13 2. Redistributions in binary form must reproduce the above copyright notice,
14  this list of conditions and the following disclaimer in the documentation
15  and/or other materials provided with the distribution.
16 
17 3. Neither the name of the copyright holder nor the names of its contributors
18  may be used to endorse or promote products derived from this software
19  without specific prior written permission.
20 
21 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
25 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
26 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 =========================================================================*/
32 #include "cxBranchList.h"
33 #include "cxBranch.h"
34 #include "cxMesh.h"
35 #include "cxVector3D.h"
36 #include <vtkPolyData.h>
37 #include "vtkCardinalSpline.h"
38 
39 typedef vtkSmartPointer<class vtkCardinalSpline> vtkCardinalSplinePtr;
40 
41 namespace cx
42 {
43 
45 {
46 
47 }
48 
49 
51 {
52 // for (int i = 0; i < mBranches.size(); i++)
53 // mBranches[i]->~Branch();
54 }
55 
57 {
58  mBranches.push_back(b);
59 }
60 
62 {
63  for( int i = 0; i < mBranches.size(); i++ )
64  {
65  if (b == mBranches[i])
66  {
67  mBranches.erase(mBranches.begin() + i);
68  return;
69  }
70  }
71 }
72 
74 {
75  mBranches.clear();
76 }
77 
78 std::vector<BranchPtr> BranchList::getBranches()
79 {
80  return mBranches;
81 }
82 
83 void BranchList::selectGenerations(int maxGeneration)
84 {
85  std::vector<int> branchNumbersToBeDeleted;
86  for( int i = 0; i < mBranches.size(); i++ )
87  {
88  int generationCounter = 1;
89  BranchPtr currentBranch = mBranches[i];
90  while (currentBranch->getParentBranch()){
91  generationCounter++;
92  currentBranch = currentBranch->getParentBranch();
93  if (generationCounter > maxGeneration)
94  {
95  branchNumbersToBeDeleted.push_back(i);
96  break;
97  }
98  }
99 
100  }
101 
102  for ( int i = branchNumbersToBeDeleted.size() - 1; i >= 0; i-- )
103  deleteBranch(mBranches[branchNumbersToBeDeleted[i]]);
104 }
105 
106 
108 {
109  for (int i = 0; i < mBranches.size(); i++)
110  {
111  Eigen::MatrixXd positions = mBranches[i]->getPositions();
112  Eigen::MatrixXd diff = positions.rightCols(positions.cols() - 1) - positions.leftCols(positions.cols() - 1);
113  Eigen::MatrixXd orientations(positions.rows(),positions.cols());
114  orientations.leftCols(orientations.cols() - 1) = diff / diff.norm();
115  orientations.rightCols(1) = orientations.col(orientations.cols() - 1);
116  mBranches[i]->setOrientations(orientations);
117  }
118 }
119 
121 {
122  for (int i = 0; i < mBranches.size(); i++)
123  {
124  Eigen::MatrixXd orientations = mBranches[i]->getOrientations();
125  Eigen::MatrixXd newOrientations(orientations.rows(),orientations.cols());
126  int numberOfColumns = orientations.cols();
127  for (int j = 0; j < numberOfColumns; j++)
128  {
129  newOrientations.col(j) = orientations.block(0,std::max(j-2,0),orientations.rows(),std::min(5,numberOfColumns-j)).rowwise().mean(); //smoothing
130  newOrientations.col(j) = newOrientations.col(j) / newOrientations.col(j).norm(); // normalizing
131  }
132  mBranches[i]->setOrientations(newOrientations);
133  }
134 }
135 
136 void BranchList::interpolateBranchPositions(int interpolationFactor){
137 
138  for (int i = 0; i < mBranches.size(); i++)
139  {
140  Eigen::MatrixXd positions = mBranches[i]->getPositions();
141  std::vector<Eigen::Vector3d> interpolatedPositions;
142  for (int j = 0; j < positions.cols()-1; j++)
143  {
144  for (int k = 0; k < interpolationFactor; k++){
145  Eigen::Vector3d interpolationPoint;
146  interpolationPoint[0] = (positions(0,j)*(interpolationFactor-k) + positions(0,j+1)*(k) ) / interpolationFactor;
147  interpolationPoint[1] = (positions(1,j)*(interpolationFactor-k) + positions(1,j+1)*(k) ) / interpolationFactor;
148  interpolationPoint[2] = (positions(2,j)*(interpolationFactor-k) + positions(2,j+1)*(k) ) / interpolationFactor;
149  interpolatedPositions.push_back(interpolationPoint);
150  }
151  }
152  Eigen::MatrixXd interpolationResult(3 , interpolatedPositions.size());
153  for (int j = 0; j < interpolatedPositions.size(); j++)
154  {
155  interpolationResult(0,j) = interpolatedPositions[j](0);
156  interpolationResult(1,j) = interpolatedPositions[j](1);
157  interpolationResult(2,j) = interpolatedPositions[j](2);
158  }
159  mBranches[i]->setPositions(interpolationResult);
160  }
161 
162 }
163 
165 {
166  for (int i = 0; i < mBranches.size(); i++)
167  {
168  Eigen::MatrixXd positions = mBranches[i]->getPositions();
169  int numberOfInputPoints = positions.cols();
170  int controlPointFactor = 10;
171  int numberOfControlPoints = numberOfInputPoints / controlPointFactor;
172 
173  vtkCardinalSplinePtr splineX = vtkSmartPointer<vtkCardinalSpline>::New();
174  vtkCardinalSplinePtr splineY = vtkSmartPointer<vtkCardinalSpline>::New();
175  vtkCardinalSplinePtr splineZ = vtkSmartPointer<vtkCardinalSpline>::New();
176 
177  if (numberOfControlPoints >= 2)
178  {
179  //add control points to spline
180  for(int j=0; j<numberOfControlPoints; j++)
181  {
182  int indexP = (j*numberOfInputPoints)/numberOfControlPoints;
183 
184  splineX->AddPoint(indexP,positions(0,indexP));
185  splineY->AddPoint(indexP,positions(1,indexP));
186  splineZ->AddPoint(indexP,positions(2,indexP));
187  }
188  //Always add the last point to complete spline
189  splineX->AddPoint(numberOfInputPoints-1,positions(0,numberOfInputPoints-1));
190  splineY->AddPoint(numberOfInputPoints-1,positions(1,numberOfInputPoints-1));
191  splineZ->AddPoint(numberOfInputPoints-1,positions(2,numberOfInputPoints-1));
192 
193  //evaluate spline - get smoothed positions
194  Eigen::MatrixXd smoothingResult(3 , numberOfInputPoints);
195  for(int j=0; j<numberOfInputPoints; j++)
196  {
197  double splineParameter = j;
198  smoothingResult(0,j) = splineX->Evaluate(splineParameter);
199  smoothingResult(1,j) = splineY->Evaluate(splineParameter);
200  smoothingResult(2,j) = splineZ->Evaluate(splineParameter);
201  }
202  mBranches[i]->setPositions(smoothingResult);
203  }
204  }
205 }
206 
207 void BranchList::findBranchesInCenterline(Eigen::MatrixXd positions)
208 {
209  positions = sortMatrix(2,positions);
210  Eigen::MatrixXd positionsNotUsed = positions;
211 
212 // int minIndex;
213  int index;
214  int splitIndex;
215  Eigen::MatrixXd::Index startIndex;
216  BranchPtr branchToSplit;
217  while (positionsNotUsed.cols() > 0)
218  {
219  if (!mBranches.empty())
220  {
221  double minDistance = 1000;
222  for (int i = 0; i < mBranches.size(); i++)
223  {
224  std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd> distances;
225  distances = dsearchn(positionsNotUsed, mBranches[i]->getPositions());
226  double d = distances.second.minCoeff(&index);
227  if (d < minDistance)
228  {
229  minDistance = d;
230  branchToSplit = mBranches[i];
231  startIndex = index;
232  if (minDistance < 2)
233  break;
234  }
235  }
236  std::pair<Eigen::MatrixXd::Index, double> dsearchResult = dsearch(positionsNotUsed.col(startIndex) , branchToSplit->getPositions());
237  splitIndex = dsearchResult.first;
238  }
239  else //if this is the first branch. Select the top position (Trachea).
240  startIndex = positionsNotUsed.cols() - 1;
241 
242  std::pair<Eigen::MatrixXd,Eigen::MatrixXd > connectedPointsResult = findConnectedPointsInCT(startIndex , positionsNotUsed);
243  Eigen::MatrixXd branchPositions = connectedPointsResult.first;
244  positionsNotUsed = connectedPointsResult.second;
245 
246  if (branchPositions.cols() >= 5) //only include brances of length >= 5 points
247  {
248  BranchPtr newBranch = BranchPtr(new Branch());
249  newBranch->setPositions(branchPositions);
250  mBranches.push_back(newBranch);
251 
252  if (mBranches.size() > 1) // do not try to split another branch when the first branch is processed
253  {
254  if ((splitIndex + 1 >= 5) && (branchToSplit->getPositions().cols() - splitIndex - 1 >= 5))
255  //do not split branch if the new branch is close to the edge of the branch
256  //if the new branch is not close to one of the edges of the
257  //connected existing branch: Split the existing branch
258  {
259  BranchPtr newBranchFromSplit = BranchPtr(new Branch());
260  Eigen::MatrixXd branchToSplitPositions = branchToSplit->getPositions();
261  newBranchFromSplit->setPositions(branchToSplitPositions.rightCols(branchToSplitPositions.cols() - splitIndex - 1));
262  branchToSplit->setPositions(branchToSplitPositions.leftCols(splitIndex + 1));
263  mBranches.push_back(newBranchFromSplit);
264  newBranchFromSplit->setParentBranch(branchToSplit);
265  newBranch->setParentBranch(branchToSplit);
266  newBranchFromSplit->setChildBranches(branchToSplit->getChildBranches());
267  branchVector branchToSplitChildren = branchToSplit->getChildBranches();
268  for (int i = 0; i < branchToSplitChildren.size(); i++)
269  branchToSplitChildren[i]->setParentBranch(newBranchFromSplit);
270  branchToSplit->deleteChildBranches();
271  branchToSplit->addChildBranch(newBranchFromSplit);
272  branchToSplit->addChildBranch(newBranch);
273  }
274  else if (splitIndex + 1 < 5)
275  // If the new branch is close to the start of the existing
276  // branch: Connect it to the same position start as the
277  // existing branch
278  {
279  newBranch->setParentBranch(branchToSplit->getParentBranch());
280  branchToSplit->getParentBranch()->addChildBranch(newBranch);
281  }
282  else if (branchToSplit->getPositions().cols() - splitIndex - 1 < 5)
283  // If the new branch is close to the end of the existing
284  // branch: Connect it to the end of the existing branch
285  {
286  newBranch->setParentBranch(branchToSplit);
287  branchToSplit->addChildBranch(newBranch);
288  }
289 
290  }
291 
292  }
293  }
294 }
295 
296 
297 BranchListPtr BranchList::removePositionsForLocalRegistration(Eigen::MatrixXd trackingPositions, double maxDistance)
298 {
299  BranchListPtr retval = BranchListPtr(new BranchList());
300  BranchPtr b;
301  for (int i = 0; i < mBranches.size(); i++)
302  {
303  b = BranchPtr(new Branch());
304  b->setPositions(mBranches[i]->getPositions());
305  b->setOrientations(mBranches[i]->getOrientations());
306  retval->addBranch(b);
307  }
308 
309  std::vector<BranchPtr> branches = retval->getBranches();
310  Eigen::MatrixXd positions;
311  Eigen::MatrixXd orientations;
312  for (int i = 0; i < branches.size(); i++)
313  {
314  positions = branches[i]->getPositions();
315  orientations = branches[i]->getOrientations();
316  std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd> distanceData;
317  distanceData = dsearchn(positions, trackingPositions);
318  Eigen::VectorXd distance = distanceData.second;
319  for (int j = positions.cols() - 1; j >= 0; j--)
320  {
321  if (distance(j) > maxDistance)
322  {
323  positions = eraseCol(j, positions);
324  orientations = eraseCol(j, orientations);
325  }
326  }
327  branches[i]->setPositions(positions);
328  branches[i]->setOrientations(orientations);
329  }
330  return retval;
331 }
332 
333 Eigen::MatrixXd sortMatrix(int rowNumber, Eigen::MatrixXd matrix)
334 {
335  for (int i = 0; i < matrix.cols() - 1; i++) {
336  for (int j = i + 1; j < matrix.cols(); j++) {
337  if (matrix(rowNumber,i) > matrix(rowNumber,j)){
338  matrix.col(i).swap(matrix.col(j));
339  }
340  }
341  }
342 return matrix;
343 }
344 
345 
346 
347 Eigen::MatrixXd eraseCol(int removeIndex, Eigen::MatrixXd positions)
348 {
349  positions.block(0 , removeIndex , positions.rows() , positions.cols() - removeIndex - 1) = positions.rightCols(positions.cols() - removeIndex - 1);
350  positions.conservativeResize(Eigen::NoChange, positions.cols() - 1);
351  return positions;
352 }
353 
354 std::pair<Eigen::MatrixXd::Index, double> dsearch(Eigen::Vector3d p, Eigen::MatrixXd positions)
355 {
356  Eigen::MatrixXd::Index index;
357  // find nearest neighbour
358  (positions.colwise() - p).colwise().squaredNorm().minCoeff(&index);
359  double d = (positions.col(index) - p).norm();
360 
361  return std::make_pair(index , d);
362 }
363 
364 std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd > dsearchn(Eigen::MatrixXd p1, Eigen::MatrixXd p2)
365 {
366  Eigen::MatrixXd::Index index;
367  std::vector<Eigen::MatrixXd::Index> indexVector;
368  Eigen::VectorXd D(p1.cols());
369  for (int i = 0; i < p1.cols(); i++)
370  {
371  // find nearest neighbour
372  (p2.colwise() - p1.col(i)).colwise().squaredNorm().minCoeff(&index);
373  D(i) = (p2.col(index) - p1.col(i)).norm();
374  indexVector.push_back(index);
375  }
376  return std::make_pair(indexVector , D);
377 }
378 
379 std::pair<Eigen::MatrixXd,Eigen::MatrixXd > findConnectedPointsInCT(int startIndex , Eigen::MatrixXd positionsNotUsed)
380 {
381  //Eigen::MatrixXd branchPositions(positionsNotUsed.rows(), positionsNotUsed.cols());
382  Eigen::MatrixXd thisPosition(3,1);
383  std::vector<Eigen::MatrixXd> branchPositionsVector;
384  thisPosition = positionsNotUsed.col(startIndex);
385  branchPositionsVector.push_back(thisPosition); //add first position to branch
386  positionsNotUsed = eraseCol(startIndex,positionsNotUsed);; //remove first position from list of remaining points
387 
388  while (positionsNotUsed.cols() > 0)
389  {
390  std::pair<Eigen::MatrixXd::Index, double > minDistance = dsearch(thisPosition, positionsNotUsed);
391  Eigen::MatrixXd::Index index = minDistance.first;
392  double d = minDistance.second;
393  if (d > 3) // more than 3 mm distance to closest point --> branch is compledted
394  break;
395 
396  thisPosition = positionsNotUsed.col(index);
397  positionsNotUsed = eraseCol(index,positionsNotUsed);
398  //add position to branch
399  branchPositionsVector.push_back(thisPosition);
400 
401  }
402 
403  Eigen::MatrixXd branchPositions(3,branchPositionsVector.size());
404 
405  for (int j = 0; j < branchPositionsVector.size(); j++)
406  {
407  branchPositions.block(0,j,3,1) = branchPositionsVector[j];
408  }
409 
410  return std::make_pair(branchPositions, positionsNotUsed);
411 }
412 
413 
414 }//namespace cx
virtual ~BranchList()
std::pair< Eigen::MatrixXd, Eigen::MatrixXd > findConnectedPointsInCT(int startIndex, Eigen::MatrixXd positionsNotUsed)
void interpolateBranchPositions(int interpolationFactor)
void smoothBranchPositions()
boost::shared_ptr< class BranchList > BranchListPtr
BranchListPtr removePositionsForLocalRegistration(Eigen::MatrixXd trackingPositions, double maxDistance)
void findBranchesInCenterline(Eigen::MatrixXd positions)
void addBranch(BranchPtr b)
void deleteAllBranches()
boost::shared_ptr< class Branch > BranchPtr
vtkSmartPointer< class vtkCardinalSpline > vtkCardinalSplinePtr
std::pair< Eigen::MatrixXd::Index, double > dsearch(Eigen::Vector3d p, Eigen::MatrixXd positions)
void calculateOrientations()
vtkSmartPointer< class vtkCardinalSpline > vtkCardinalSplinePtr
std::vector< BranchPtr > getBranches()
void deleteBranch(BranchPtr b)
class org_custusx_registration_method_bronchoscopy_EXPORT Branch
Definition: cxBranch.h:47
std::pair< std::vector< Eigen::MatrixXd::Index >, Eigen::VectorXd > dsearchn(Eigen::MatrixXd p1, Eigen::MatrixXd p2)
Eigen::MatrixXd eraseCol(int removeIndex, Eigen::MatrixXd positions)
void selectGenerations(int maxGeneration)
Eigen::MatrixXd sortMatrix(int rowNumber, Eigen::MatrixXd matrix)
void smoothOrientations()
std::vector< BranchPtr > branchVector
Definition: cxBranch.h:49