PlusLib  2.9.0
Software library for tracked ultrasound image acquisition, calibration, and processing.
PlusFidLineFinder.cxx
Go to the documentation of this file.
1 /*=Plus=header=begin======================================================
2 Program: Plus
3 Copyright (c) Laboratory for Percutaneous Surgery. All rights reserved.
4 See License.txt for details.
5 =========================================================Plus=header=end*/
6 
7 #include "PlusConfigure.h"
8 #include "PlusFidLineFinder.h"
9 #include "igsioMath.h"
10 #include "PlusFidSegmentation.h"
11 #include "vtkMath.h"
12 #include <algorithm>
13 
14 #include "vnl/vnl_vector.h"
15 #include "vnl/vnl_matrix.h"
16 #include "vnl/vnl_cross.h"
17 #include "vnl/algo/vnl_qr.h"
18 #include "vnl/algo/vnl_svd.h"
19 
20 //-----------------------------------------------------------------------------
21 
23 {
24  m_FrameSize[0] = -1;
25  m_FrameSize[1] = -1;
26  m_FrameSize[2] = -1;
28 
29  for (int i = 0 ; i < 6 ; i++)
30  {
32  }
33 
34  for (int i = 0 ; i < 16 ; i++)
35  {
37  }
38 
40 
42 
43  m_MinThetaRad = -1.0;
44  m_MaxThetaRad = -1.0;
45 }
46 
47 //-----------------------------------------------------------------------------
48 
50 {
51 
52 }
53 
54 //-----------------------------------------------------------------------------
55 
57 {
58  LOG_TRACE("FidLineFinder::ComputeParameters");
59 
60  //Computation of MinTheta and MaxTheta from the physical probe rotation range
61  std::vector<PlusNWire> nWires;
62 
63  for (unsigned int i = 0 ; i < m_Patterns.size() ; i++)
64  {
65  if (typeid(m_Patterns.at(i)) == typeid(PlusNWire)) //if it is a NWire
66  {
67  PlusNWire* tempNWire = (PlusNWire*)(&(m_Patterns.at(i)));
68  nWires.push_back(*tempNWire);
69  }
70  }
71 
72  double* imageNormalVectorInPhantomFrameMaximumRotationAngleDeg = GetImageNormalVectorInPhantomFrameMaximumRotationAngleDeg();
73  std::vector<double> thetaXrad, thetaYrad, thetaZrad;
74  thetaXrad.push_back(vtkMath::RadiansFromDegrees(imageNormalVectorInPhantomFrameMaximumRotationAngleDeg[0]));
75  thetaXrad.push_back(0);
76  thetaXrad.push_back(vtkMath::RadiansFromDegrees(imageNormalVectorInPhantomFrameMaximumRotationAngleDeg[1]));
77  thetaYrad.push_back(vtkMath::RadiansFromDegrees(imageNormalVectorInPhantomFrameMaximumRotationAngleDeg[2]));
78  thetaYrad.push_back(0);
79  thetaYrad.push_back(vtkMath::RadiansFromDegrees(imageNormalVectorInPhantomFrameMaximumRotationAngleDeg[3]));
80  thetaZrad.push_back(vtkMath::RadiansFromDegrees(imageNormalVectorInPhantomFrameMaximumRotationAngleDeg[4]));
81  thetaZrad.push_back(0);
82  thetaZrad.push_back(vtkMath::RadiansFromDegrees(imageNormalVectorInPhantomFrameMaximumRotationAngleDeg[5]));
83 
84  vnl_matrix<double> imageToPhantomTransform(4, 4);
85 
86  for (int i = 0 ; i < 4 ; i++)
87  {
88  for (int j = 0 ; j < 4 ; j++)
89  {
90  imageToPhantomTransform.put(i, j, GetImageToPhantomTransform()[j + 4 * i]);
91  }
92  }
93 
94  vnl_vector<double> pointA(3), pointB(3), pointC(3);
95 
96  //3 points from the Nwires
97  for (int i = 0; i < 3 ; i++)
98  {
99  pointA.put(i, nWires[0].GetWires()[0].EndPointFront[i]);
100  pointB.put(i, nWires[0].GetWires()[0].EndPointBack[i]);
101  pointC.put(i, nWires[0].GetWires()[1].EndPointFront[i]);
102  }
103 
104  //create 2 vectors out of them
105  vnl_vector<double> AB(3);
106  AB = pointB - pointA;
107  vnl_vector<double> AC(3);
108  AC = pointC - pointA;
109 
110  vnl_vector<double> normalVectorInPhantomCoord(3);
111  normalVectorInPhantomCoord = vnl_cross_3d(AB, AC); //Get the vector normal to the wire plane
112 
113  vnl_vector<double> normalVectorInPhantomCoordExtended(4, 0);
114 
115  for (unsigned int i = 0 ; i < normalVectorInPhantomCoord.size() ; i++)
116  {
117  normalVectorInPhantomCoordExtended.put(i, normalVectorInPhantomCoord.get(i));
118  }
119 
120  vnl_vector<double> normalImagePlane(3, 0); //vector normal to the image plane
121  normalImagePlane.put(2, 1);
122 
123  vnl_vector<double> imageYunitVector(3, 0);
124  imageYunitVector.put(1, 1); //(0,1,0)
125 
126  std::vector<double> finalAngleTable;
127 
128  for (int i = 0 ; i < 3 ; i++)
129  {
130  for (int j = 0 ; j < 3 ; j++)
131  {
132  for (int k = 0 ; k < 3 ; k++)
133  {
134  vnl_matrix<double> totalRotation(4, 4, 0); //The overall rotation matrix (after applying rotation around X, Y and Z axis
135  double tX = thetaYrad[i];
136  double tY = thetaYrad[j];
137  double tZ = thetaYrad[k];
138  totalRotation.put(0, 0, cos(tY)*cos(tZ));
139  totalRotation.put(0, 1, -cos(tX)*sin(tZ) + sin(tX)*sin(tY)*cos(tZ));
140  totalRotation.put(0, 2, sin(tX)*sin(tZ) + cos(tX)*sin(tY)*cos(tZ));
141  totalRotation.put(1, 0, cos(tY)*sin(tZ));
142  totalRotation.put(1, 1, cos(tX)*cos(tZ) + sin(tX)*sin(tY)*sin(tZ));
143  totalRotation.put(1, 2, -sin(tX)*cos(tZ) + cos(tX)*sin(tY)*sin(tZ));
144  totalRotation.put(2, 0, -sin(tY));
145  totalRotation.put(2, 1, sin(tX)*cos(tY));
146  totalRotation.put(2, 2, cos(tX)*cos(tY));
147  totalRotation.put(3, 3, 1);
148 
149  vnl_matrix<double> totalTranform(4, 4); //The overall tranform matrix: Rotation*ImageToPhantom
150  totalTranform = totalRotation * imageToPhantomTransform;
151 
152  vnl_vector<double> normalVectorInImageCoordExtended(4); //extended because it is a 4 dimension vector (result from a 4x4 matrix)
153 
154  //Get the normal vector in image coordinates by applying the total transform to the normal vector in phantom coordinates
155  normalVectorInImageCoordExtended = totalTranform * normalVectorInPhantomCoordExtended;
156 
157  vnl_vector<double> normalVectorInImageCoord(3); //Make it 3 dimensions, the 4th is a useless value only needed for 4x4 matrix operations
158 
159  for (unsigned int i = 0 ; i < normalVectorInImageCoord.size() ; i++)
160  {
161  normalVectorInImageCoord.put(i, normalVectorInImageCoordExtended.get(i));
162  }
163 
164  vnl_vector<double> lineDirectionVector(3);
165  lineDirectionVector = vnl_cross_3d(normalVectorInImageCoord, normalImagePlane);
166 
167  double dotProductValue = dot_product(lineDirectionVector, imageYunitVector);
168  double normOfLineDirectionvector = lineDirectionVector.two_norm();
169  double angle = acos(dotProductValue / normOfLineDirectionvector); //get the angle between the line direction vector and the (0,1,0) vector
170  finalAngleTable.push_back(angle);
171  }
172  }
173  }
174 
175  //Get the maximum and the minimum angle from the table
176  m_MaxThetaRad = (*std::max_element(finalAngleTable.begin(), finalAngleTable.end()));
177  m_MinThetaRad = (*std::min_element(finalAngleTable.begin(), finalAngleTable.end()));
178 }
179 
180 //-----------------------------------------------------------------------------
181 
182 PlusStatus PlusFidLineFinder::ReadConfiguration(vtkXMLDataElement* configData)
183 {
184  LOG_TRACE("FidLineFinder::ReadConfiguration");
185 
186  XML_FIND_NESTED_ELEMENT_OPTIONAL(segmentationParameters, configData, "Segmentation");
187  if (!segmentationParameters)
188  {
189  segmentationParameters = igsioXmlUtils::GetNestedElementWithName(configData, "Segmentation");
191  }
192 
193  XML_READ_SCALAR_ATTRIBUTE_WARNING(double, ApproximateSpacingMmPerPixel, segmentationParameters);
194 
195  //if the tolerance parameters are computed automatically
196  int computeSegmentationParametersFromPhantomDefinition(0);
197  if (segmentationParameters->GetScalarAttribute("ComputeSegmentationParametersFromPhantomDefinition", computeSegmentationParametersFromPhantomDefinition)
198  && computeSegmentationParametersFromPhantomDefinition != 0)
199  {
200  XML_FIND_NESTED_ELEMENT_REQUIRED(phantomDefinition, segmentationParameters, "PhantomDefinition");
201  XML_FIND_NESTED_ELEMENT_REQUIRED(customTransforms, segmentationParameters, "CustomTransforms");
202  XML_READ_VECTOR_ATTRIBUTE_WARNING(double, 16, ImageToPhantomTransform, customTransforms);
203 
204  XML_READ_VECTOR_ATTRIBUTE_WARNING(double, 6, ImageNormalVectorInPhantomFrameMaximumRotationAngleDeg, segmentationParameters);
205  XML_READ_SCALAR_ATTRIBUTE_WARNING(double, CollinearPointsMaxDistanceFromLineMm, segmentationParameters);
206 
208  }
209  else
210  {
211  XML_READ_SCALAR_ATTRIBUTE_WARNING(double, MinThetaDegrees, segmentationParameters);
212  XML_READ_SCALAR_ATTRIBUTE_WARNING(double, MaxThetaDegrees, segmentationParameters);
213  XML_READ_SCALAR_ATTRIBUTE_WARNING(double, CollinearPointsMaxDistanceFromLineMm, segmentationParameters);
214  }
215 
216  return PLUS_SUCCESS;
217 }
218 
219 //-----------------------------------------------------------------------------
220 
221 std::vector<PlusNWire> PlusFidLineFinder::GetNWires()
222 {
223  std::vector<PlusNWire> nWires;
224 
225  for (unsigned int i = 0 ; i < m_Patterns.size() ; i++)
226  {
227  PlusNWire* tempNWire = (PlusNWire*)((m_Patterns.at(i)));
228  if (tempNWire)
229  {
230  nWires.push_back(*tempNWire);
231  }
232  }
233 
234  return nWires;
235 }
236 
237 //-----------------------------------------------------------------------------
238 
239 void PlusFidLineFinder::SetFrameSize(const FrameSizeType& frameSize)
240 {
241  LOG_TRACE("FidLineFinder::SetFrameSize(" << frameSize[0] << ", " << frameSize[1] << ")");
242 
243  if ((m_FrameSize[0] == frameSize[0]) && (m_FrameSize[1] == frameSize[1]))
244  {
245  return;
246  }
247 
248  m_FrameSize[0] = frameSize[0];
249  m_FrameSize[1] = frameSize[1];
250  m_FrameSize[2] = 1;
251 }
252 
253 //-----------------------------------------------------------------------------
254 
256 {
257  //LOG_TRACE("FidLineFinder::ComputeSlope");
258 
259  double x1 = dot1.GetX();
260  double y1 = dot1.GetY();
261 
262  double x2 = dot2.GetX();
263  double y2 = dot2.GetY();
264 
265  double y = (y2 - y1);
266  double x = (x2 - x1);
267 
268  double angleRad = atan2(y, x);
269 
270  return angleRad;
271 }
272 
273 //-----------------------------------------------------------------------------
274 
276 {
277  if (line.GetNumberOfPoints() < 1)
278  {
279  LOG_WARNING("Cannot compute parameters for an empty line");
280  return;
281  }
282 
283  // Compute line intensity
284  std::vector<int> pointNum;
285  double lineIntensity = 0;
286  for (unsigned int i = 0; i < line.GetNumberOfPoints(); i++)
287  {
288  pointNum.push_back(line.GetPoint(i));
289  lineIntensity += m_DotsVector[pointNum[i]].GetDotIntensity();
290  }
291  line.SetIntensity(lineIntensity);
292 
293  //Computing the line start point, end point, and length
294  if (line.GetNumberOfPoints() == 1)
295  {
296  line.SetStartPointIndex(line.GetPoint(0));
297  line.SetEndPointIndex(line.GetPoint(0));
298  line.SetLength(0);
299  }
300  else
301  {
302  line.SetStartPointIndex(line.GetPoint(0));
303  // Choose the start point to be the one with the largest X position (closest to the marked side of the probe)
304  int largestXposition = m_DotsVector[line.GetPoint(0)].GetX();
305  for (unsigned int i = 0; i < line.GetNumberOfPoints(); i++)
306  {
307  double currentXposition = m_DotsVector[line.GetPoint(i)].GetX();
308  if (currentXposition > largestXposition)
309  {
310  largestXposition = currentXposition;
311  line.SetStartPointIndex(line.GetPoint(i));
312  }
313  }
314  // Search for the end of the line (the point that is farthest from the start point
315  int maxDistance = 0;
316  for (unsigned int i = 0; i < line.GetNumberOfPoints(); i++)
317  {
318  double currentDistance = m_DotsVector[line.GetStartPointIndex()].GetDistanceFrom(m_DotsVector[line.GetPoint(i)]);
319  if (currentDistance > maxDistance)
320  {
321  maxDistance = currentDistance;
322  line.SetEndPointIndex(line.GetPoint(i));
323  }
324  }
325  // Make sure that the start point is the farthest point from the endpoint
326  maxDistance = 0;
327  for (unsigned int i = 0; i < line.GetNumberOfPoints(); i++)
328  {
329  double currentDistance = m_DotsVector[line.GetEndPointIndex()].GetDistanceFrom(m_DotsVector[line.GetPoint(i)]);
330  if (currentDistance > maxDistance)
331  {
332  maxDistance = currentDistance;
333  line.SetStartPointIndex(line.GetPoint(i));
334  }
335  }
336  line.SetLength(maxDistance); //This actually computes the length (not return) and set the endpoint as well
337  }
338 
339  // Compute the line normal
340  if (line.GetNumberOfPoints() > 2) //separating cases: 2-points lines and n-points lines, way simpler for 2-points lines
341  {
342  //computing the line equation c + n1*x + n2*y = 0
343  std::vector<double> x;
344  std::vector<double> y;
345 
346  double n1, n2, c;
347 
348  vnl_matrix<double> A(line.GetNumberOfPoints(), 3, 1);
349 
350  for (unsigned int i = 0; i < line.GetNumberOfPoints(); i++)
351  {
352  x.push_back(m_DotsVector[pointNum[i]].GetX());
353  y.push_back(m_DotsVector[pointNum[i]].GetY());
354  A.put(i, 1, x[i]);
355  A.put(i, 2, y[i]);
356  }
357 
358  //using QR matrix decomposition
359  vnl_qr<double> QR(A);
360  vnl_matrix<double> Q = QR.Q();
361  vnl_matrix<double> R = QR.R();
362 
363  //the B matrix is a subset of the R matrix (because we are in 2D)
364  vnl_matrix<double> B(2, 2, 0);
365  B.put(0, 0, R(1, 1));
366  B.put(0, 1, R(1, 2));
367  B.put(1, 1, R(2, 2));
368 
369  //single Value decomposition of B
370  vnl_svd<double> SVD(B);
371  vnl_matrix<double> V = SVD.V();
372 
373  //We get the needed coefficients from V
374  n1 = V(0, 1);
375  n2 = V(1, 1);
376  c = -(n1 * R(0, 1) + n2 * R(0, 2)) / R(0, 0);
377 
378  if (-n2 < 0 && n1 < 0)
379  {
380  line.SetDirectionVector(0, n2); //the vector (n1,n2) is orthogonal to the line, therefore (-n2,n1) is a direction vector
381  line.SetDirectionVector(1, -n1);
382  }
383  else
384  {
385  line.SetDirectionVector(0, -n2); //the vector (n1,n2) is orthogonal to the line, therefore (-n2,n1) is a direction vector
386  line.SetDirectionVector(1, n1);
387  }
388  }
389  else//for 2-points lines, way simpler and faster
390  {
391  double xdif = m_DotsVector[line.GetEndPointIndex()].GetX() - m_DotsVector[line.GetStartPointIndex()].GetX();
392  double ydif = m_DotsVector[line.GetEndPointIndex()].GetY() - m_DotsVector[line.GetStartPointIndex()].GetY();
393 
394  line.SetDirectionVector(0, xdif);
395  line.SetDirectionVector(1, ydif);
396  }
397 }
398 
399 //-----------------------------------------------------------------------------
400 
402 {
403  //LOG_TRACE("FidLineFinder::SegmentLength");
404 
405  double xd = d2.GetX() - d1.GetX();
406  double yd = d2.GetY() - d1.GetY();
407  return sqrtf(xd * xd + yd * yd);
408 }
409 
410 //-----------------------------------------------------------------------------
411 
413 {
414  double x[3], y[3], z[3];
415 
416  x[0] = m_DotsVector[line.GetStartPointIndex()].GetX();
417  x[1] = m_DotsVector[line.GetStartPointIndex()].GetY();
418  x[2] = 0;
419 
420  y[0] = m_DotsVector[line.GetEndPointIndex()].GetX();
421  y[1] = m_DotsVector[line.GetEndPointIndex()].GetY();
422  y[2] = 0;
423 
424  z[0] = dot.GetX();
425  z[1] = dot.GetY();
426  z[2] = 0;
427 
428  return igsioMath::ComputeDistanceLinePoint(x, y, z);
429 }
430 
431 //-----------------------------------------------------------------------------
432 
434 {
435  LOG_TRACE("FidLineFinder::FindLines2Points");
436 
437  if (m_DotsVector.size() < 2)
438  {
439  return;
440  }
441 
442  std::vector<PlusFidLine> twoPointsLinesVector;
443 
444  for (unsigned int i = 0 ; i < m_Patterns.size() ; i++)
445  {
446  //the expected length of the line
447  int lineLenPx = floor(m_Patterns[i]->GetDistanceToOriginMm()[m_Patterns[i]->GetWires().size() - 1] / m_ApproximateSpacingMmPerPixel + 0.5);
448 
449  for (unsigned int dot1Index = 0; dot1Index < m_DotsVector.size() - 1; dot1Index++)
450  {
451  for (unsigned int dot2Index = dot1Index + 1; dot2Index < m_DotsVector.size(); dot2Index++)
452  {
453  double length = SegmentLength(m_DotsVector[dot1Index], m_DotsVector[dot2Index]);
454  bool acceptLength = fabs(length - lineLenPx) < floor(m_Patterns[i]->GetDistanceToOriginToleranceMm()[m_Patterns[i]->GetWires().size() - 1] / m_ApproximateSpacingMmPerPixel + 0.5);
455 
456  if (acceptLength) //to only add valid two point lines
457  {
458  double angleRad = ComputeAngleRad(m_DotsVector[dot1Index], m_DotsVector[dot2Index]);
459  bool acceptAngle = AcceptAngleRad(angleRad);
460 
461  if (acceptAngle)
462  {
463  PlusFidLine twoPointsLine;
464  twoPointsLine.AddPoint(dot1Index);
465  twoPointsLine.AddPoint(dot2Index);
466 
467  bool duplicate = std::binary_search(twoPointsLinesVector.begin(), twoPointsLinesVector.end(), twoPointsLine, PlusFidLine::compareLines);
468 
469  if (!duplicate)
470  {
471  twoPointsLine.SetStartPointIndex(dot1Index);
472  ComputeLine(twoPointsLine);
473 
474  twoPointsLinesVector.push_back(twoPointsLine);
475  std::sort(twoPointsLinesVector.begin(), twoPointsLinesVector.end(), PlusFidLine::compareLines); //the lines need to be sorted that way each time for the binary search to be performed
476  }
477  }
478  }
479  }
480  }
481  }
482  std::sort(twoPointsLinesVector.begin(), twoPointsLinesVector.end(), PlusFidLine::lessThan); //sort the lines by intensity finally
483 
484  m_LinesVector.push_back(twoPointsLinesVector);
485 }
486 
487 //-----------------------------------------------------------------------------
488 
490 {
491  /* For each point, loop over each 2-point line and try to make a 3-point
492  * line. For the third point use the theta of the line and compute a value
493  * for p. Accept the line if the compute p is within some small distance
494  * of the 2-point line. */
495 
496  LOG_TRACE("FidLineFinder::FindLines3Points");
497 
499  unsigned int maxNumberOfPointsPerLine(0);
500 
501  for (unsigned int i = 0 ; i < m_Patterns.size() ; i++)
502  {
503  if (int(m_Patterns[i]->GetWires().size()) > maxNumberOfPointsPerLine)
504  {
505  maxNumberOfPointsPerLine = m_Patterns[i]->GetWires().size();
506  }
507  }
508 
509  for (unsigned int i = 0 ; i < m_Patterns.size() ; i++)
510  {
511  for (unsigned int linesVectorIndex = 3 ; linesVectorIndex <= maxNumberOfPointsPerLine ; linesVectorIndex++)
512  {
513  if (linesVectorIndex > m_LinesVector.size())
514  {
515  continue;
516  }
517 
518  for (unsigned int l = 0; l < m_LinesVector[linesVectorIndex - 1].size(); l++)
519  {
520  PlusFidLine currentShorterPointsLine;
521  currentShorterPointsLine = m_LinesVector[linesVectorIndex - 1][l]; //the current max point line we want to expand
522 
523  for (unsigned int b3 = 0; b3 < m_DotsVector.size(); b3++)
524  {
525  std::vector<int> candidatesIndex;
526  bool checkDuplicateFlag = false;//assume there is no duplicate
527 
528  for (unsigned int previousPoints = 0 ; previousPoints < currentShorterPointsLine.GetNumberOfPoints() ; previousPoints++)
529  {
530  candidatesIndex.push_back(currentShorterPointsLine.GetPoint(previousPoints));
531  if (candidatesIndex[previousPoints] == b3)
532  {
533  checkDuplicateFlag = true;//the point we want to add is already a point of the line
534  }
535  }
536 
537  if (checkDuplicateFlag)
538  {
539  continue;
540  }
541 
542  candidatesIndex.push_back(b3);
543  double pointToLineDistance = ComputeDistancePointLine(m_DotsVector[b3], currentShorterPointsLine);
544 
545  if (pointToLineDistance <= dist)
546  {
548 
549  // To find unique lines, each line must have a unique configuration of points.
550  std::sort(candidatesIndex.begin(), candidatesIndex.end());
551 
552  for (unsigned int f = 0; f < candidatesIndex.size(); f++)
553  {
554  line.AddPoint(candidatesIndex[f]);
555  }
556  line.SetStartPointIndex(currentShorterPointsLine.GetStartPointIndex());
557 
558 
559  double length = SegmentLength(m_DotsVector[currentShorterPointsLine.GetStartPointIndex()], m_DotsVector[b3]); //distance between the origin and the point we try to add
560 
561  int lineLenPx = floor(m_Patterns[i]->GetDistanceToOriginMm()[linesVectorIndex - 2] / m_ApproximateSpacingMmPerPixel + 0.5);
562  bool acceptLength = fabs(length - lineLenPx) < floor(m_Patterns[i]->GetDistanceToOriginToleranceMm()[linesVectorIndex - 2] / m_ApproximateSpacingMmPerPixel + 0.5);
563 
564  if (!acceptLength)
565  {
566  continue;
567  }
568 
569  // Create the vector between the origin point to the end point and between the origin point to the new point to check if the new point is between the origin and the end point
570  double originToEndPointVector[3] = { m_DotsVector[currentShorterPointsLine.GetEndPointIndex()].GetX() - m_DotsVector[currentShorterPointsLine.GetStartPointIndex()].GetX(),
571  m_DotsVector[currentShorterPointsLine.GetEndPointIndex()].GetY() - m_DotsVector[currentShorterPointsLine.GetStartPointIndex()].GetY(),
572  0
573  };
574 
575  double originToNewPointVector[3] = {m_DotsVector[b3].GetX() - m_DotsVector[currentShorterPointsLine.GetStartPointIndex()].GetX(),
576  m_DotsVector[b3].GetY() - m_DotsVector[currentShorterPointsLine.GetStartPointIndex()].GetY(),
577  0
578  };
579 
580  double dot = vtkMath::Dot(originToEndPointVector, originToNewPointVector);
581 
582  // Reject the line if the middle point is outside the original line
583  if (dot < 0)
584  {
585  continue;
586  }
587 
588  if (m_LinesVector.size() <= linesVectorIndex) //in case the maxpoint lines has not found any yet (the binary search works on empty vector, not on NULL one obviously)
589  {
590  std::vector<PlusFidLine> emptyLine;
591  m_LinesVector.push_back(emptyLine);
592  }
593 
594  if (!std::binary_search(m_LinesVector[linesVectorIndex].begin(), m_LinesVector[linesVectorIndex].end(), line, PlusFidLine::compareLines))
595  {
596  ComputeLine(line);
597  if (AcceptLine(line))
598  {
599  m_LinesVector[linesVectorIndex].push_back(line);
600  // sort the lines so that lines that are already in the list can be quickly found by a binary search
601  std::sort(m_LinesVector[linesVectorIndex].begin(), m_LinesVector[linesVectorIndex].end(), PlusFidLine::compareLines);
602  }
603  }
604  }
605  }
606  }
607  }
608  }
609  if (m_LinesVector[m_LinesVector.size() - 1].empty())
610  {
611  m_LinesVector.pop_back();
612  }
613 }
614 
615 //-----------------------------------------------------------------------------
616 
618 {
619  //LOG_TRACE("FidLineFinder::Clear");
620 
621  m_DotsVector.clear();
622  m_LinesVector.clear();
623  m_CandidateFidValues.clear();
624 
625  std::vector<PlusFidLine> emptyLine;
626  m_LinesVector.push_back(emptyLine); //initializing the 0 vector of lines (unused)
627  m_LinesVector.push_back(emptyLine); //initializing the 1 vector of lines (unused)
628 }
629 
630 //-----------------------------------------------------------------------------
631 
633 {
634  //then angle must be in a certain range that is given in half space (in config file) so it needs to check the whole space (if 20 is accepted, so should -160 as it is the same orientation)
635 
636  if (angleRad > m_MinThetaRad && angleRad < m_MaxThetaRad)
637  {
638  return true;
639  }
640  if (m_MaxThetaRad < vtkMath::Pi() / 2 && m_MinThetaRad > -vtkMath::Pi() / 2)
641  {
642  if (angleRad < m_MaxThetaRad - vtkMath::Pi() || angleRad > m_MinThetaRad + vtkMath::Pi())
643  {
644  return true;
645  }
646  }
647  else if (m_MaxThetaRad > vtkMath::Pi() / 2)
648  {
649  if (angleRad < m_MaxThetaRad - vtkMath::Pi() && angleRad > m_MinThetaRad - vtkMath::Pi())
650  {
651  return true;
652  }
653  }
654  else if (m_MinThetaRad < -vtkMath::Pi() / 2)
655  {
656  if (angleRad < m_MaxThetaRad + vtkMath::Pi() && angleRad > m_MinThetaRad + vtkMath::Pi())
657  {
658  return true;
659  }
660  }
661  return false;
662 }
663 
664 //-----------------------------------------------------------------------------
665 
667 {
668  double angleRad = PlusFidLine::ComputeAngleRad(line);
669  bool acceptAngle = AcceptAngleRad(angleRad);
670  return acceptAngle;
671 }
672 
673 //-----------------------------------------------------------------------------
674 
676 {
677  LOG_TRACE("FidLineFinder::FindLines");
678 
679  // Make pairs of dots into 2-point lines.
681 
682  // Make 2-point lines and dots into 3-point lines.
684 
685  // Sort by intensity.
686  std::sort(m_LinesVector[m_LinesVector.size() - 1].begin(), m_LinesVector[m_LinesVector.size() - 1].end(), PlusFidLine::lessThan);
687 }
688 
689 //----------------------------------------------------------------------------
690 std::vector<std::vector<PlusFidLine>>& PlusFidLineFinder::GetLinesVector()
691 {
692  return m_LinesVector;
693 }
694 
695 //-----------------------------------------------------------------------------
697 {
698  m_MinThetaRad = vtkMath::RadiansFromDegrees(angleDeg);
699 }
700 
701 //-----------------------------------------------------------------------------
703 {
704  m_MaxThetaRad = vtkMath::RadiansFromDegrees(angleDeg);
705 }
706 
707 //-----------------------------------------------------------------------------
709 {
710  for (int i = 0; i < 16; i++)
711  {
712  m_ImageToPhantomTransform[i] = matrixElements[i];
713  }
714 }
715 
716 //-----------------------------------------------------------------------------
718 {
719  for (int i = 0; i < 6; i++)
720  {
722  }
723 }
double m_MaxLinePairDistanceErrorPercent
double m_CollinearPointsMaxDistanceFromLineMm
static bool lessThan(const PlusFidLine &line1, const PlusFidLine &line2)
int int int int y2
Definition: phidget22.h:4262
static double SegmentLength(const PlusFidDot &dot1, const PlusFidDot &dot2)
unsigned int GetNumberOfPoints() const
const char int line
Definition: phidget22.h:2458
double m_ApproximateSpacingMmPerPixel
igsioStatus PlusStatus
Definition: PlusCommon.h:40
PlusStatus ReadConfiguration(vtkXMLDataElement *rootConfigElement)
Initial rotation matrix R
Definition: algo3.m:24
void SetImageToPhantomTransform(double *matrixElements)
for i
static double ComputeAngleRad(const PlusFidDot &dot1, const PlusFidDot &dot2)
int x1
Definition: phidget22.h:4262
static bool compareLines(const PlusFidLine &line1, const PlusFidLine &line2)
bool AcceptAngleRad(double angleRad)
std::vector< PlusFidDot > m_CandidateFidValues
std::vector< PlusNWire > GetNWires()
#define PLUS_SUCCESS
Definition: PlusCommon.h:44
bool AcceptLine(PlusFidLine &line)
void SetFrameSize(const FrameSizeType &frameSize)
FrameSizeType m_FrameSize
double m_ImageNormalVectorInPhantomFrameMaximumRotationAngleDeg[6]
int int y1
Definition: phidget22.h:4262
int int int x2
Definition: phidget22.h:4262
void SetStartPointIndex(int index)
int x
Definition: phidget22.h:4265
std::vector< std::vector< PlusFidLine > > & GetLinesVector()
std::vector< PlusFidPattern * > m_Patterns
double ComputeDistancePointLine(const PlusFidDot &dot, const PlusFidLine &line)
static void SetDefaultSegmentationParameters(vtkXMLDataElement *segmentationElement)
void SetMinThetaDegrees(double angleDeg)
int GetPoint(int aIndex) const
static double ComputeAngleRad(const PlusFidLine &line1)
void SetImageNormalVectorInPhantomFrameMaximumRotationAngleDeg(double *anglesDeg)
Direction vectors of rods y
Definition: algo3.m:15
double * GetImageNormalVectorInPhantomFrameMaximumRotationAngleDeg()
double * GetImageToPhantomTransform()
uint32_t * maxDistance
Definition: phidget22.h:4654
std::vector< PlusFidDot > m_DotsVector
std::vector< std::vector< PlusFidLine > > m_LinesVector
void ComputeLine(PlusFidLine &line)
void SetMaxThetaDegrees(double angleDeg)
double m_ImageToPhantomTransform[16]