CustusX  2021.04.21-dev+develop.337f9
An IGT application
cxBronchoscopePositionProjection.cpp
Go to the documentation of this file.
1 
2 
4 #include <vtkPolyData.h>
5 #include "cxDoubleProperty.h"
6 #include "cxBranchList.h"
7 #include "cxBranch.h"
8 #include <boost/math/special_functions/fpclassify.hpp> // isnan
9 #include "cxLogger.h"
10 
11 
12 namespace cx
13 {
14 
16  isPreviousProjectedPointSet(false),
17  mBranchListPtr(new BranchList),
18  mProjectedIndex(0),
19  mUseAdvancedCenterlineProjection(false)
20 {
21 }
22 
24  isPreviousProjectedPointSet(false),
25  mProjectedIndex(0),
26  mUseAdvancedCenterlineProjection(false)
27 {
28  mCLpoints = this->getCenterlinePositions(centerline, prMd);
29 }
30 
32 {
33 }
34 
36 {
37  mRunFromWidget = runFromWidget;
38 }
39 
40 void BronchoscopePositionProjection::setAdvancedCenterlineOption(bool useAdvancedCenterlineProjection)
41 {
42  mUseAdvancedCenterlineProjection = useAdvancedCenterlineProjection;
43 }
44 
46 {
47  mMaxDistanceToCenterline = DoubleProperty::initialize("Max distance to centerline (mm)", "",
48  "Set max distance to centerline in mm", 30, DoubleRange(1, 100, 1), 0,
49  root);
50  mMaxDistanceToCenterline->setGuiRepresentation(DoubleProperty::grSLIDER);
51 }
52 
54 {
55  return mMaxDistanceToCenterline;
56 }
57 
59 {
60  if (mRunFromWidget && mMaxDistanceToCenterline->getValue())
61  mMaxDistanceToCenterlineValue = mMaxDistanceToCenterline->getValue();
62 
63  return mMaxDistanceToCenterlineValue;
64 }
65 
67 {
68  mMaxSearchDistance = DoubleProperty::initialize("Max search distance along centerline (mm)", "",
69  "Set max search distance along centerline in mm", 20, DoubleRange(1, 100, 1), 0,
70  root);
71  mMaxSearchDistance->setGuiRepresentation(DoubleProperty::grSLIDER);
72 }
73 
75 {
76  return mMaxSearchDistance;
77 }
78 
80 {
81  if (mRunFromWidget && mMaxSearchDistance->getValue())
82  mMaxSearchDistanceValue = mMaxSearchDistance->getValue();
83 
84  return mMaxSearchDistanceValue;
85 }
86 
88 {
89  mAlpha = DoubleProperty::initialize("Alpha ", "",
90  "Weighting of position and orientation in projection (high alpha is giving orientations high weight).", 1.0, DoubleRange(0, 10, 0.1), 1, root);
91  mAlpha->setGuiRepresentation(DoubleProperty::grSLIDER);
92 }
93 
95 {
96  return mAlpha;
97 }
98 
100 {
101  if (mRunFromWidget && mAlpha->getValue())
102  mAlphaValue = mAlpha->getValue();
103 
104  return mAlphaValue;
105 }
106 
108 {
109 
110  int N = centerline->GetNumberOfPoints();
111  Eigen::MatrixXd CLpoints(3,N);
112  for(vtkIdType i = 0; i < N; i++)
113  {
114  double p[3];
115  centerline->GetPoint(i,p);
116  Eigen::Vector3d position;
117  position(0) = p[0]; position(1) = p[1]; position(2) = p[2];
118  CLpoints.block(0 , i , 3 , 1) = rMd.coord(position);
119  }
120  return CLpoints;
121 }
122 
124 {
125  m_rMpr = rMpr;
126  if (mBranchListPtr)
127  mBranchListPtr->deleteAllBranches();
128 
129  mCLpoints = getCenterlinePositions(centerline, rMd);
130 
131  mBranchListPtr->findBranchesInCenterline(mCLpoints);
132  //mBranchListPtr->interpolateBranchPositions(10);
133  //mBranchListPtr->smoothBranchPositions();
134  mBranchListPtr->smoothOrientations();
135 
136  std::cout << "Number of branches in CT centerline: " << mBranchListPtr->getBranches().size() << std::endl;
137 }
138 
139 //Can be used instead of processCenterline(...) if you have a preprosessed branchList to be used in the registration process.
141 {
142  if (!branchList)
143  return;
144 
145  m_rMpr = rMpr;
146  mBranchListPtr = branchList;
147 }
148 
149 //Used when run without widget
151 {
152  mMaxDistanceToCenterlineValue = maxDistance;
153 }
154 
155 //Used when run without widget
157 {
158  mMaxSearchDistanceValue = maxDistance;
159 }
160 
161 //Used when run without widget
163 {
164  mAlphaValue = alpha;
165 }
166 
168 {
169  Transform3D rMt = m_rMpr * prMt;
170  Eigen::VectorXd toolPos = rMt.matrix().topRightCorner(3 , 1);
171  Eigen::MatrixXd::Index index;
172  Transform3D new_rMt = rMt;
173 
174  Eigen::VectorXd P(mCLpoints.cols());
175  for (int i = 0; i < mCLpoints.cols(); i++)
176  {
177  double p0 = ( mCLpoints(0,i) - toolPos(0) );
178  double p1 = ( mCLpoints(1,i) - toolPos(1) );
179  double p2 = ( mCLpoints(2,i) - toolPos(2) );
180 
181  P(i) = sqrt( p0*p0 + p1*p1 + p2*p2 );
182  }
183 
184  P.minCoeff(&index);
185  if (P.minCoeff() < maxDistance)
186  {
187  new_rMt.matrix().topRightCorner(3 , 1) = mCLpoints.col(index);
188  }
189 
190  Transform3D new_prMt = m_rMpr.inverse() * new_rMt;
191  return new_prMt;
192 }
193 
195 {
196  Transform3D rMt = m_rMpr * prMt;
197  Eigen::VectorXd toolPos = rMt.matrix().topRightCorner(3 , 1);
198  Transform3D new_rMt = rMt;
199 
200  double minDistance = 100000;
201  int minDistancePositionIndex;
202  BranchPtr minDistanceBranch;
203  std::vector<BranchPtr> branches = mBranchListPtr->getBranches();
204  for (int i = 0; i < branches.size(); i++)
205  {
206  Eigen::MatrixXd positions = branches[i]->getPositions();
207  for (int j = 0; j < positions.cols(); j++)
208  {
209  double D = findDistance(positions.col(j), toolPos);
210  if (D < minDistance)
211  {
212  minDistance = D;
213  minDistanceBranch = branches[i];
214  minDistancePositionIndex = j;
215  }
216  }
217  }
218 
219  if (minDistance < maxDistance)
220  {
221  Eigen::MatrixXd positions = minDistanceBranch->getPositions();
222  new_rMt.matrix().topRightCorner(3 , 1) = positions.col(minDistancePositionIndex);
223  mProjectedBranchPtr = minDistanceBranch;
224  mProjectedIndex = minDistancePositionIndex;
225  isPreviousProjectedPointSet = true;
226  //new_rMt = updateProjectedCameraOrientation(new_rMt, mProjectedBranchPtr, mProjectedIndex);
227  }
228  else
229  {
230  isPreviousProjectedPointSet = false;
231  }
232 
233  Transform3D new_prMt = m_rMpr.inverse() * new_rMt;
234  return new_prMt;
235 }
236 
238 {
239  Transform3D rMt = m_rMpr * prMt;
240  Eigen::VectorXd toolPos = rMt.matrix().topRightCorner(3 , 1);
241  Eigen::VectorXd toolOrientation = rMt.matrix().block(0 , 2 , 3 , 1);
242  Transform3D new_rMt = rMt;
243 
244  double minDistance = 100000;
245  int minDistancePositionIndex;
246  BranchPtr minDistanceBranch;
247  for (int i = 0; i < mSearchBranchPtrVector.size(); i++)
248  {
249  Eigen::MatrixXd positions = mSearchBranchPtrVector[i]->getPositions();
250  Eigen::MatrixXd orientations = mSearchBranchPtrVector[i]->getOrientations();
251 
252  //double D = findDistance(positions.col(mSearchIndexVector[i]), toolPos);
253  double alpha = getAlphaValue();
254  double D = findDistanceWithOrientation(positions.col(mSearchIndexVector[i]), toolPos, orientations.col(mSearchIndexVector[i]), toolOrientation, alpha);
255  if (D < minDistance)
256  {
257  minDistance = D;
258  minDistanceBranch = mSearchBranchPtrVector[i];
259  minDistancePositionIndex = mSearchIndexVector[i];
260  }
261  }
262  if (minDistance < maxDistance)
263  {
264  Eigen::MatrixXd positions = minDistanceBranch->getPositions();
265  new_rMt.matrix().topRightCorner(3 , 1) = positions.col(minDistancePositionIndex);
266  mProjectedBranchPtr = minDistanceBranch;
267  mProjectedIndex = minDistancePositionIndex;
268  isPreviousProjectedPointSet = true;
269  //new_rMt = updateProjectedCameraOrientation(new_rMt, mProjectedBranchPtr, mProjectedIndex);
270  }
271  else
272  {
273  isPreviousProjectedPointSet = false;
274  }
275 
276  Transform3D new_prMt = m_rMpr.inverse() * new_rMt;
277  return new_prMt;
278 
279 }
280 
282 {
283  mSearchBranchPtrVector.clear();
284  mSearchIndexVector.clear();
285  mSearchBranchPtrVector.push_back(mProjectedBranchPtr);
286  mSearchIndexVector.push_back(mProjectedIndex);
287  double currentSearchDistance = 0;
288 
289  Eigen::MatrixXd positions = mProjectedBranchPtr->getPositions();
290  if (mProjectedIndex < positions.cols() - 1)
291  {
292  currentSearchDistance = findDistance( positions.col(mProjectedIndex), positions.col(mProjectedIndex+1) );
293  searchBranchDown(mProjectedBranchPtr, mProjectedIndex + 1, currentSearchDistance, maxSearchDistance);
294  }
295  if (mProjectedIndex > 0)
296  {
297  currentSearchDistance = findDistance( positions.col(mProjectedIndex), positions.col(mProjectedIndex-1) );
298  searchBranchUp(mProjectedBranchPtr, mProjectedIndex - 1, currentSearchDistance, maxSearchDistance);
299  }
300 
301 }
302 
303 void BronchoscopePositionProjection::searchBranchUp(BranchPtr searchBranchPtr, int startIndex, double currentSearchDistance, double maxSearchDistance)
304 {
305  Eigen::MatrixXd positions = searchBranchPtr->getPositions();
306  mSearchBranchPtrVector.push_back(searchBranchPtr);
307  mSearchIndexVector.push_back(startIndex);
308  for (int i = startIndex-1; i>=0; i--)
309  {
310  currentSearchDistance = currentSearchDistance + findDistance( positions.col(i+1), positions.col(i) );
311  if (currentSearchDistance < maxSearchDistance)
312  {
313  mSearchBranchPtrVector.push_back(searchBranchPtr);
314  mSearchIndexVector.push_back(i);
315  }
316  else
317  return;
318  }
319  BranchPtr parentBranchPtr = searchBranchPtr->getParentBranch();
320 
321  if (parentBranchPtr)
322  {
323  std::vector<BranchPtr> childBranches = parentBranchPtr->getChildBranches();
324  searchBranchUp(parentBranchPtr, parentBranchPtr->getPositions().cols()-1, currentSearchDistance, maxSearchDistance);
325  for (int i = 0; i < childBranches.size(); i++)
326  if (childBranches[i] != searchBranchPtr)
327  searchBranchDown(childBranches[i], 0, currentSearchDistance, maxSearchDistance);
328  }
329 }
330 
331 void BronchoscopePositionProjection::searchBranchDown(BranchPtr searchBranchPtr, int startIndex, double currentSearchDistance, double maxSearchDistance)
332 {
333  Eigen::MatrixXd positions = searchBranchPtr->getPositions();
334  mSearchBranchPtrVector.push_back(searchBranchPtr);
335  mSearchIndexVector.push_back(startIndex);
336  for (int i = startIndex+1; i<positions.cols(); i++)
337  {
338  currentSearchDistance = currentSearchDistance + findDistance( positions.col(i), positions.col(i-1) );
339  if (currentSearchDistance < maxSearchDistance)
340  {
341  mSearchBranchPtrVector.push_back(searchBranchPtr);
342  mSearchIndexVector.push_back(i);
343  }
344  else
345  return;
346  }
347  std::vector<BranchPtr> childBranches = searchBranchPtr->getChildBranches();
348  for (int i = 0; i < childBranches.size(); i++)
349  searchBranchDown(childBranches[i], 0,currentSearchDistance,maxSearchDistance);
350 }
351 
352 Transform3D BronchoscopePositionProjection::findProjectedPoint(Transform3D prMt, double maxDistance, double maxSearchDistance)
353 {
354  //double maxSearchDistance = 25; //mm
355  Transform3D new_prMt;
356  if (isPreviousProjectedPointSet)
357  {
358  findSearchPositions(maxSearchDistance);
359  new_prMt = findClosestPointInSearchPositions(prMt, maxDistance);
360  }
361  else
362  new_prMt = findClosestPointInBranches(prMt, maxDistance);
363 
364  return new_prMt;
365 }
366 
368 {
369  return mUseAdvancedCenterlineProjection;
370 }
371 
372 double findDistance(Eigen::MatrixXd p1, Eigen::MatrixXd p2)
373 {
374  double d0 = p1(0) - p2(0);
375  double d1 = p1(1) - p2(1);
376  double d2 = p1(2) - p2(2);
377 
378  double D = sqrt( d0*d0 + d1*d1 + d2*d2 );
379 
380  return D;
381 }
382 
383 double findDistanceWithOrientation(Eigen::VectorXd position1, Eigen::VectorXd position2, Eigen::VectorXd orientation1, Eigen::VectorXd orientation2, double alpha)
384 {
385  double d0 = position1(0) - position2(0);
386  double d1 = position1(1) - position2(1);
387  double d2 = position1(2) - position2(2);
388  double o0 = fmod( orientation2(0) - orientation1(0) , 2 );
389  double o1 = fmod( orientation2(1) - orientation1(1) , 2 );
390  double o2 = fmod( orientation2(2) - orientation1(2) , 2 );
391 
392  double P = sqrt( d0*d0 + d1*d1 + d2*d2 );
393  double O = sqrt( o0*o0 + o1*o1 + o2*o2 );
394 
395  if (boost::math::isnan( O ) )
396  O = 4;
397 
398  if ( (o0>2) || (o1>2) || (o2>2) )
399  std::cout << "Warning in bronchoscopyRegistration.cpp: Error on oriantation calculation in dsearch2n. Orientation > 2." << std::endl;
400 
401  double D = P + alpha * O;
402 
403  return D;
404 }
405 
407 {
408  Eigen::MatrixXd branchPositions = branch->getPositions();
409  int numberOfPositions = branchPositions.cols();
410  int lookBackIndex; // = std::max(0 , index-20);
411  Vector3D lookBackPosition;
412 // if (index >=20){
413 // lookBackIndex = index-20;
414 // lookBackPosition = branchPositions.col(lookBackIndex);
415 // }
416 // else if (branch->getParentBranch())
417 // {
418 // Eigen::MatrixXd parentBranchPositions = branch->getParentBranch()->getPositions();
419 // lookBackIndex = parentBranchPositions.cols() - 1 - index - 20;
420 // lookBackIndex = std::max(0 , index-20);
421 // }
422 // else
423 // {
424 // lookBackIndex = 0;
425 // lookBackPosition = branchPositions.col(lookBackIndex);
426 // }
427 
428  lookBackIndex = std::max(0 , index-20);
429  lookBackPosition = branchPositions.col(lookBackIndex);
430  int lookForwardIndex = std::min(numberOfPositions-1 , index+20);
431  Vector3D lookForwardPosition = branchPositions.col(lookForwardIndex);
432 
433  Vector3D viewDirection = (lookForwardPosition - lookBackPosition).normalized();
434 
435  prMt.matrix().col(2).head(3) = viewDirection;
436 
437  return prMt;
438 }
439 
440 } /* namespace cx */
void setBranchList(BranchListPtr branchList, Transform3D rMpr)
Transform3D findClosestPointInSearchPositions(Transform3D prMt, double maxDistance)
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
Eigen::MatrixXd getCenterlinePositions(vtkPolyDataPtr centerline, Transform3D rMd)
boost::shared_ptr< class BranchList > BranchListPtr
Utility class for describing a bounded numeric range.
Definition: cxDoubleRange.h:32
void searchBranchUp(BranchPtr searchBranchPtr, int startIndex, double currentSearchDistance, double maxSearchDistance)
void processCenterline(vtkPolyDataPtr centerline, Transform3D rMd, Transform3D rMpr)
Transform3D findClosestPointInBranches(Transform3D prMt, double maxDistance)
Transform3D updateProjectedCameraOrientation(Transform3D prMt, BranchPtr branch, int index)
boost::shared_ptr< class Branch > BranchPtr
double findDistance(Eigen::MatrixXd p1, Eigen::MatrixXd p2)
Transform3D findClosestPoint(Transform3D prMt, double maxDistance)
double findDistanceWithOrientation(Eigen::VectorXd position1, Eigen::VectorXd position2, Eigen::VectorXd orientation1, Eigen::VectorXd orientation2, double alpha)
vtkSmartPointer< vtkPolyData > vtkPolyDataPtr
boost::shared_ptr< class DoubleProperty > DoublePropertyPtr
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:42
void searchBranchDown(BranchPtr searchBranchPtr, int startIndex, double currentSearchDistance, double maxSearchDistance)
static DoublePropertyPtr initialize(const QString &uid, QString name, QString help, double value, DoubleRange range, int decimals, QDomNode root=QDomNode())
Transform3D findProjectedPoint(Transform3D prMt, double maxDistance, double maxSearchDistance)
void setAdvancedCenterlineOption(bool useAdvancedCenterlineProjection)
Namespace for all CustusX production code.