PlusLib  2.9.0
Software library for tracked ultrasound image acquisition, calibration, and processing.
TransformInterpolationTest.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 "PlusMath.h"
9 #include "vtkPlusHTMLGenerator.h"
10 #include "vtkMath.h"
11 #include "vtkMatrix4x4.h"
12 #include "vtkPlusBuffer.h"
13 #include "vtkPlusDevice.h"
14 #include "vtkIGSIOSequenceIO.h"
15 #include "vtkIGSIOTrackedFrameList.h"
16 #include "vtksys/CommandLineArguments.hxx"
17 #include "vtksys/SystemTools.hxx"
18 
19 int main(int argc, char **argv)
20 {
21  int numberOfErrors(0);
22 
23  bool printHelp(false);
24  std::string inputMetafile;
25  std::string inputBaselineReportFilePath("");
26  double inputMaxTranslationDifference(0.5);
27  double inputMaxRotationDifference(1.0);
28  std::string inputTransformName;
29 
30  int verboseLevel = vtkPlusLogger::LOG_LEVEL_UNDEFINED;
31 
32  vtksys::CommandLineArguments args;
33  args.Initialize(argc, argv);
34 
35  args.AddArgument("--transform", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &inputTransformName, "Transform name used for generating transform interpolation");
36  args.AddArgument("--help", vtksys::CommandLineArguments::NO_ARGUMENT, &printHelp, "Print this help.");
37  args.AddArgument("--source-seq-file", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &inputMetafile, "Input sequence metafile.");
38  args.AddArgument("--max-rotation-difference", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &inputMaxRotationDifference, "Maximum rotation difference in degrees (Default: 1 deg).");
39  args.AddArgument("--max-translation-difference", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &inputMaxTranslationDifference, "Maximum translation difference (Default: 0.5 mm).");
40  args.AddArgument("--verbose", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &verboseLevel, "Verbose level (1=error only, 2=warning, 3=info, 4=debug, 5=trace)");
41 
42  if ( !args.Parse() )
43  {
44  std::cerr << "Problem parsing arguments" << std::endl;
45  std::cout << "Help: " << args.GetHelp() << std::endl;
46  exit(EXIT_FAILURE);
47  }
48 
49  if ( printHelp )
50  {
51  std::cout << args.GetHelp() << std::endl;
52  exit(EXIT_SUCCESS);
53  }
54 
55  vtkPlusLogger::Instance()->SetLogLevel(verboseLevel);
56 
57  if ( inputMetafile.empty() )
58  {
59  std::cerr << "input-metafile argument required!" << std::endl;
60  std::cout << "Help: " << args.GetHelp() << std::endl;
61  exit(EXIT_FAILURE);
62  }
63 
64  // Read buffer
65  LOG_INFO("Reading tracker meta file...");
66  vtkSmartPointer<vtkIGSIOTrackedFrameList> trackerFrameList = vtkSmartPointer<vtkIGSIOTrackedFrameList>::New();
67  if( vtkIGSIOSequenceIO::Read(inputMetafile, trackerFrameList) != PLUS_SUCCESS )
68  {
69  LOG_ERROR("Failed to read sequence metafile from file: " << inputMetafile );
70  }
71 
72  igsioTransformName transformName;
73  if ( transformName.SetTransformName(inputTransformName.c_str())!= PLUS_SUCCESS )
74  {
75  LOG_ERROR("Invalid transform name: " << inputTransformName );
76  return EXIT_FAILURE;
77  }
78 
79  LOG_INFO("Copy buffer to tracker buffer...");
80  vtkSmartPointer<vtkPlusBuffer> trackerBuffer = vtkSmartPointer<vtkPlusBuffer>::New();
81  // force recomputation of the filtered timestamps to test timestamp filtering as well
82  if (trackerBuffer->CopyTransformFromTrackedFrameList(trackerFrameList, vtkPlusBuffer::READ_UNFILTERED_COMPUTE_FILTERED_TIMESTAMPS, transformName)!=PLUS_SUCCESS)
83  {
84  LOG_ERROR("CopyDefaultTrackerDataToBuffer failed");
85  numberOfErrors++;
86  }
87 
88  vtkPlusLogger::PrintProgressbar( 100 );
89  std::cout << std::endl;
90 
91  // Check interpolation results
92  //****************************
93 
94  double endTime(0);
95  if ( trackerBuffer->GetLatestTimeStamp(endTime) != ITEM_OK )
96  {
97  LOG_ERROR("Failed to get latest timestamp from tracker buffer!");
98  exit(EXIT_FAILURE);
99  }
100 
101  double startTime(0);
102  if ( trackerBuffer->GetOldestTimeStamp(startTime) != ITEM_OK )
103  {
104  LOG_ERROR("Failed to get oldest timestamp from tracker buffer!");
105  exit(EXIT_FAILURE);
106  }
107 
108  const double frameRate = trackerBuffer->GetFrameRate();
109 
110  vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New();
111  vtkSmartPointer<vtkMatrix4x4> prevmatrix = vtkSmartPointer<vtkMatrix4x4>::New();
112 
113  for ( double newTime = startTime; newTime < endTime; newTime += 1.0 / (frameRate * 5.0) )
114  {
115  StreamBufferItem bufferItem;
116  if ( trackerBuffer->GetStreamBufferItemFromTime(newTime, &bufferItem, vtkPlusBuffer::INTERPOLATED) != ITEM_OK )
117  {
118  LOG_DEBUG("Failed to get tracker buffer item from time: " << std::fixed << newTime );
119  continue;
120  }
121 
122  if ( bufferItem.GetStatus()!=TOOL_OK )
123  {
124  LOG_DEBUG("Tracker item is missing or invalid (index: " << bufferItem.GetIndex() << ")" );
125  continue;
126  }
127 
128  long bufferIndex = bufferItem.GetIndex();
129 
130  if ( bufferItem.GetMatrix(matrix) != PLUS_SUCCESS )
131  {
132  LOG_ERROR("Failed to get matrix from buffer!");
133  numberOfErrors++;
134  continue;
135  }
136 
137  if ( fabs(newTime - startTime) < 0.0001 )
138  {
139  // this is the first matrix, we cannot compare it with the previous one.
140  prevmatrix->DeepCopy(matrix);
141  newTime += 1.0 / (frameRate * 5.0);
142  continue;
143  }
144 
145 
146  double rotDiff = igsioMath::GetOrientationDifference(matrix, prevmatrix);
147  if ( rotDiff > inputMaxRotationDifference )
148  {
149  LOG_ERROR("Rotation difference is larger than the max rotation difference (difference=" << std::fixed << rotDiff << ", threshold=" << inputMaxRotationDifference << ", itemIndex=" << bufferIndex << ", timestamp=" << newTime << ")!");
150  numberOfErrors++;
151  }
152 
153  double transDiff = igsioMath::GetPositionDifference(matrix, prevmatrix);
154  if ( transDiff > inputMaxTranslationDifference)
155  {
156  LOG_ERROR("Translation difference is larger than the max translation difference (difference=" << std::fixed << transDiff << ", threshold=" << inputMaxTranslationDifference << ", itemIndex=" << bufferIndex << ", timestamp=" << newTime << ")!");
157  numberOfErrors++;
158  }
159 
160  LOG_DEBUG("bufferIndex = " << bufferIndex << " Rotation diff = " << rotDiff << ", translation diff = " << transDiff);
161 
162  prevmatrix->DeepCopy(matrix);
163  }
164 
165  if ( numberOfErrors != 0 )
166  {
167  LOG_INFO("Test failed!");
168  return EXIT_FAILURE;
169  }
170 
171  LOG_INFO("Test completed successfully!");
172  return EXIT_SUCCESS;
173  }
174 
ToolStatus GetStatus() const
#define PLUS_SUCCESS
Definition: PlusCommon.h:44
PlusStatus GetMatrix(vtkMatrix4x4 *outputMatrix)
unsigned long GetIndex()
int main(int argc, char **argv)
static vtkIGSIOLogger * Instance()