PlusLib  2.9.0
Software library for tracked ultrasound image acquisition, calibration, and processing.
TrackingTest.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 
13 // Local includes
14 #include "PlusConfigure.h"
15 #include "vtkPlusDataCollector.h"
16 #include "vtkPlusChannel.h"
17 #include "vtkPlusDataSource.h"
18 #include "vtkPlusDevice.h"
19 #include "igsioMath.h"
20 #ifdef PLUS_RENDERING_ENABLED
21 #include "vtkPlusToolAxesActor.h"
22 #endif
23 
24 // VTK includes
25 #include <vtkCallbackCommand.h>
26 #include <vtkCamera.h>
27 #include <vtkCommand.h>
28 #include <vtkInteractorStyleTrackballCamera.h>
29 #include <vtkMatrix4x4.h>
30 #include <vtkRenderWindow.h>
31 #include <vtkRenderWindowInteractor.h>
32 #include <vtkRenderer.h>
33 #include <vtkSmartPointer.h>
34 #include <vtkTextActor.h>
35 #include <vtkTextProperty.h>
36 #include <vtkTimerLog.h>
37 #include <vtkTransform.h>
38 #include <vtkXMLUtilities.h>
39 #include <vtksys/CommandLineArguments.hxx>
40 #include <vtksys/SystemTools.hxx>
41 
42 class vtkMyCallback : public vtkCommand
43 {
44 public:
45  static vtkMyCallback* New()
46  {
47  return new vtkMyCallback;
48  }
49 
50  vtkMyCallback()
51  {
52  this->TimerId = -1;
53  this->StepperTextActor = vtkTextActor::New();
54  }
55 
56  virtual ~vtkMyCallback()
57  {
58  this->StepperTextActor->Delete();
59  this->StepperTextActor = NULL;
60 #ifdef PLUS_RENDERING_ENABLED
61  for (std::map<std::string, vtkPlusToolAxesActor*>::iterator it = this->ToolActors.begin(); it != this->ToolActors.end(); ++it)
62  {
63  //this->Renderer->RemoveActor(this->ToolActors[i]);
64  it->second->Delete();
65  it->second = NULL;
66  }
67 #endif
68  }
69 
70  void Init()
71  {
72  // Create a text actor for tracking information
73  vtkTextProperty* textprop = this->StepperTextActor->GetTextProperty();
74  textprop->SetColor(1, 0, 0);
75  textprop->SetFontFamilyToArial();
76  textprop->SetFontSize(15);
77  textprop->SetJustificationToLeft();
78  textprop->SetVerticalJustificationToTop();
79  this->StepperTextActor->VisibilityOn();
80  this->StepperTextActor->SetDisplayPosition(20, 65);
81  this->Renderer->AddActor(this->StepperTextActor);
82 
83  vtkPlusDevice* aDevice = NULL;
84  this->DataCollector->GetDevice(aDevice, DeviceId);
85  if (aDevice == NULL)
86  {
87  return;
88  }
89  for (DataSourceContainerConstIterator it = aDevice->GetToolIteratorBegin(); it != aDevice->GetToolIteratorEnd(); ++it)
90  {
91  vtkPlusDataSource* tool = it->second;
92  AddNewToolActor(tool->GetId());
93  SetToolVisible(tool->GetId(), true);
94  }
95 
96  this->RenderWindowInteractor->AddObserver(vtkCommand::TimerEvent, this);
97  // When the interactor stops it stops our timer as well, trigger a restart when the interactor stops
98  this->RenderWindowInteractor->AddObserver(vtkCommand::EndInteractionEvent, this);
99 
100  this->TimerId = this->RenderWindowInteractor->CreateOneShotTimer(100);
101  }
102 
103  void AddNewToolActor(const std::string& aToolId)
104  {
105 #ifdef PLUS_RENDERING_ENABLED
107  this->Renderer->AddActor(actor);
108  actor->SetVisibility(false);
109  this->ToolActors[aToolId] = actor;
110  actor->SetName(aToolId);
111 #else
112  LOG_WARNING("Cannot add actor when VTK_RENDERING_BACKEND is None!");
113 #endif
114  }
115 
116  void SetToolVisible(const std::string& aToolId, bool visible)
117  {
118 #ifdef PLUS_RENDERING_ENABLED
119  this->ToolActors[aToolId]->SetVisibility(visible);
120 #else
121  LOG_WARNING("Cannot change tool visibility when VTK_RENDERING_BACKEND is None!");
122 #endif
123  }
124 
125  void SetToolToTrackerTransform(const std::string& aToolId, vtkMatrix4x4* toolToTrackerTransform)
126  {
127 #ifdef PLUS_RENDERING_ENABLED
128  vtkSmartPointer<vtkTransform> normalizedTransform = vtkSmartPointer<vtkTransform>::New();
129  normalizedTransform->SetMatrix(toolToTrackerTransform);
130  this->ToolActors[aToolId]->SetUserTransform(normalizedTransform);
131 #else
132  LOG_WARNING("Cannot set tool transform when VTK_RENDERING_BACKEND is None!");
133 #endif
134  }
135 
136  virtual void Execute(vtkObject* caller, unsigned long, void*)
137  {
138  std::ostringstream ss;
139  ss.precision(2);
140 
141  igsioTrackedFrame trackedFrame;
142  if (this->BroadcastChannel->GetTrackedFrame(trackedFrame) != PLUS_SUCCESS)
143  {
144  LOG_ERROR("Failed to get tracked frame!");
145  this->TimerId = this->RenderWindowInteractor->CreateOneShotTimer(100);
146  return;
147  }
148 
149  vtkPlusDevice* aDevice = NULL;
150  this->DataCollector->GetDevice(aDevice, DeviceId);
151  if (aDevice == NULL)
152  {
153  return;
154  }
155 
156  std::vector<igsioTransformName> transformNameList;
157  trackedFrame.GetFrameTransformNameList(transformNameList);
158  for (std::vector<igsioTransformName>::iterator it = transformNameList.begin(); it != transformNameList.end(); ++it)
159  {
160  igsioTransformName transformName = *it;
161 
162  vtkPlusDataSource* tool = NULL;
163  if (aDevice->GetTool(transformName.GetTransformName().c_str(), tool) != PLUS_SUCCESS)
164  {
165  LOG_ERROR("Failed to get tool: " << transformName.From());
166  continue;
167  }
168 
169  std::string strTransformName;
170  transformName.GetTransformName(strTransformName);
171  // Transform name
172  ss << strTransformName << ": ";
173 
174  vtkSmartPointer<vtkMatrix4x4> toolToTrackerTransform = vtkSmartPointer<vtkMatrix4x4>::New(); // a new transform matrix has to be provided to each SetToolToTrackerTransform call
175  if (trackedFrame.GetFrameTransform(transformName, toolToTrackerTransform) != PLUS_SUCCESS)
176  {
177  ss << "failed to get transform\n";
178  SetToolVisible(tool->GetId(), false);
179  continue;
180  }
181 
182  ToolStatus status(TOOL_INVALID);
183  trackedFrame.GetFrameTransformStatus(transformName, status);
184 
185  if (status != TOOL_OK)
186  {
187  ss << "missing or out of view\n";
188  SetToolVisible(tool->GetId(), false);
189  continue;
190  }
191 
192  // There is a valid transform
193  SetToolToTrackerTransform(tool->GetId(), toolToTrackerTransform);
194  SetToolVisible(tool->GetId(), true);
195  ss << std::fixed
196  << toolToTrackerTransform->GetElement(0, 0) << " " << toolToTrackerTransform->GetElement(0, 1) << " " << toolToTrackerTransform->GetElement(0, 2) << " " << toolToTrackerTransform->GetElement(0, 3) << " / "
197  << toolToTrackerTransform->GetElement(1, 0) << " " << toolToTrackerTransform->GetElement(1, 1) << " " << toolToTrackerTransform->GetElement(1, 2) << " " << toolToTrackerTransform->GetElement(1, 3) << " / "
198  << toolToTrackerTransform->GetElement(2, 0) << " " << toolToTrackerTransform->GetElement(2, 1) << " " << toolToTrackerTransform->GetElement(2, 2) << " " << toolToTrackerTransform->GetElement(2, 3) << " / "
199  << toolToTrackerTransform->GetElement(3, 0) << " " << toolToTrackerTransform->GetElement(3, 1) << " " << toolToTrackerTransform->GetElement(3, 2) << " " << toolToTrackerTransform->GetElement(3, 3) << "\n";
200  }
201 
202  this->StepperTextActor->SetInput(ss.str().c_str());
203  this->StepperTextActor->Modified();
204 
205  this->Renderer->GetRenderWindow()->Render();
206 
207  static bool firstUpdate = true;
208  if (firstUpdate)
209  {
210  this->Renderer->ResetCamera();
211  firstUpdate = false;
212  }
213 
214  this->TimerId = this->RenderWindowInteractor->CreateOneShotTimer(100);
215  }
216 
217  vtkPlusDataCollector* DataCollector;
218  vtkPlusChannel* BroadcastChannel;
219  std::string DeviceId;
220  vtkRenderer* Renderer;
221  vtkRenderWindowInteractor* RenderWindowInteractor;
222  vtkTextActor* StepperTextActor;
223 #ifdef PLUS_RENDERING_ENABLED
224  std::map<std::string, vtkPlusToolAxesActor*> ToolActors;
225 #endif
226  int TimerId;
227 };
228 
229 int main(int argc, char** argv)
230 {
231  bool printHelp(false);
232  std::string inputConfigFileName;
233  std::string inputToolSourceId;
234  double inputAcqTimeLength(60);
235  std::string outputTrackerBufferSequenceFileName;
236  bool renderingOff(false);
237 
238  int verboseLevel = vtkPlusLogger::LOG_LEVEL_UNDEFINED;
239 
240  vtksys::CommandLineArguments args;
241  args.Initialize(argc, argv);
242 
243  args.AddArgument("--help", vtksys::CommandLineArguments::NO_ARGUMENT, &printHelp, "Print this help.");
244  args.AddArgument("--config-file", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &inputConfigFileName, "Name of the input configuration file.");
245  args.AddArgument("--tool-name", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &inputToolSourceId, "Will print the actual transform of this tool (names were defined in the config file, default is the first active tool)");
246  args.AddArgument("--acq-time-length", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &inputAcqTimeLength, "Length of acquisition time in seconds (Default: 60s)");
247  args.AddArgument("--output-tracker-buffer-seq-file-name", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &outputTrackerBufferSequenceFileName, "Filename of the output tracker bufffer sequence metafile (Default: TrackerBufferMetafile)");
248  args.AddArgument("--rendering-off", vtksys::CommandLineArguments::NO_ARGUMENT, &renderingOff, "Run test without rendering.");
249  args.AddArgument("--verbose", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &verboseLevel, "Verbose level (1=error only, 2=warning, 3=info, 4=debug, 5=trace)");
250 
251  if (!args.Parse())
252  {
253  std::cerr << "Problem parsing arguments" << std::endl;
254  std::cout << "Help: " << args.GetHelp() << std::endl;
255  exit(EXIT_FAILURE);
256  }
257 
258  if (printHelp)
259  {
260  std::cout << args.GetHelp() << std::endl;
261  exit(EXIT_SUCCESS);
262  }
263 
264  vtkPlusLogger::Instance()->SetLogLevel(verboseLevel);
265 
266  if (inputConfigFileName.empty())
267  {
268  std::cerr << "--config-file is required" << std::endl;
269  exit(EXIT_FAILURE);
270  }
271 
273 
274  vtkSmartPointer<vtkXMLDataElement> configRootElement = vtkSmartPointer<vtkXMLDataElement>::New();
275  if (PlusXmlUtils::ReadDeviceSetConfigurationFromFile(configRootElement, inputConfigFileName.c_str()) == PLUS_FAIL)
276  {
277  LOG_ERROR("Unable to read configuration from file " << inputConfigFileName.c_str());
278  return EXIT_FAILURE;
279  }
280 
281  vtkSmartPointer<vtkPlusDataCollector> dataCollector = vtkSmartPointer<vtkPlusDataCollector>::New();
282  if (dataCollector->ReadConfiguration(configRootElement) != PLUS_SUCCESS)
283  {
284  LOG_ERROR("Unable to parse data collection XML tag.");
285  return EXIT_FAILURE;
286  }
287  std::string deviceId;
288  vtkXMLDataElement* dataCollectionElement = configRootElement->FindNestedElementWithName("DataCollection");
289  vtkXMLDataElement* deviceElement = dataCollectionElement->FindNestedElementWithName("Device");
290  if (deviceElement != NULL)
291  {
292  deviceId = std::string(deviceElement->GetAttribute("Id"));
293  }
294 
295  dataCollector->Connect();
296  dataCollector->Start();
297 
298  vtkPlusDevice* aDevice = NULL;
299  dataCollector->GetDevice(aDevice, deviceId);
300  if (aDevice == NULL)
301  {
302  LOG_ERROR("Unable to retrieve device \'" << deviceId << "\'.");
303  return EXIT_FAILURE;
304  }
305  if (aDevice->OutputChannelCount() == 0)
306  {
307  LOG_ERROR("No channels to retrieve data from. Check config file.");
308  return EXIT_FAILURE;
309  }
310  vtkPlusChannel* aChannel = *(aDevice->GetOutputChannelsStart());
311 
312  const double acqStartTime = vtkTimerLog::GetUniversalTime();
313 
314  if (!aChannel->GetTrackingEnabled())
315  {
316  LOG_ERROR("Tracking is not enabled!");
317  return EXIT_FAILURE;
318  }
319 
320  vtkPlusDataSource* tool = NULL;
321  if (!inputToolSourceId.empty())
322  {
323  if (aChannel->GetTool(tool, inputToolSourceId.c_str()) != PLUS_SUCCESS)
324  {
325  LOG_ERROR("Failed to get tool with name: " << inputToolSourceId);
326  return EXIT_FAILURE;
327  }
328  }
329  else
330  {
331  if (aChannel->GetToolsStartIterator() == aChannel->GetToolsEndIterator())
332  {
333  LOG_ERROR("There is no active tool!");
334  return EXIT_FAILURE;
335  }
336 
337  // Use the first active tool
338  tool = aChannel->GetToolsStartIterator()->second;
339  }
340 
341  if (tool == NULL)
342  {
343  LOG_ERROR("Tool does not exist anymore!");
344  return EXIT_FAILURE;
345  }
346 
347  if (renderingOff)
348  {
349  // No rendering, just show the output on the console
350  LOG_DEBUG("Rendering is disabled");
351 
352  StreamBufferItem bufferItem;
353  vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New();
354 
355  while (acqStartTime + inputAcqTimeLength > vtkTimerLog::GetUniversalTime())
356  {
357 
358  tool->GetLatestStreamBufferItem(&bufferItem);
359  if (bufferItem.GetMatrix(matrix) != PLUS_SUCCESS)
360  {
361  LOG_ERROR("Failed to get matrix from buffer item!");
362  continue;
363  }
364 
365  std::string transformParameters = igsioMath::GetTransformParametersString(matrix);
366  std::string status = igsioCommon::ConvertToolStatusToString(bufferItem.GetStatus());
367 
368  std::ostringstream message;
369  message << "Tool name: " << tool->GetId() << "Transform: ";
370  for (int r = 0; r < 4; r++)
371  {
372  for (int c = 0; c < 4; c++)
373  {
374  message << " " << std::fixed << std::setprecision(5) << std::setw(8) << std::setfill(' ') << matrix->GetElement(r, c);
375  }
376  message << " ";
377  }
378  message << " Status: " << status;
379 
380  LOG_INFO(message.str());
381  vtksys::SystemTools::Delay(200);
382  }
383  }
384  else
385  {
386 #ifdef PLUS_RENDERING_ENABLED
387  // Start live rendering
388  vtkSmartPointer<vtkRenderWindow> renWin = vtkSmartPointer<vtkRenderWindow>::New();
389  vtkSmartPointer<vtkRenderer> renderer = vtkSmartPointer<vtkRenderer>::New();
390  renWin->AddRenderer(renderer);
391 
392  //Create the interactor that handles the event loop
393  vtkSmartPointer<vtkRenderWindowInteractor> iren = vtkSmartPointer<vtkRenderWindowInteractor>::New();
394  iren->SetRenderWindow(renWin);
395 
396  // Switch interactor style to trackball
397  vtkSmartPointer<vtkInteractorStyleTrackballCamera> style = vtkSmartPointer<vtkInteractorStyleTrackballCamera>::New();
398  iren->SetInteractorStyle(style);
399 
400  // Must be called after iren and renderer are linked or there will be problems
401  renderer->Render();
402 
403  // iren must be initialized so that it can handle events
404  iren->Initialize();
405 
406  // Set up transform display actors
407  vtkSmartPointer<vtkMyCallback> transformDisplayUpdater = vtkSmartPointer<vtkMyCallback>::New();
408  transformDisplayUpdater->DataCollector = dataCollector;
409  transformDisplayUpdater->BroadcastChannel = aChannel;
410  transformDisplayUpdater->Renderer = renderer;
411  transformDisplayUpdater->DeviceId = deviceId;
412  transformDisplayUpdater->RenderWindowInteractor = iren;
413  transformDisplayUpdater->Init();
414 
415  // Add an origin display actor
416  vtkSmartPointer<vtkPlusToolAxesActor> originActor = vtkSmartPointer<vtkPlusToolAxesActor>::New();
417  originActor->SetName("origin");
418  renderer->AddActor(originActor);
419 
420  // Set projection to parallel to enable estimate distances
421  renderer->GetActiveCamera()->ParallelProjectionOn();
422 
423  iren->Start();
424 #else
425  LOG_WARNING("Cannot render the scene when when VTK_RENDERING_BACKEND is None!");
426 #endif
427  }
428 
429  dataCollector->Disconnect();
430 
431  if (!outputTrackerBufferSequenceFileName.empty())
432  {
433  LOG_INFO("Copy tracker...");
434  vtkSmartPointer<vtkPlusDevice> tracker = vtkSmartPointer<vtkPlusDevice>::New();
435  tracker->DeepCopy(*aDevice);
436  std::string fullPath = vtkPlusConfig::GetInstance()->GetOutputPath(outputTrackerBufferSequenceFileName);
437  LOG_INFO("Write tracker to " << fullPath);
438  tracker->WriteToolsToSequenceFile(fullPath.c_str(), true);
439  }
440 
441  std::cout << "Test completed successfully!" << std::endl;
442  return EXIT_SUCCESS;
443 }
DataSourceContainer::const_iterator DataSourceContainerConstIterator
ToolStatus GetStatus() const
std::string GetOutputPath(const std::string &subPath)
Abstract interface for tracker and video devices.
Definition: vtkPlusDevice.h:60
std::string GetTransformName() const
static vtkPlusToolAxesActor * New()
DataSourceContainerIterator GetToolsStartIterator()
PlusStatus GetTool(vtkPlusDataSource *&aTool, const std::string &toolSourceId)
vtkRenderWindowInteractor * iren
int main(int argc, char **argv)
DataSourceContainerIterator GetToolsEndIterator()
Actor for displaying coordinate system axes.
#define PLUS_FAIL
Definition: PlusCommon.h:43
void SetName(const std::string &name)
static vtkPlusConfig * GetInstance()
Manages devices that record image or positional data.
#define PLUS_SUCCESS
Definition: PlusCommon.h:44
ChannelContainerConstIterator GetOutputChannelsStart() const
PlusStatus GetMatrix(vtkMatrix4x4 *outputMatrix)
bool GetTrackingEnabled() const
virtual ItemStatus GetLatestStreamBufferItem(StreamBufferItem *bufferItem)
static vtkIGSIOLogger * Instance()
DataSourceContainerConstIterator GetToolIteratorBegin() const
Contains an optional timestamped circular buffer containing the video images and a number of timestam...
const char * message
Definition: phidget22.h:2457
virtual int OutputChannelCount() const
PlusStatus GetTool(const char *aToolSourceId, vtkPlusDataSource *&aTool) const
DataSourceContainerConstIterator GetToolIteratorEnd() const
static PlusStatus ReadDeviceSetConfigurationFromFile(vtkXMLDataElement *config, const char *filename)
Definition: PlusXmlUtils.h:23
Interface to a 3D positioning tool, video source, or generalized data stream.