Fraxinus  16.5.0-fx-rc3
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_r)
208 {
209  positions_r = sortMatrix(2,positions_r);
210  Eigen::MatrixXd positionsNotUsed_r = positions_r;
211 
212 // int minIndex;
213  int index;
214  int splitIndex;
215  Eigen::MatrixXd::Index startIndex;
216  BranchPtr branchToSplit;
217  while (positionsNotUsed_r.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_r, 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_r.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_r.cols() - 1;
241 
242  std::pair<Eigen::MatrixXd,Eigen::MatrixXd > connectedPointsResult = findConnectedPointsInCT(startIndex , positionsNotUsed_r);
243  Eigen::MatrixXd branchPositions = connectedPointsResult.first;
244  positionsNotUsed_r = 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  if(branchToSplit->getParentBranch())
281  branchToSplit->getParentBranch()->addChildBranch(newBranch);
282  }
283  else if (branchToSplit->getPositions().cols() - splitIndex - 1 < 5)
284  // If the new branch is close to the end of the existing
285  // branch: Connect it to the end of the existing branch
286  {
287  newBranch->setParentBranch(branchToSplit);
288  branchToSplit->addChildBranch(newBranch);
289  }
290 
291  }
292 
293  }
294  }
295 }
296 
297 
298 BranchListPtr BranchList::removePositionsForLocalRegistration(Eigen::MatrixXd trackingPositions, double maxDistance)
299 {
300  BranchListPtr retval = BranchListPtr(new BranchList());
301  BranchPtr b;
302  for (int i = 0; i < mBranches.size(); i++)
303  {
304  b = BranchPtr(new Branch());
305  b->setPositions(mBranches[i]->getPositions());
306  b->setOrientations(mBranches[i]->getOrientations());
307  retval->addBranch(b);
308  }
309 
310  std::vector<BranchPtr> branches = retval->getBranches();
311  Eigen::MatrixXd positions;
312  Eigen::MatrixXd orientations;
313  for (int i = 0; i < branches.size(); i++)
314  {
315  positions = branches[i]->getPositions();
316  orientations = branches[i]->getOrientations();
317  std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd> distanceData;
318  distanceData = dsearchn(positions, trackingPositions);
319  Eigen::VectorXd distance = distanceData.second;
320  for (int j = positions.cols() - 1; j >= 0; j--)
321  {
322  if (distance(j) > maxDistance)
323  {
324  positions = eraseCol(j, positions);
325  orientations = eraseCol(j, orientations);
326  }
327  }
328  branches[i]->setPositions(positions);
329  branches[i]->setOrientations(orientations);
330  }
331  return retval;
332 }
333 
334 Eigen::MatrixXd sortMatrix(int rowNumber, Eigen::MatrixXd matrix)
335 {
336  for (int i = 0; i < matrix.cols() - 1; i++) {
337  for (int j = i + 1; j < matrix.cols(); j++) {
338  if (matrix(rowNumber,i) > matrix(rowNumber,j)){
339  matrix.col(i).swap(matrix.col(j));
340  }
341  }
342  }
343 return matrix;
344 }
345 
346 
347 
348 Eigen::MatrixXd eraseCol(int removeIndex, Eigen::MatrixXd positions)
349 {
350  positions.block(0 , removeIndex , positions.rows() , positions.cols() - removeIndex - 1) = positions.rightCols(positions.cols() - removeIndex - 1);
351  positions.conservativeResize(Eigen::NoChange, positions.cols() - 1);
352  return positions;
353 }
354 
355 std::pair<Eigen::MatrixXd::Index, double> dsearch(Eigen::Vector3d p, Eigen::MatrixXd positions)
356 {
357  Eigen::MatrixXd::Index index;
358  // find nearest neighbour
359  (positions.colwise() - p).colwise().squaredNorm().minCoeff(&index);
360  double d = (positions.col(index) - p).norm();
361 
362  return std::make_pair(index , d);
363 }
364 
365 std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd > dsearchn(Eigen::MatrixXd p1, Eigen::MatrixXd p2)
366 {
367  Eigen::MatrixXd::Index index;
368  std::vector<Eigen::MatrixXd::Index> indexVector;
369  Eigen::VectorXd D(p1.cols());
370  for (int i = 0; i < p1.cols(); i++)
371  {
372  // find nearest neighbour
373  (p2.colwise() - p1.col(i)).colwise().squaredNorm().minCoeff(&index);
374  D(i) = (p2.col(index) - p1.col(i)).norm();
375  indexVector.push_back(index);
376  }
377  return std::make_pair(indexVector , D);
378 }
379 
380 std::pair<Eigen::MatrixXd,Eigen::MatrixXd > findConnectedPointsInCT(int startIndex , Eigen::MatrixXd positionsNotUsed)
381 {
382  //Eigen::MatrixXd branchPositions(positionsNotUsed.rows(), positionsNotUsed.cols());
383  Eigen::MatrixXd thisPosition(3,1);
384  std::vector<Eigen::MatrixXd> branchPositionsVector;
385  thisPosition = positionsNotUsed.col(startIndex);
386  branchPositionsVector.push_back(thisPosition); //add first position to branch
387  positionsNotUsed = eraseCol(startIndex,positionsNotUsed);; //remove first position from list of remaining points
388 
389  while (positionsNotUsed.cols() > 0)
390  {
391  std::pair<Eigen::MatrixXd::Index, double > minDistance = dsearch(thisPosition, positionsNotUsed);
392  Eigen::MatrixXd::Index index = minDistance.first;
393  double d = minDistance.second;
394  if (d > 3) // more than 3 mm distance to closest point --> branch is compledted
395  break;
396 
397  thisPosition = positionsNotUsed.col(index);
398  positionsNotUsed = eraseCol(index,positionsNotUsed);
399  //add position to branch
400  branchPositionsVector.push_back(thisPosition);
401 
402  }
403 
404  Eigen::MatrixXd branchPositions(3,branchPositionsVector.size());
405 
406  for (int j = 0; j < branchPositionsVector.size(); j++)
407  {
408  branchPositions.block(0,j,3,1) = branchPositionsVector[j];
409  }
410 
411  return std::make_pair(branchPositions, positionsNotUsed);
412 }
413 
414 
415 }//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 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)
void findBranchesInCenterline(Eigen::MatrixXd positions_r)
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