PlusLib  2.9.0
Software library for tracked ultrasound image acquisition, calibration, and processing.
vtkPlusBrachyStepperPhantomRegistrationAlgo.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 
9 #include "PlusMath.h"
10 #include "igsioTrackedFrame.h"
11 
13 #include "vtkDoubleArray.h"
14 #include "vtkPlusHTMLGenerator.h"
15 #include "vtkMath.h"
16 #include "vtkMatrix4x4.h"
17 #include "vtkObjectFactory.h"
18 #include "vtkPoints.h"
19 #include "vtkIGSIOTrackedFrameList.h"
20 #include "vtkTransform.h"
21 #include "vtkIGSIOTransformRepository.h"
22 #include "vtkVariantArray.h"
23 
24 #include "vtksys/SystemTools.hxx"
25 
26 #include "vnl/vnl_cross.h"
27 
28 //----------------------------------------------------------------------------
29 
31 vtkCxxSetObjectMacro(vtkPlusBrachyStepperPhantomRegistrationAlgo, TransformRepository, vtkIGSIOTransformRepository);
32 
33 //----------------------------------------------------------------------------
34 
35 //----------------------------------------------------------------------------
37 {
38  this->TrackedFrameList = NULL;
40  this->TransformRepository = NULL;
41  this->SetSpacing(0, 0);
42  this->SetCenterOfRotationPx(0, 0);
43 
44  this->PhantomCoordinateFrame = NULL;
45  this->ReferenceCoordinateFrame = NULL;
46 
47  this->PhantomToReferenceTransformMatrix = vtkMatrix4x4::New();
48 }
49 
50 //----------------------------------------------------------------------------
52 {
53  // remove references to objects avoid memory leaks
54  this->SetTrackedFrameList(NULL);
55  this->SetTransformRepository(NULL);
56  // delete member variables
57  if (this->PhantomToReferenceTransformMatrix != NULL)
58  {
59  this->PhantomToReferenceTransformMatrix->Delete();
61  }
62 }
63 
64 //----------------------------------------------------------------------------
65 void vtkPlusBrachyStepperPhantomRegistrationAlgo::PrintSelf(ostream& os, vtkIndent indent)
66 {
67  os << std::endl;
68  this->Superclass::PrintSelf(os, indent);
69  os << indent << "Update time: " << UpdateTime.GetMTime() << std::endl;
70  os << indent << "Spacing: " << this->Spacing[0] << " " << this->Spacing[1] << std::endl;
71  os << indent << "Center of rotation (px): " << this->CenterOfRotationPx[0] << " " << this->CenterOfRotationPx[1] << std::endl;
72 
73  if (this->PhantomToReferenceTransformMatrix != NULL)
74  {
75  os << indent << "Phantom to reference transform: " << std::endl;
76  this->PhantomToReferenceTransformMatrix->PrintSelf(os, indent);
77  }
78 
79  if (this->TrackedFrameList != NULL)
80  {
81  os << indent << "TrackedFrameList:" << std::endl;
82  this->TrackedFrameList->PrintSelf(os, indent);
83  }
84 }
85 
86 
87 //----------------------------------------------------------------------------
88 void vtkPlusBrachyStepperPhantomRegistrationAlgo::SetInputs(vtkIGSIOTrackedFrameList* trackedFrameList, double spacing[2], double centerOfRotationPx[2], vtkIGSIOTransformRepository* transformRepository, const std::vector<PlusNWire>& nWires)
89 {
90  LOG_TRACE("vtkPlusBrachyStepperPhantomRegistrationAlgo::SetInputs");
91  this->SetTrackedFrameList(trackedFrameList);
92  this->SetSpacing(spacing);
93  this->SetCenterOfRotationPx(centerOfRotationPx);
94  this->SetTransformRepository(transformRepository);
95  this->NWires = nWires;
96  this->Modified();
97 }
98 
99 
100 //----------------------------------------------------------------------------
102 {
103  LOG_TRACE("vtkPlusBrachyStepperPhantomRegistrationAlgo::GetRotationEncoderScale");
104  // Update result
105  PlusStatus status = this->Update();
106 
107  if (this->PhantomToReferenceTransformMatrix == NULL)
108  {
109  this->PhantomToReferenceTransformMatrix = vtkMatrix4x4::New();
110  }
111 
112  phantomToReferenceTransformMatrix->DeepCopy(this->PhantomToReferenceTransformMatrix);
113  return status;
114 }
115 
116 //----------------------------------------------------------------------------
118 {
119  LOG_TRACE("vtkPlusBrachyStepperPhantomRegistrationAlgo::Update");
120 
121  if (this->GetMTime() < this->UpdateTime.GetMTime())
122  {
123  LOG_DEBUG("Rotation encoder calibration result is up-to-date!");
124  return PLUS_SUCCESS;
125  }
126 
127  // Check if TrackedFrameList is MF oriented BRIGHTNESS image
128  if (vtkIGSIOTrackedFrameList::VerifyProperties(this->TrackedFrameList, US_IMG_ORIENT_MF, US_IMG_BRIGHTNESS) != PLUS_SUCCESS)
129  {
130  LOG_ERROR("Failed to perform calibration - tracked frame list is invalid");
131  return PLUS_FAIL;
132  }
133 
134  // ==================================================================================
135  // Compute the distance from the probe to phantom
136  // ==================================================================================
137  // 1. This point-to-line distance holds the key to relate the position of the TRUS
138  // rotation center to the precisely designed iCAL phantom geometry in Solid Edge CAD.
139  // 2. Here we employ a straight-forward method based on vector theory as one of the
140  // simplest and most efficient way to compute a point-line distance.
141  // FORMULA: D_O2AB = norm( cross(OA,OB) ) / norm(A-B)
142  // ==================================================================================
143 
144  std::vector<vtkSmartPointer<vtkPoints> > vectorOfWirePoints;
145  for (unsigned int index = 0; index < this->TrackedFrameList->GetNumberOfTrackedFrames(); ++index)
146  {
147  // Get tracked frame from list
148  igsioTrackedFrame* trackedFrame = this->TrackedFrameList->GetTrackedFrame(index);
149 
150  if (trackedFrame->GetFiducialPointsCoordinatePx() == NULL)
151  {
152  LOG_ERROR("Unable to get segmented fiducial points from tracked frame - FiducialPointsCoordinatePx is NULL, frame is not yet segmented (position in the list: " << index << ")!");
153  continue;
154  }
155 
156  if (trackedFrame->GetFiducialPointsCoordinatePx()->GetNumberOfPoints() == 0)
157  {
158  LOG_DEBUG("Unable to get segmented fiducial points from tracked frame - couldn't segment image (position in the list: " << index << ")!");
159  continue;
160  }
161 
162  // Add wire #4 (point A) wire #6 (point B) and wire #3 (point C) pixel coordinates to phantom to probe distance point set
163  vtkSmartPointer<vtkPoints> pointset = vtkSmartPointer<vtkPoints>::New();
164  pointset->SetDataTypeToDouble();
165  pointset->InsertNextPoint(trackedFrame->GetFiducialPointsCoordinatePx()->GetPoint(3));
166  pointset->InsertNextPoint(trackedFrame->GetFiducialPointsCoordinatePx()->GetPoint(5));
167  pointset->InsertNextPoint(trackedFrame->GetFiducialPointsCoordinatePx()->GetPoint(2));
168  vectorOfWirePoints.push_back(pointset);
169  }
170 
171 
172  vnl_vector<double> rotationCenter3x1InMm(3, 0);
173  rotationCenter3x1InMm.put(0, this->CenterOfRotationPx[0] * this->Spacing[0]);
174  rotationCenter3x1InMm.put(1, this->CenterOfRotationPx[1] * this->Spacing[1]);
175  rotationCenter3x1InMm.put(2, 0);
176 
177  // Total number images used for this computation
178  const int totalNumberOfImages2ComputePtLnDist = vectorOfWirePoints.size();
179 
180  if (totalNumberOfImages2ComputePtLnDist == 0)
181  {
182  LOG_ERROR("Failed to register phantom to reference. Probe distance calculation data is empty!");
183  return PLUS_FAIL;
184  }
185 
186  // This will keep a trace on all the calculated distance
187  vnl_vector<double> listOfPhantomToProbeVerticalDistanceInMm(totalNumberOfImages2ComputePtLnDist, 0);
188  vnl_vector<double> listOfPhantomToProbeHorizontalDistanceInMm(totalNumberOfImages2ComputePtLnDist, 0);
189 
190  for (int i = 0; i < totalNumberOfImages2ComputePtLnDist; i++)
191  {
192  // Extract point A
193  vnl_vector<double> pointAInMm(3, 0);
194  pointAInMm.put(0, vectorOfWirePoints[i]->GetPoint(0)[0] * this->Spacing[0]);
195  pointAInMm.put(1, vectorOfWirePoints[i]->GetPoint(0)[1] * this->Spacing[1]);
196  pointAInMm.put(2, 0);
197 
198  // Extract point B
199  vnl_vector<double> pointBInMm(3, 0);
200  pointBInMm.put(0, vectorOfWirePoints[i]->GetPoint(1)[0] * this->Spacing[0]);
201  pointBInMm.put(1, vectorOfWirePoints[i]->GetPoint(1)[1] * this->Spacing[1]);
202  pointBInMm.put(2, 0);
203 
204  // Extract point C
205  vnl_vector<double> pointCInMm(3, 0);
206  pointCInMm.put(0, vectorOfWirePoints[i]->GetPoint(2)[0] * this->Spacing[0]);
207  pointCInMm.put(1, vectorOfWirePoints[i]->GetPoint(2)[1] * this->Spacing[1]);
208  pointCInMm.put(2, 0);
209 
210  // Construct vectors among rotation center, point A, and point B.
211  const vnl_vector<double> vectorRotationCenterToPointAInMm = pointAInMm - rotationCenter3x1InMm;
212  const vnl_vector<double> vectorRotationCenterToPointBInMm = pointBInMm - rotationCenter3x1InMm;
213  const vnl_vector<double> vectorRotationCenterToPointCInMm = pointCInMm - rotationCenter3x1InMm;
214  const vnl_vector<double> vectorPointAToPointBInMm = pointBInMm - pointAInMm;
215  const vnl_vector<double> vectorPointBToPointCInMm = pointCInMm - pointBInMm;
216 
217  // Compute the point-line distance from probe to the line passing through A and B points, based on the
218  // standard vector theory. FORMULA: D_O2AB = norm( cross(OA,OB) ) / norm(A-B)
219  const double thisPhantomToProbeVerticalDistanceInMm = vnl_cross_3d(vectorRotationCenterToPointAInMm, vectorRotationCenterToPointBInMm).magnitude() / vectorPointAToPointBInMm.magnitude();
220 
221  // Compute the point-line distance from probe to the line passing through B and C points, based on the
222  // standard vector theory. FORMULA: D_O2AB = norm( cross(OA,OB) ) / norm(A-B)
223  const double thisPhantomToProbeHorizontalDistanceInMm = vnl_cross_3d(vectorRotationCenterToPointBInMm, vectorRotationCenterToPointCInMm).magnitude() / vectorPointBToPointCInMm.magnitude();
224 
225  // Populate the data container
226  listOfPhantomToProbeVerticalDistanceInMm.put(i, thisPhantomToProbeVerticalDistanceInMm);
227  listOfPhantomToProbeHorizontalDistanceInMm.put(i, thisPhantomToProbeHorizontalDistanceInMm);
228  }
229 
230  double wire6ToProbeDistanceInMm[2] = { listOfPhantomToProbeHorizontalDistanceInMm.mean(), listOfPhantomToProbeVerticalDistanceInMm.mean() };
231  double phantomToReferenceDistanceInMm[3] = { this->NWires[1].GetWires()[2].EndPointFront[0] + wire6ToProbeDistanceInMm[0], this->NWires[1].GetWires()[2].EndPointFront[1] + wire6ToProbeDistanceInMm[1], 0 };
232 
233  LOG_INFO("Phantom to probe distance (mm): " << phantomToReferenceDistanceInMm[0] << " " << phantomToReferenceDistanceInMm[1]);
234 
235  vtkSmartPointer<vtkTransform> phantomToReferenceTransform = vtkSmartPointer<vtkTransform>::New();
236  phantomToReferenceTransform->Identity();
237  phantomToReferenceTransform->Translate(phantomToReferenceDistanceInMm);
238  phantomToReferenceTransform->Inverse();
239 
240  this->PhantomToReferenceTransformMatrix->DeepCopy(phantomToReferenceTransform->GetMatrix());
241 
242  // Save result
243  if (this->TransformRepository)
244  {
245  igsioTransformName phantomToReferenceTransformName(this->PhantomCoordinateFrame, this->ReferenceCoordinateFrame);
246  this->TransformRepository->SetTransform(phantomToReferenceTransformName, this->PhantomToReferenceTransformMatrix);
247  this->TransformRepository->SetTransformPersistent(phantomToReferenceTransformName, true);
248  this->TransformRepository->SetTransformDate(phantomToReferenceTransformName, vtkIGSIOAccurateTimer::GetInstance()->GetDateAndTimeString().c_str());
249  this->TransformRepository->SetTransformError(phantomToReferenceTransformName, -1); //TODO
250  }
251  else
252  {
253  LOG_INFO("Transform repository object is NULL, cannot save results into it");
254  }
255 
256  this->UpdateTime.Modified();
257 
258  return PLUS_SUCCESS;
259 }
260 
261 //-----------------------------------------------------------------------------
263 {
264  LOG_TRACE("vtkPlusBrachyStepperPhantomRegistrationAlgo::ReadConfiguration");
265 
266  XML_FIND_NESTED_ELEMENT_REQUIRED(phantomRegistrationElement, aConfig, "vtkPlusBrachyStepperPhantomRegistrationAlgo");
267 
268  XML_READ_CSTRING_ATTRIBUTE_REQUIRED(PhantomCoordinateFrame, phantomRegistrationElement);
269  XML_READ_CSTRING_ATTRIBUTE_REQUIRED(ReferenceCoordinateFrame, phantomRegistrationElement);
270 
271  return PLUS_SUCCESS;
272 }
igsioStatus PlusStatus
Definition: PlusCommon.h:40
for i
virtual void SetSpacing(double, double)
virtual void SetCenterOfRotationPx(double, double)
#define PLUS_FAIL
Definition: PlusCommon.h:43
void SetTransformRepository(vtkIGSIOTransformRepository *)
virtual void PrintSelf(ostream &os, vtkIndent indent) VTK_OVERRIDE
#define PLUS_SUCCESS
Definition: PlusCommon.h:44
virtual void SetTrackedFrameList(vtkIGSIOTrackedFrameList *)
vtkCxxSetObjectMacro(vtkPlusBrachyStepperPhantomRegistrationAlgo, TransformRepository, vtkIGSIOTransformRepository)
virtual void SetInputs(vtkIGSIOTrackedFrameList *trackedFrameList, double spacing[2], double centerOfRotationPx[2], vtkIGSIOTransformRepository *transformRepository, const std::vector< PlusNWire > &nWires)
vtkStandardNewMacro(vtkPlusBrachyStepperPhantomRegistrationAlgo)
virtual PlusStatus GetPhantomToReferenceTransformMatrix(vtkMatrix4x4 *phantomToReferenceTransform)
Phantom registration algorithm for image to probe calibration with brachy stepper.