PlusLib  2.9.0
Software library for tracked ultrasound image acquisition, calibration, and processing.
vtkPlusBuffer.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 // Local includes
8 #include "PlusConfigure.h"
9 #include "igsioMath.h"
10 #include "igsioTrackedFrame.h"
11 #include "vtkPlusBuffer.h"
12 #include "vtkPlusDevice.h"
13 #include "vtkPlusSequenceIO.h"
14 #include "vtkIGSIOTrackedFrameList.h"
15 
16 // VTK includes
17 #include <vtkDoubleArray.h>
18 #include <vtkImageData.h>
19 #include <vtkIntArray.h>
20 #include <vtkMath.h>
21 #include <vtkMatrix4x4.h>
22 #include <vtkObjectFactory.h>
23 #include <vtkUnsignedLongLongArray.h>
24 
25 // vtkAddon includes
26 #include <vtkStreamingVolumeCodec.h>
27 
28 static const double NEGLIGIBLE_TIME_DIFFERENCE = 0.00001; // in seconds, used for comparing between exact timestamps
29 static const double ANGLE_INTERPOLATION_WARNING_THRESHOLD_DEG = 10; // if the interpolated orientation differs from both the interpolated orientation by more than this threshold then display a warning
30 
32 
33 #define LOCAL_LOG_ERROR(msg) \
34 { \
35  std::ostringstream msgStream; \
36  if( this->DescriptiveName == NULL ) \
37  { \
38  msgStream << " " << msg << std::ends; \
39  } \
40  else \
41  { \
42  msgStream << this->DescriptiveName << ": " << msg << std::ends; \
43  } \
44  std::string finalStr(msgStream.str()); \
45  LOG_ERROR(finalStr); \
46 }
47 #define LOCAL_LOG_WARNING(msg) \
48 { \
49  std::ostringstream msgStream; \
50  if( this->DescriptiveName == NULL ) \
51 { \
52  msgStream << " " << msg << std::ends; \
53 } \
54  else \
55 { \
56  msgStream << this->DescriptiveName << ": " << msg << std::ends; \
57 } \
58  std::string finalStr(msgStream.str()); \
59  LOG_WARNING(finalStr); \
60 }
61 #define LOCAL_LOG_DEBUG(msg) \
62 { \
63  std::ostringstream msgStream; \
64  if( this->DescriptiveName == NULL ) \
65 { \
66  msgStream << " " << msg << std::ends; \
67 } \
68  else \
69 { \
70  msgStream << this->DescriptiveName << ": " << msg << std::ends; \
71 } \
72  std::string finalStr(msgStream.str()); \
73  LOG_DEBUG(finalStr); \
74 }
75 
76 //----------------------------------------------------------------------------
77 // vtkPlusBuffer
78 //----------------------------------------------------------------------------
80  : PixelType(VTK_UNSIGNED_CHAR)
81  , NumberOfScalarComponents(1)
82  , ImageType(US_IMG_BRIGHTNESS)
83  , ImageOrientation(US_IMG_ORIENT_MF)
84  , StreamBuffer(vtkPlusTimestampedCircularBuffer::New())
85  , MaxAllowedTimeDifference(0.5)
86  , DescriptiveName(NULL)
87 {
88  this->FrameSize[0] = 0;
89  this->FrameSize[1] = 0;
90  this->FrameSize[2] = 1; // by default we assume we have a single-slice image
91 
92  // 150 is a reasonable default value, it means that we keep the last 5 secods of acquired data @30fps
93  // (and last 2.5 seconds @60fps). It should be enough to have all the needed data available and
94  // it does not consume too much memory, even for images.
95  this->SetBufferSize(150);
96 }
97 
98 //----------------------------------------------------------------------------
100 {
101  if (this->StreamBuffer != NULL)
102  {
103  this->StreamBuffer->Delete();
104  this->StreamBuffer = NULL;
105  }
106 
107  this->SetDescriptiveName(nullptr);
108 }
109 
110 //----------------------------------------------------------------------------
111 void vtkPlusBuffer::PrintSelf(ostream& os, vtkIndent indent)
112 {
113  this->Superclass::PrintSelf(os, indent);
114  os << indent << "Frame size in pixel: " << this->GetFrameSize()[0] << " " << this->GetFrameSize()[1] << " " << this->GetFrameSize()[2] << std::endl;
115  os << indent << "Scalar pixel type: " << vtkImageScalarTypeNameMacro(this->GetPixelType()) << std::endl;
116  os << indent << "Image type: " << igsioCommon::GetStringFromUsImageType(this->GetImageType()) << std::endl;
117  os << indent << "Image orientation: " << igsioCommon::GetStringFromUsImageOrientation(this->GetImageOrientation()) << std::endl;
118 
119  os << indent << "StreamBuffer: " << this->StreamBuffer << "\n";
120  if (this->StreamBuffer)
121  {
122  this->StreamBuffer->PrintSelf(os, indent.GetNextIndent());
123  }
124 }
125 
126 //----------------------------------------------------------------------------
128 {
129  igsioLockGuard<StreamItemCircularBuffer> dataBufferGuardedLock(this->StreamBuffer);
130  PlusStatus result = PLUS_SUCCESS;
131 
132  for (int i = 0; i < this->StreamBuffer->GetBufferSize(); ++i)
133  {
134  if (!this->StreamBuffer->GetBufferItemPointerFromBufferIndex(i)->GetFrame().IsFrameEncoded())
135  {
137  {
138  LOCAL_LOG_ERROR("Failed to allocate memory for frame " << i);
139  result = PLUS_FAIL;
140  }
141  }
142  }
143  return result;
144 }
145 
146 //----------------------------------------------------------------------------
148 {
149  this->StreamBuffer->SetLocalTimeOffsetSec(offsetSec);
150 }
151 
152 //----------------------------------------------------------------------------
154 {
155  return this->StreamBuffer->GetLocalTimeOffsetSec();
156 }
157 
158 //----------------------------------------------------------------------------
160 {
161  return this->StreamBuffer->GetBufferSize();
162 }
163 
164 //----------------------------------------------------------------------------
166 {
167  if (bufsize < 0)
168  {
169  LOCAL_LOG_ERROR("Invalid buffer size requested: " << bufsize);
170  return PLUS_FAIL;
171  }
172  if (this->StreamBuffer->GetBufferSize() == bufsize)
173  {
174  // no change
175  return PLUS_SUCCESS;
176  }
177 
178  PlusStatus result = PLUS_SUCCESS;
179  if (this->StreamBuffer->SetBufferSize(bufsize) != PLUS_SUCCESS)
180  {
181  result = PLUS_FAIL;
182  }
183  if (this->AllocateMemoryForFrames() != PLUS_SUCCESS)
184  {
185  return PLUS_FAIL;
186  }
187 
188  return result;
189 }
190 
191 //----------------------------------------------------------------------------
192 bool vtkPlusBuffer::CheckFrameFormat(const FrameSizeType& frameSizeInPx, igsioCommon::VTKScalarPixelType pixelType, US_IMAGE_TYPE imgType, int numberOfScalarComponents)
193 {
194  // don't add a frame if it doesn't match the buffer frame format
195  if (frameSizeInPx[0] != this->GetFrameSize()[0] ||
196  frameSizeInPx[1] != this->GetFrameSize()[1] ||
197  frameSizeInPx[2] != this->GetFrameSize()[2])
198  {
199  LOCAL_LOG_WARNING("Frame format and buffer frame format does not match (expected frame size: " << this->GetFrameSize()[0]
200  << "x" << this->GetFrameSize()[1] << "x" << this->GetFrameSize()[2] << " received: " << frameSizeInPx[0] << "x" << frameSizeInPx[1] << "x" << frameSizeInPx[2] << ")!");
201  return false;
202  }
203 
204  if (pixelType != this->GetPixelType())
205  {
206  LOCAL_LOG_WARNING("Frame pixel type (" << vtkImageScalarTypeNameMacro(pixelType)
207  << ") and buffer pixel type (" << vtkImageScalarTypeNameMacro(this->GetPixelType()) << ") mismatch");
208  return false;
209  }
210 
211  if (imgType != this->GetImageType())
212  {
213  LOCAL_LOG_WARNING("Frame image type (" << igsioCommon::GetStringFromUsImageType(imgType) << ") and buffer image type (" << igsioCommon::GetStringFromUsImageType(this->GetImageType()) << ") mismatch");
214  return false;
215  }
216 
217  if (numberOfScalarComponents != this->GetNumberOfScalarComponents())
218  {
219  LOCAL_LOG_WARNING("Frame number of scalar components (" << numberOfScalarComponents << ") and buffer number of components (" << this->GetNumberOfScalarComponents() << ") mismatch");
220  return false;
221  }
222 
223  return true;
224 }
225 
226 //----------------------------------------------------------------------------
227 PlusStatus vtkPlusBuffer::AddItem(vtkImageData* frame,
228  US_IMAGE_ORIENTATION usImageOrientation,
229  US_IMAGE_TYPE imageType,
230  long frameNumber,
231  const std::array<int, 3>& clipRectangleOrigin,
232  const std::array<int, 3>& clipRectangleSize,
233  double unfilteredTimestamp/*=UNDEFINED_TIMESTAMP*/,
234  double filteredTimestamp/*=UNDEFINED_TIMESTAMP*/,
235  const igsioFieldMapType* customFields /*=NULL*/)
236 {
237  if (frame == NULL)
238  {
239  LOCAL_LOG_ERROR("vtkPlusBuffer: Unable to add NULL frame to video buffer!");
240  return PLUS_FAIL;
241  }
242 
243  const int* frameExtent = frame->GetExtent();
244  if (frameExtent[1] - frameExtent[0] + 1 < 0 || frameExtent[3] - frameExtent[2] + 1 < 0 || frameExtent[5] - frameExtent[4] + 1 < 0)
245  {
246  LOG_ERROR("Invalid negative values sent to vtkPlusUsDevice::AddItem. Aborting.");
247  return PLUS_FAIL;
248  }
249 
250  FrameSizeType frameSize = {static_cast<unsigned int>(frameExtent[1] - frameExtent[0] + 1), static_cast<unsigned int>(frameExtent[3] - frameExtent[2] + 1), static_cast<unsigned int>(frameExtent[5] - frameExtent[4] + 1)};
251  return this->AddItem(reinterpret_cast<unsigned char*>(frame->GetScalarPointer()),
252  usImageOrientation,
253  frameSize,
254  frame->GetScalarType(),
255  frame->GetNumberOfScalarComponents(),
256  imageType,
257  0,
258  frameNumber,
259  clipRectangleOrigin,
260  clipRectangleSize,
261  unfilteredTimestamp,
262  filteredTimestamp,
263  customFields);
264 }
265 
266 //----------------------------------------------------------------------------
267 PlusStatus vtkPlusBuffer::AddItem(const igsioVideoFrame* frame,
268  long frameNumber,
269  const std::array<int, 3>& clipRectangleOrigin,
270  const std::array<int, 3>& clipRectangleSize,
271  double unfilteredTimestamp/*=UNDEFINED_TIMESTAMP*/,
272  double filteredTimestamp/*=UNDEFINED_TIMESTAMP*/,
273  const igsioFieldMapType* customFields /*=NULL*/)
274 {
275  if (frame == NULL)
276  {
277  LOCAL_LOG_ERROR("vtkPlusBuffer: Unable to add NULL frame to video buffer!");
278  return PLUS_FAIL;
279  }
280 
281  if (frame->IsFrameEncoded())
282  {
283  unsigned int numberOfComponents = 0;
284  frame->GetNumberOfScalarComponents(numberOfComponents);
285  int dimensions[3] = { 0, 0, 0 };
286  frame->GetEncodedFrame()->GetDimensions(dimensions);
287  FrameSizeType frameSize = { static_cast<unsigned int>(dimensions[0]),
288  static_cast<unsigned int>(dimensions[1]),
289  static_cast<unsigned int>(dimensions[2])
290  };
291  return this->AddItem(NULL,
292  frame->GetImageOrientation(),
293  frameSize,
294  this->PixelType,
295  numberOfComponents,
296  frame->GetImageType(),
297  0,
298  frameNumber,
299  clipRectangleOrigin,
300  clipRectangleSize,
301  unfilteredTimestamp,
302  filteredTimestamp,
303  customFields,
304  frame->GetEncodedFrame());
305  }
306  else
307  {
308  return this->AddItem(frame->GetImage(),
309  frame->GetImageOrientation(),
310  frame->GetImageType(),
311  frameNumber,
312  clipRectangleOrigin,
313  clipRectangleSize,
314  unfilteredTimestamp,
315  filteredTimestamp,
316  customFields);
317  }
318 }
319 
320 //----------------------------------------------------------------------------
321 PlusStatus vtkPlusBuffer::AddItem(const igsioFieldMapType& fields,
322  long frameNumber,
323  double unfilteredTimestamp/*=UNDEFINED_TIMESTAMP*/,
324  double filteredTimestamp/*=UNDEFINED_TIMESTAMP*/)
325 {
326  if (fields.empty())
327  {
328  return PLUS_SUCCESS;
329  }
330 
331  if (unfilteredTimestamp == UNDEFINED_TIMESTAMP)
332  {
333  unfilteredTimestamp = vtkIGSIOAccurateTimer::GetSystemTime();
334  }
335  if (filteredTimestamp == UNDEFINED_TIMESTAMP)
336  {
337  bool filteredTimestampProbablyValid = true;
338  if (this->StreamBuffer->CreateFilteredTimeStampForItem(frameNumber, unfilteredTimestamp, filteredTimestamp, filteredTimestampProbablyValid) != PLUS_SUCCESS)
339  {
340  LOCAL_LOG_DEBUG("Failed to create filtered timestamp for tracker buffer item with item index: " << frameNumber);
341  return PLUS_FAIL;
342  }
343  if (!filteredTimestampProbablyValid)
344  {
345  LOG_INFO("Filtered timestamp is probably invalid for tracker buffer item with item index=" << frameNumber << ", time=" << unfilteredTimestamp << ". The item may have been tagged with an inaccurate timestamp, therefore it will not be recorded.");
346  return PLUS_SUCCESS;
347  }
348  }
349  else
350  {
351  this->StreamBuffer->AddToTimeStampReport(frameNumber, unfilteredTimestamp, filteredTimestamp);
352  }
353 
354  int bufferIndex(0);
355  BufferItemUidType itemUid;
356 
357  igsioLockGuard<StreamItemCircularBuffer> dataBufferGuardedLock(this->StreamBuffer);
358  if (this->StreamBuffer->PrepareForNewItem(filteredTimestamp, itemUid, bufferIndex) != PLUS_SUCCESS)
359  {
360  // Just a debug message, because we want to avoid unnecessary warning messages if the timestamp is the same as last one
361  LOCAL_LOG_DEBUG("vtkPlusBuffer: Failed to prepare for adding new frame to tracker buffer!");
362  return PLUS_FAIL;
363  }
364 
365  // get the pointer to the correct location in the tracker buffer, where this data needs to be copied
366  StreamBufferItem* newObjectInBuffer = this->StreamBuffer->GetBufferItemPointerFromBufferIndex(bufferIndex);
367  if (newObjectInBuffer == NULL)
368  {
369  LOCAL_LOG_ERROR("vtkPlusBuffer: Failed to get pointer to data buffer object from the tracker buffer for the new frame!");
370  return PLUS_FAIL;
371  }
372 
373  newObjectInBuffer->SetFilteredTimestamp(filteredTimestamp);
374  newObjectInBuffer->SetUnfilteredTimestamp(unfilteredTimestamp);
375  newObjectInBuffer->SetIndex(frameNumber);
376  newObjectInBuffer->SetUid(itemUid);
377 
378  // Add custom fields
379  for (igsioFieldMapType::const_iterator it = fields.begin(); it != fields.end(); ++it)
380  {
381  newObjectInBuffer->SetFrameField(it->first, it->second.second, it->second.first);
382  std::string name(it->first);
383  }
384 
385  return PLUS_SUCCESS;
386 }
387 
388 //----------------------------------------------------------------------------
390  US_IMAGE_ORIENTATION usImageOrientation,
391  const FrameSizeType& inputFrameSizeInPx,
393  unsigned int numberOfScalarComponents,
394  US_IMAGE_TYPE imageType,
395  int numberOfBytesToSkip,
396  long frameNumber,
397  const std::array<int, 3>& clipRectangleOrigin,
398  const std::array<int, 3>& clipRectangleSize,
399  double unfilteredTimestamp /*= UNDEFINED_TIMESTAMP*/,
400  double filteredTimestamp /*= UNDEFINED_TIMESTAMP*/,
401  const igsioFieldMapType* customFields /*= NULL */,
402  vtkStreamingVolumeFrame* encodedFrame /*=NULL*/)
403 {
404  if (unfilteredTimestamp == UNDEFINED_TIMESTAMP)
405  {
406  unfilteredTimestamp = vtkIGSIOAccurateTimer::GetSystemTime();
407  }
408 
409  if (filteredTimestamp == UNDEFINED_TIMESTAMP)
410  {
411  bool filteredTimestampProbablyValid = true;
412  if (this->StreamBuffer->CreateFilteredTimeStampForItem(frameNumber, unfilteredTimestamp, filteredTimestamp, filteredTimestampProbablyValid) != PLUS_SUCCESS)
413  {
414  LOCAL_LOG_WARNING("Failed to create filtered timestamp for video buffer item with item index: " << frameNumber);
415  return PLUS_FAIL;
416  }
417  if (!filteredTimestampProbablyValid)
418  {
419  LOG_INFO("Filtered timestamp is probably invalid for video buffer item with item index=" << frameNumber << ", time=" <<
420  unfilteredTimestamp << ". The item may have been tagged with an inaccurate timestamp, therefore it will not be recorded.");
421  return PLUS_SUCCESS;
422  }
423  }
424  else
425  {
426  this->StreamBuffer->AddToTimeStampReport(frameNumber, unfilteredTimestamp, filteredTimestamp);
427  }
428 
429  if (imageDataPtr == NULL && encodedFrame == NULL)
430  {
431  LOG_ERROR("vtkPlusBuffer: Unable to add NULL frame to video buffer!");
432  return PLUS_FAIL;
433  }
434 
435  igsioVideoFrame::FlipInfoType flipInfo;
436  if (igsioVideoFrame::GetFlipAxes(usImageOrientation, imageType, this->ImageOrientation, flipInfo) != PLUS_SUCCESS)
437  {
438  LOG_ERROR("Failed to convert image data to the requested orientation, from " << igsioCommon::GetStringFromUsImageOrientation(usImageOrientation) <<
439  " to " << igsioCommon::GetStringFromUsImageOrientation(this->ImageOrientation));
440  return PLUS_FAIL;
441  }
442 
443  // Calculate the output frame size to validate that buffer is correctly setup
444  FrameSizeType outputFrameSizeInPx = { inputFrameSizeInPx[0], inputFrameSizeInPx[1], inputFrameSizeInPx[2] };
445  if (igsioCommon::IsClippingRequested(clipRectangleOrigin, clipRectangleSize))
446  {
447  outputFrameSizeInPx[0] = clipRectangleSize[0];
448  outputFrameSizeInPx[1] = clipRectangleSize[1];
449  outputFrameSizeInPx[2] = clipRectangleSize[2];
450  }
451 
452  if (flipInfo.tranpose == igsioVideoFrame::TRANSPOSE_IJKtoKIJ)
453  {
454  unsigned int temp = outputFrameSizeInPx[0];
455  outputFrameSizeInPx[0] = outputFrameSizeInPx[2];
456  outputFrameSizeInPx[2] = outputFrameSizeInPx[1];
457  outputFrameSizeInPx[1] = temp;
458  }
459 
460  if (!this->CheckFrameFormat(outputFrameSizeInPx, pixelType, imageType, numberOfScalarComponents))
461  {
462  LOG_ERROR("vtkPlusBuffer: Unable to add frame to video buffer - frame format doesn't match!");
463  return PLUS_FAIL;
464  }
465 
466  int bufferIndex(0);
467  BufferItemUidType itemUid;
468  igsioLockGuard<StreamItemCircularBuffer> dataBufferGuardedLock(this->StreamBuffer);
469  if (this->StreamBuffer->PrepareForNewItem(filteredTimestamp, itemUid, bufferIndex) != PLUS_SUCCESS)
470  {
471  // Just a debug message, because we want to avoid unnecessary warning messages if the timestamp is the same as last one
472  LOCAL_LOG_DEBUG("vtkPlusBuffer: Failed to prepare for adding new frame to video buffer!");
473  return PLUS_FAIL;
474  }
475 
476  // get the pointer to the correct location in the frame buffer, where this data needs to be copied
477  StreamBufferItem* newObjectInBuffer = this->StreamBuffer->GetBufferItemPointerFromBufferIndex(bufferIndex);
478  if (newObjectInBuffer == NULL)
479  {
480  LOCAL_LOG_ERROR("vtkPlusBuffer: Failed to get pointer to video buffer object from the video buffer for the new frame!");
481  return PLUS_FAIL;
482  }
483 
484  FrameSizeType receivedFrameSize = { 0, 0, 0 };
485  newObjectInBuffer->GetFrame().GetFrameSize(receivedFrameSize);
486 
487  if (imageDataPtr && !encodedFrame &&
488  (outputFrameSizeInPx[0] != receivedFrameSize[0]
489  || outputFrameSizeInPx[1] != receivedFrameSize[1]
490  || outputFrameSizeInPx[2] != receivedFrameSize[2]))
491  {
492  LOCAL_LOG_ERROR("Input frame size is different from buffer frame size (input: " <<
493  outputFrameSizeInPx[0] << "x" << outputFrameSizeInPx[1] << "x" << outputFrameSizeInPx[2] <<
494  ", buffer: " <<
495  receivedFrameSize[0] << "x" << receivedFrameSize[1] << "x" << receivedFrameSize[2] << ")!");
496  return PLUS_FAIL;
497  }
498 
499  // Skip the numberOfBytesToSkip bytes, e.g. header size
500  if (imageDataPtr != NULL)
501  {
502  unsigned char* byteImageDataPtr = reinterpret_cast<unsigned char*>(imageDataPtr);
503  byteImageDataPtr += numberOfBytesToSkip;
504 
505  if (igsioVideoFrame::GetOrientedClippedImage(byteImageDataPtr, flipInfo, imageType, pixelType, numberOfScalarComponents, inputFrameSizeInPx, newObjectInBuffer->GetFrame(), clipRectangleOrigin, clipRectangleSize) != PLUS_SUCCESS)
506  {
507  LOCAL_LOG_ERROR("Failed to convert input US image to the requested orientation!");
508  return PLUS_FAIL;
509  }
510  }
511  else if (encodedFrame != NULL)
512  {
513  newObjectInBuffer->GetFrame().SetEncodedFrame(encodedFrame);
514  }
515 
516  newObjectInBuffer->SetFilteredTimestamp(filteredTimestamp);
517  newObjectInBuffer->SetUnfilteredTimestamp(unfilteredTimestamp);
518  newObjectInBuffer->SetIndex(frameNumber);
519  newObjectInBuffer->SetUid(itemUid);
520  newObjectInBuffer->GetFrame().SetImageType(imageType);
521 
522  // Add custom fields
523  if (customFields != NULL)
524  {
525  for (igsioFieldMapType::const_iterator it = customFields->begin(); it != customFields->end(); ++it)
526  {
527  newObjectInBuffer->SetFrameField(it->first, it->second.second, it->second.first);
528  std::string name(it->first);
529  if (name.find("Transform") != std::string::npos)
530  {
531  newObjectInBuffer->SetValidTransformData(true);
532  }
533  }
534  }
535 
536  return PLUS_SUCCESS;
537 }
538 
539 //----------------------------------------------------------------------------
540 PlusStatus vtkPlusBuffer::AddItem(void* imageDataPtr, const FrameSizeType& frameSize, unsigned int inputFrameSizeInBytes, US_IMAGE_TYPE imageType, long frameNumber, double unfilteredTimestamp /*= UNDEFINED_TIMESTAMP*/, double filteredTimestamp /*= UNDEFINED_TIMESTAMP*/, const igsioFieldMapType* customFields /*= NULL*/)
541 {
542  if (unfilteredTimestamp == UNDEFINED_TIMESTAMP)
543  {
544  unfilteredTimestamp = vtkIGSIOAccurateTimer::GetSystemTime();
545  }
546 
547  if (filteredTimestamp == UNDEFINED_TIMESTAMP)
548  {
549  bool filteredTimestampProbablyValid = true;
550  if (this->StreamBuffer->CreateFilteredTimeStampForItem(frameNumber, unfilteredTimestamp, filteredTimestamp, filteredTimestampProbablyValid) != PLUS_SUCCESS)
551  {
552  LOCAL_LOG_WARNING("Failed to create filtered timestamp for video buffer item with item index: " << frameNumber);
553  return PLUS_FAIL;
554  }
555  if (!filteredTimestampProbablyValid)
556  {
557  LOG_INFO("Filtered timestamp is probably invalid for video buffer item with item index=" << frameNumber << ", time=" <<
558  unfilteredTimestamp << ". The item may have been tagged with an inaccurate timestamp, therefore it will not be recorded.");
559  return PLUS_SUCCESS;
560  }
561  }
562  else
563  {
564  this->StreamBuffer->AddToTimeStampReport(frameNumber, unfilteredTimestamp, filteredTimestamp);
565  }
566 
567  if (imageDataPtr == NULL)
568  {
569  LOG_ERROR("vtkPlusBuffer: Unable to add NULL frame to video buffer!");
570  return PLUS_FAIL;
571  }
572 
573  if (frameSize[0] != this->GetFrameSize()[0] || frameSize[1] != this->GetFrameSize()[1] || frameSize[2] != this->GetFrameSize()[2])
574  {
575  LOG_ERROR("vtkPlusBuffer: Unable to add frame to video buffer - frame format doesn't match!");
576  return PLUS_FAIL;
577  }
578 
579  int bufferIndex(0);
580  BufferItemUidType itemUid;
581  igsioLockGuard<StreamItemCircularBuffer> dataBufferGuardedLock(this->StreamBuffer);
582  if (this->StreamBuffer->PrepareForNewItem(filteredTimestamp, itemUid, bufferIndex) != PLUS_SUCCESS)
583  {
584  // Just a debug message, because we want to avoid unnecessary warning messages if the timestamp is the same as last one
585  LOCAL_LOG_DEBUG("vtkPlusBuffer: Failed to prepare for adding new frame to video buffer!");
586  return PLUS_FAIL;
587  }
588 
589  // get the pointer to the correct location in the frame buffer, where this data needs to be copied
590  StreamBufferItem* newObjectInBuffer = this->StreamBuffer->GetBufferItemPointerFromBufferIndex(bufferIndex);
591  if (newObjectInBuffer == NULL)
592  {
593  LOCAL_LOG_ERROR("vtkPlusBuffer: Failed to get pointer to video buffer object from the video buffer for the new frame!");
594  return PLUS_FAIL;
595  }
596 
597  unsigned int bufferFrameSizeBytes = newObjectInBuffer->GetFrame().GetFrameSizeInBytes();
598  if (bufferFrameSizeBytes < inputFrameSizeInBytes)
599  {
600  LOCAL_LOG_ERROR("Input frame size is larger than buffer frame size (input: " << inputFrameSizeInBytes << ", buffer: " << bufferFrameSizeBytes << ")!");
601  return PLUS_FAIL;
602  }
603 
604  newObjectInBuffer->SetFilteredTimestamp(filteredTimestamp);
605  newObjectInBuffer->SetUnfilteredTimestamp(unfilteredTimestamp);
606  newObjectInBuffer->SetIndex(frameNumber);
607  newObjectInBuffer->SetUid(itemUid);
608  newObjectInBuffer->GetFrame().SetImageType(imageType);
609  memcpy(newObjectInBuffer->GetFrame().GetImage()->GetScalarPointer(), imageDataPtr, inputFrameSizeInBytes);
610 
611  // Add custom fields
612  if (customFields != NULL)
613  {
614  for (igsioFieldMapType::const_iterator it = customFields->begin(); it != customFields->end(); ++it)
615  {
616  newObjectInBuffer->SetFrameField(it->first, it->second.second, it->second.first);
617  std::string name(it->first);
618  if (name.find("Transform") != std::string::npos)
619  {
620  newObjectInBuffer->SetValidTransformData(true);
621  }
622  }
623  }
624 
625  newObjectInBuffer->SetFrameField("FrameSizeInBytes", igsioCommon::ToString<unsigned int>(inputFrameSizeInBytes));
626 
627  return PLUS_SUCCESS;
628 }
629 
630 //----------------------------------------------------------------------------
631 PlusStatus vtkPlusBuffer::AddTimeStampedItem(vtkMatrix4x4* matrix, ToolStatus status, unsigned long frameNumber, double unfilteredTimestamp, double filteredTimestamp/*=UNDEFINED_TIMESTAMP*/, const igsioFieldMapType* customFields /*= NULL*/)
632 {
633  if (matrix == NULL)
634  {
635  LOCAL_LOG_ERROR("vtkPlusBuffer: Unable to add NULL matrix to tracker buffer!");
636  return PLUS_FAIL;
637  }
638  if (unfilteredTimestamp == UNDEFINED_TIMESTAMP)
639  {
640  unfilteredTimestamp = vtkIGSIOAccurateTimer::GetSystemTime();
641  }
642  if (filteredTimestamp == UNDEFINED_TIMESTAMP)
643  {
644  bool filteredTimestampProbablyValid = true;
645  if (this->StreamBuffer->CreateFilteredTimeStampForItem(frameNumber, unfilteredTimestamp, filteredTimestamp, filteredTimestampProbablyValid) != PLUS_SUCCESS)
646  {
647  LOCAL_LOG_DEBUG("Failed to create filtered timestamp for tracker buffer item with item index: " << frameNumber);
648  return PLUS_FAIL;
649  }
650  if (!filteredTimestampProbablyValid)
651  {
652  LOG_INFO("Filtered timestamp is probably invalid for tracker buffer item with item index=" << frameNumber << ", time=" << unfilteredTimestamp << ". The item may have been tagged with an inaccurate timestamp, therefore it will not be recorded.");
653  return PLUS_SUCCESS;
654  }
655  }
656  else
657  {
658  this->StreamBuffer->AddToTimeStampReport(frameNumber, unfilteredTimestamp, filteredTimestamp);
659  }
660 
661  int bufferIndex(0);
662  BufferItemUidType itemUid;
663 
664  igsioLockGuard<StreamItemCircularBuffer> dataBufferGuardedLock(this->StreamBuffer);
665  if (this->StreamBuffer->PrepareForNewItem(filteredTimestamp, itemUid, bufferIndex) != PLUS_SUCCESS)
666  {
667  // Just a debug message, because we want to avoid unnecessary warning messages if the timestamp is the same as last one
668  LOCAL_LOG_DEBUG("vtkPlusBuffer: Failed to prepare for adding new frame to tracker buffer!");
669  return PLUS_FAIL;
670  }
671 
672  // get the pointer to the correct location in the tracker buffer, where this data needs to be copied
673  StreamBufferItem* newObjectInBuffer = this->StreamBuffer->GetBufferItemPointerFromBufferIndex(bufferIndex);
674  if (newObjectInBuffer == NULL)
675  {
676  LOCAL_LOG_ERROR("vtkPlusBuffer: Failed to get pointer to data buffer object from the tracker buffer for the new frame!");
677  return PLUS_FAIL;
678  }
679 
680  PlusStatus itemStatus = newObjectInBuffer->SetMatrix(matrix);
681  newObjectInBuffer->SetStatus(status);
682  newObjectInBuffer->SetFilteredTimestamp(filteredTimestamp);
683  newObjectInBuffer->SetUnfilteredTimestamp(unfilteredTimestamp);
684  newObjectInBuffer->SetIndex(frameNumber);
685  newObjectInBuffer->SetUid(itemUid);
686 
687  // Add custom fields
688  if (customFields != NULL)
689  {
690  for (igsioFieldMapType::const_iterator it = customFields->begin(); it != customFields->end(); ++it)
691  {
692  newObjectInBuffer->SetFrameField(it->first, it->second.second, it->second.first);
693  std::string name(it->first);
694  if (name.find("Transform") != std::string::npos)
695  {
696  newObjectInBuffer->SetValidTransformData(true);
697  }
698  }
699  }
700 
701  return itemStatus;
702 }
703 
704 //----------------------------------------------------------------------------
706 {
707  return this->StreamBuffer->GetLatestTimeStamp(latestTimestamp);
708 }
709 
710 //----------------------------------------------------------------------------
712 {
713  return this->StreamBuffer->GetOldestTimeStamp(oldestTimestamp);
714 }
715 
716 //----------------------------------------------------------------------------
718 {
719  return this->StreamBuffer->GetTimeStamp(uid, timestamp);
720 }
721 
722 //----------------------------------------------------------------------------
724 {
725  return this->StreamBuffer->GetIndex(uid, index);
726 }
727 
728 
729 //----------------------------------------------------------------------------
731 {
732  return this->StreamBuffer->GetBufferIndexFromTime(time, bufferIndex);
733 }
734 
735 //----------------------------------------------------------------------------
736 void vtkPlusBuffer::SetAveragedItemsForFiltering(int averagedItemsForFiltering)
737 {
738  this->StreamBuffer->SetAveragedItemsForFiltering(averagedItemsForFiltering);
739 }
740 
741 //----------------------------------------------------------------------------
743 {
745 }
746 
747 //----------------------------------------------------------------------------
748 void vtkPlusBuffer::SetStartTime(double startTime)
749 {
750  this->StreamBuffer->SetStartTime(startTime);
751 }
752 
753 //----------------------------------------------------------------------------
755 {
756  return this->StreamBuffer->GetStartTime();
757 }
758 
759 //----------------------------------------------------------------------------
760 PlusStatus vtkPlusBuffer::GetTimeStampReportTable(vtkTable* timeStampReportTable)
761 {
762  return this->StreamBuffer->GetTimeStampReportTable(timeStampReportTable);
763 }
764 
765 //----------------------------------------------------------------------------
767 {
768  if (bufferItem == NULL)
769  {
770  LOCAL_LOG_ERROR("Unable to copy data buffer item into a NULL data buffer item!");
771  return ITEM_UNKNOWN_ERROR;
772  }
773 
774  igsioLockGuard<StreamItemCircularBuffer> dataBufferGuardedLock(this->StreamBuffer);
775 
776  StreamBufferItem* dataItem = NULL;
777  ItemStatus itemStatus = this->StreamBuffer->GetBufferItemPointerFromUid(uid, dataItem);
778  if (itemStatus != ITEM_OK)
779  {
780  LOCAL_LOG_WARNING("Failed to retrieve data item");
781  return itemStatus;
782  }
783 
784  if (bufferItem->DeepCopy(dataItem) != PLUS_SUCCESS)
785  {
786  LOCAL_LOG_WARNING("Failed to copy data item");
787  return ITEM_UNKNOWN_ERROR;
788  }
789 
790  return ITEM_OK;
791 }
792 
793 //----------------------------------------------------------------------------
795 {
796  LOG_TRACE("vtkPlusBuffer::DeepCopy");
797 
798  this->StreamBuffer->DeepCopy(buffer->StreamBuffer);
799  if (buffer->GetFrameSize()[0] != -1 && buffer->GetFrameSize()[1] != -1 && buffer->GetFrameSize()[2] != -1)
800  {
801  this->SetFrameSize(buffer->GetFrameSize());
802  }
803  this->SetPixelType(buffer->GetPixelType());
804  this->SetImageType(buffer->GetImageType());
806  this->SetImageOrientation(buffer->GetImageOrientation());
807  this->SetBufferSize(buffer->GetBufferSize());
808 }
809 
810 //----------------------------------------------------------------------------
812 {
813  this->StreamBuffer->Clear();
814 }
815 
816 //----------------------------------------------------------------------------
817 PlusStatus vtkPlusBuffer::SetFrameSize(unsigned int x, unsigned int y, unsigned int z, bool allocateFrames/*=true*/)
818 {
819  if (x != 0 && y != 0 && z == 0)
820  {
821  LOCAL_LOG_WARNING("Single slice images should have a dimension of z=1");
822  z = 1;
823  }
824  if (this->FrameSize[0] == x && this->FrameSize[1] == y && this->FrameSize[2] == z)
825  {
826  // no change
827  return PLUS_SUCCESS;
828  }
829  this->FrameSize[0] = x;
830  this->FrameSize[1] = y;
831  this->FrameSize[2] = z;
832  if (allocateFrames)
833  {
834  return AllocateMemoryForFrames();
835  }
836  return PLUS_SUCCESS;
837 }
838 
839 //----------------------------------------------------------------------------
840 PlusStatus vtkPlusBuffer::SetFrameSize(const FrameSizeType& frameSize, bool allocateFrames/*=true*/)
841 {
842  return SetFrameSize(frameSize[0], frameSize[1], frameSize[2], allocateFrames);
843 }
844 
845 //----------------------------------------------------------------------------
847 {
848  if (pixelType == this->PixelType)
849  {
850  // no change
851  return PLUS_SUCCESS;
852  }
853  this->PixelType = pixelType;
854  return AllocateMemoryForFrames();
855 }
856 
857 //----------------------------------------------------------------------------
858 PlusStatus vtkPlusBuffer::SetNumberOfScalarComponents(unsigned int numberOfScalarComponents)
859 {
860  if (numberOfScalarComponents == this->NumberOfScalarComponents)
861  {
862  // no change
863  return PLUS_SUCCESS;
864  }
865  this->NumberOfScalarComponents = numberOfScalarComponents;
866  return AllocateMemoryForFrames();
867 }
868 
869 //----------------------------------------------------------------------------
871 {
872  if (imgType < US_IMG_TYPE_XX || imgType >= US_IMG_TYPE_LAST)
873  {
874  LOCAL_LOG_ERROR("Invalid image type attempted to set in the video buffer: " << imgType);
875  return PLUS_FAIL;
876  }
877  this->ImageType = imgType;
878  return PLUS_SUCCESS;
879 }
880 
881 //----------------------------------------------------------------------------
882 PlusStatus vtkPlusBuffer::SetImageOrientation(US_IMAGE_ORIENTATION imgOrientation)
883 {
884  if (imgOrientation < US_IMG_ORIENT_XX || imgOrientation >= US_IMG_ORIENT_LAST)
885  {
886  LOCAL_LOG_ERROR("Invalid image orientation attempted to set in the video buffer: " << imgOrientation);
887  return PLUS_FAIL;
888  }
889  this->ImageOrientation = imgOrientation;
890  for (int frameNumber = 0; frameNumber < this->StreamBuffer->GetBufferSize(); frameNumber++)
891  {
892  this->StreamBuffer->GetBufferItemPointerFromBufferIndex(frameNumber)->GetFrame().SetImageOrientation(imgOrientation);
893  }
894  return PLUS_SUCCESS;
895 }
896 
897 //----------------------------------------------------------------------------
899 {
900  return igsioVideoFrame::GetNumberOfBytesPerScalar(GetPixelType());
901 }
902 
903 //----------------------------------------------------------------------------
905 {
906  return this->GetNumberOfScalarComponents() * igsioVideoFrame::GetNumberOfBytesPerScalar(GetPixelType());
907 }
908 
909 //----------------------------------------------------------------------------
910 PlusStatus vtkPlusBuffer::CopyImagesFromTrackedFrameList(vtkIGSIOTrackedFrameList* sourceTrackedFrameList, TIMESTAMP_FILTERING_OPTION timestampFiltering, bool copyFrameFields)
911 {
912  int numberOfErrors = 0;
913 
914  const int numberOfVideoFrames = sourceTrackedFrameList->GetNumberOfTrackedFrames();
915  LOCAL_LOG_DEBUG("CopyImagesFromTrackedFrameList will copy " << numberOfVideoFrames << " frames");
916 
917  FrameSizeType frameSize = {0, 0, 0};
918  sourceTrackedFrameList->GetTrackedFrame(0)->GetImageData()->GetFrameSize(frameSize);
919  bool isFrameEncoded = sourceTrackedFrameList->GetTrackedFrame(0)->GetImageData()->IsFrameEncoded();
920  this->SetFrameSize(frameSize, !isFrameEncoded);
921  if (!isFrameEncoded)
922  {
923  this->SetPixelType(sourceTrackedFrameList->GetTrackedFrame(0)->GetImageData()->GetVTKScalarPixelType());
924  }
925  unsigned int numberOfScalarComponents(1);
926  if (sourceTrackedFrameList->GetTrackedFrame(0)->GetImageData()->GetNumberOfScalarComponents(numberOfScalarComponents) != PLUS_SUCCESS)
927  {
928  LOG_ERROR("Unable to retrieve number of scalar components.");
929  return PLUS_FAIL;
930  }
931  this->SetNumberOfScalarComponents(numberOfScalarComponents);
932 
933  if (this->SetBufferSize(numberOfVideoFrames) != PLUS_SUCCESS)
934  {
935  LOCAL_LOG_ERROR("Failed to set video buffer size!");
936  return PLUS_FAIL;
937  }
938 
939  bool requireTimestamp = false;
940  if (timestampFiltering == READ_FILTERED_AND_UNFILTERED_TIMESTAMPS || timestampFiltering == READ_FILTERED_IGNORE_UNFILTERED_TIMESTAMPS)
941  {
942  requireTimestamp = true;
943  }
944 
945  bool requireUnfilteredTimestamp = false;
946  if (timestampFiltering == READ_FILTERED_AND_UNFILTERED_TIMESTAMPS || timestampFiltering == READ_UNFILTERED_COMPUTE_FILTERED_TIMESTAMPS)
947  {
948  requireUnfilteredTimestamp = true;
949  }
950 
951  bool requireFrameStatus = false;
952  bool requireFrameNumber = false;
953  if (timestampFiltering == READ_UNFILTERED_COMPUTE_FILTERED_TIMESTAMPS)
954  {
955  // frame status and number is required for the filtered timestamp computation
956  requireFrameStatus = true;
957  requireFrameNumber = true;
958  }
959 
960  LOG_INFO("Copy buffer to video buffer...");
961  for (int frameNumber = 0; frameNumber < numberOfVideoFrames; frameNumber++)
962  {
963  igsioFieldMapType customFields;
964  if (copyFrameFields)
965  {
966  // Copy all custom fields
967  igsioFieldMapType sourceCustomFields = sourceTrackedFrameList->GetTrackedFrame(frameNumber)->GetCustomFields();
968  igsioFieldMapType::iterator fieldIterator;
969  for (fieldIterator = sourceCustomFields.begin(); fieldIterator != sourceCustomFields.end(); fieldIterator++)
970  {
971  // skip special fields
972  if (igsioCommon::IsEqualInsensitive(fieldIterator->first, "TimeStamp"))
973  {
974  continue;
975  }
976  if (igsioCommon::IsEqualInsensitive(fieldIterator->first, "UnfilteredTimestamp"))
977  {
978  continue;
979  }
980  if (igsioCommon::IsEqualInsensitive(fieldIterator->first, "FrameNumber"))
981  {
982  continue;
983  }
984  // add custom field
985  customFields[fieldIterator->first] = fieldIterator->second;
986  }
987  }
988 
989  // read filtered timestamp
990  double timestamp(0);
991  std::string strTimestamp = sourceTrackedFrameList->GetTrackedFrame(frameNumber)->GetFrameField("Timestamp");
992  if (!strTimestamp.empty())
993  {
994  if (igsioCommon::StringToNumber<double>(strTimestamp, timestamp) != PLUS_SUCCESS && requireTimestamp)
995  {
996  LOCAL_LOG_ERROR("Unable to convert Timestamp '" << strTimestamp << "' to double for frame #" << frameNumber);
997  numberOfErrors++;
998  continue;
999  }
1000  }
1001  else if (requireTimestamp)
1002  {
1003  LOCAL_LOG_ERROR("Unable to read Timestamp field of frame #" << frameNumber);
1004  numberOfErrors++;
1005  continue;
1006  }
1007 
1008  // read unfiltered timestamp
1009  double unfilteredtimestamp(0);
1010  std::string strUnfilteredTimestamp = sourceTrackedFrameList->GetTrackedFrame(frameNumber)->GetFrameField("UnfilteredTimestamp");
1011  if (!strUnfilteredTimestamp.empty())
1012  {
1013  if (igsioCommon::StringToNumber<double>(strUnfilteredTimestamp, unfilteredtimestamp) != PLUS_SUCCESS && requireUnfilteredTimestamp)
1014  {
1015  LOCAL_LOG_ERROR("Unable to convert UnfilteredTimestamp '" << strUnfilteredTimestamp << "' to double for frame #" << frameNumber);
1016  numberOfErrors++;
1017  continue;
1018  }
1019  }
1020  else if (requireUnfilteredTimestamp)
1021  {
1022  LOCAL_LOG_ERROR("Unable to read UnfilteredTimestamp field of frame #" << frameNumber);
1023  numberOfErrors++;
1024  continue;
1025  }
1026 
1027  // read frame number
1028  const std::string strFrameNumber = sourceTrackedFrameList->GetTrackedFrame(frameNumber)->GetFrameField("FrameNumber");
1029  unsigned long frmnum(0);
1030  if (!strFrameNumber.empty())
1031  {
1032  if (igsioCommon::StringToNumber<unsigned long>(strFrameNumber, frmnum) != PLUS_SUCCESS && requireFrameNumber)
1033  {
1034  LOCAL_LOG_ERROR("Unable to convert FrameNumber '" << strFrameNumber << "' to integer for frame #" << frameNumber);
1035  numberOfErrors++;
1036  continue;
1037  }
1038  }
1039  else if (requireFrameNumber)
1040  {
1041  LOCAL_LOG_ERROR("Unable to read FrameNumber field of frame #" << frameNumber);
1042  numberOfErrors++;
1043  continue;
1044  }
1045 
1046  std::array<int, 3> clipRectOrigin = {igsioCommon::NO_CLIP, igsioCommon::NO_CLIP, igsioCommon::NO_CLIP};
1047  std::array<int, 3> clipRectSize = {igsioCommon::NO_CLIP, igsioCommon::NO_CLIP, igsioCommon::NO_CLIP};
1048  switch (timestampFiltering)
1049  {
1051  if (this->AddItem(sourceTrackedFrameList->GetTrackedFrame(frameNumber)->GetImageData(), frmnum, clipRectOrigin, clipRectSize, unfilteredtimestamp, timestamp, &customFields) != PLUS_SUCCESS)
1052  {
1053  LOCAL_LOG_WARNING("Failed to add video frame to buffer from sequence metafile with frame #" << frameNumber);
1054  }
1055  break;
1057  if (this->AddItem(sourceTrackedFrameList->GetTrackedFrame(frameNumber)->GetImageData(), frmnum, clipRectOrigin, clipRectSize, unfilteredtimestamp, UNDEFINED_TIMESTAMP, &customFields) != PLUS_SUCCESS)
1058  {
1059  LOCAL_LOG_WARNING("Failed to add video frame to buffer from sequence metafile with frame #" << frameNumber);
1060  }
1061  break;
1063  if (this->AddItem(sourceTrackedFrameList->GetTrackedFrame(frameNumber)->GetImageData(), frmnum, clipRectOrigin, clipRectSize, timestamp, timestamp, &customFields) != PLUS_SUCCESS)
1064  {
1065  LOCAL_LOG_WARNING("Failed to add video frame to buffer from sequence metafile with frame #" << frameNumber);
1066  }
1067  break;
1068  default:
1069  break;
1070  }
1071  }
1072 
1073  return (numberOfErrors > 0 ? PLUS_FAIL : PLUS_SUCCESS);
1074 }
1075 
1076 //----------------------------------------------------------------------------
1077 PlusStatus vtkPlusBuffer::WriteToSequenceFile(const char* filename, bool useCompression /*=false*/)
1078 {
1079  LOG_TRACE("vtkPlusBuffer::WriteToSequenceFile");
1080 
1081  vtkSmartPointer<vtkIGSIOTrackedFrameList> trackedFrameList = vtkSmartPointer<vtkIGSIOTrackedFrameList>::New();
1082 
1083  PlusStatus status = PLUS_SUCCESS;
1084 
1085  for (BufferItemUidType frameUid = this->GetOldestItemUidInBuffer(); frameUid <= this->GetLatestItemUidInBuffer(); ++frameUid)
1086  {
1087  StreamBufferItem bufferItem;
1088  if (this->GetStreamBufferItem(frameUid, &bufferItem) != ITEM_OK)
1089  {
1090  LOCAL_LOG_ERROR("Unable to get frame from buffer with UID: " << frameUid);
1091  status = PLUS_FAIL;
1092  continue;
1093  }
1094 
1095  igsioTrackedFrame* trackedFrame = new igsioTrackedFrame;
1096 
1097  // Add image data
1098  trackedFrame->SetImageData(bufferItem.GetFrame());
1099 
1100  // Add tracking data
1101  vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New();
1102  bufferItem.GetMatrix(matrix);
1103  trackedFrame->SetFrameTransform(igsioTransformName("Tool", "Tracker"), matrix);
1104  trackedFrame->SetFrameTransformStatus(igsioTransformName("Tool", "Tracker"), bufferItem.GetStatus());
1105 
1106  // Add filtered timestamp
1107  double filteredTimestamp = bufferItem.GetFilteredTimestamp(this->GetLocalTimeOffsetSec());
1108  std::ostringstream timestampFieldValue;
1109  timestampFieldValue << std::fixed << filteredTimestamp;
1110  trackedFrame->SetFrameField("Timestamp", timestampFieldValue.str());
1111 
1112  // Add unfiltered timestamp
1113  double unfilteredTimestamp = bufferItem.GetUnfilteredTimestamp(this->GetLocalTimeOffsetSec());
1114  std::ostringstream unfilteredtimestampFieldValue;
1115  unfilteredtimestampFieldValue << std::fixed << unfilteredTimestamp;
1116  trackedFrame->SetFrameField("UnfilteredTimestamp", unfilteredtimestampFieldValue.str());
1117 
1118  // Add frame number
1119  unsigned long frameNumber = bufferItem.GetIndex();
1120  std::ostringstream frameNumberFieldValue;
1121  frameNumberFieldValue << std::fixed << frameNumber;
1122  trackedFrame->SetFrameField("FrameNumber", frameNumberFieldValue.str());
1123 
1124  // Add custom fields
1125  const igsioFieldMapType& customFields = bufferItem.GetFrameFieldMap();
1126  for (igsioFieldMapType::const_iterator cf = customFields.begin(); cf != customFields.end(); ++cf)
1127  {
1128  trackedFrame->SetFrameField(cf->first, cf->second.second, cf->second.first);
1129  }
1130 
1131  // Add tracked frame to the list
1132  trackedFrameList->TakeTrackedFrame(trackedFrame);
1133  }
1134 
1135  // Save tracked frames to metafile
1136  if (vtkPlusSequenceIO::Write(filename, trackedFrameList, trackedFrameList->GetImageOrientation(), useCompression) != PLUS_SUCCESS)
1137  {
1138  LOCAL_LOG_ERROR("Failed to save tracked frames to sequence metafile!");
1139  return PLUS_FAIL;
1140  }
1141 
1142  return status;
1143 }
1144 
1145 //-----------------------------------------------------------------------------
1147 {
1148  this->StreamBuffer->SetTimeStampReporting(enable);
1149 }
1150 
1151 //-----------------------------------------------------------------------------
1153 {
1154  return this->StreamBuffer->GetTimeStampReporting();
1155 }
1156 
1157 //----------------------------------------------------------------------------
1158 // Returns the two buffer items that are closest previous and next buffer items relative to the specified time.
1159 // itemA is the closest item
1161 {
1162  igsioLockGuard<StreamItemCircularBuffer> dataBufferGuardedLock(this->StreamBuffer);
1163 
1164  // The returned item is computed by interpolation between itemA and itemB in time. The itemA is the closest item to the requested time.
1165  // Accept itemA (the closest item) as is if it is very close to the requested time.
1166  // Accept interpolation between itemA and itemB if all the followings are true:
1167  // - both itemA and itemB exist and are valid
1168  // - time difference between the requested time and itemA is below a threshold
1169  // - time difference between the requested time and itemB is below a threshold
1170 
1171  // itemA is the item that is the closest to the requested time, get its UID and time
1172  BufferItemUidType itemAuid(0);
1173  ItemStatus status = this->StreamBuffer->GetItemUidFromTime(time, itemAuid);
1174  if (status != ITEM_OK)
1175  {
1176  switch (status)
1177  {
1179  LOCAL_LOG_DEBUG("vtkPlusBuffer: Cannot get any item from the data buffer for time: " << std::fixed << time << ". Item is not available yet.");
1180  break;
1182  LOCAL_LOG_DEBUG("vtkPlusBuffer: Cannot get any item from the data buffer for time: " << std::fixed << time << ". Item is not available anymore.");
1183  break;
1184  default:
1185  break;
1186  }
1187  return PLUS_FAIL;
1188  }
1189  status = this->GetStreamBufferItem(itemAuid, &itemA);
1190  if (status != ITEM_OK)
1191  {
1192  LOCAL_LOG_ERROR("vtkPlusBuffer: Failed to get data buffer item with Uid: " << itemAuid);
1193  return PLUS_FAIL;
1194  }
1195 
1196  // If tracker is out of view, etc. then we don't have a valid before and after the requested time, so we cannot do interpolation
1197  if (itemA.GetStatus() != TOOL_OK)
1198  {
1199  // tracker is out of view, ...
1200  LOCAL_LOG_DEBUG("vtkPlusBuffer: Cannot do data interpolation. The closest item to the requested time (time: " << std::fixed << time << ", uid: " << itemAuid << ") is invalid.");
1201  return PLUS_FAIL;
1202  }
1203 
1204  double itemAtime(0);
1205  status = this->StreamBuffer->GetTimeStamp(itemAuid, itemAtime);
1206  if (status != ITEM_OK)
1207  {
1208  LOCAL_LOG_ERROR("vtkPlusBuffer: Failed to get data buffer timestamp (time: " << std::fixed << time << ", uid: " << itemAuid << ")");
1209  return PLUS_FAIL;
1210  }
1211 
1212  // If the time difference is negligible then don't interpolate, just return the closest item
1213  if (fabs(itemAtime - time) < NEGLIGIBLE_TIME_DIFFERENCE)
1214  {
1215  //No need for interpolation, it's very close to the closest element
1216  itemB.DeepCopy(&itemA);
1217  return PLUS_SUCCESS;
1218  }
1219 
1220  // If the closest item is too far, then we don't do interpolation
1221  if (fabs(itemAtime - time) > this->GetMaxAllowedTimeDifference())
1222  {
1223  LOCAL_LOG_ERROR("vtkPlusBuffer: Cannot perform interpolation, time difference compared to itemA is too big " << std::fixed << fabs(itemAtime - time) << " ( closest item time: " << itemAtime << ", requested time: " << time << ").");
1224  return PLUS_FAIL;
1225  }
1226 
1227  // Find the closest item on the other side of the timescale (so that time is between itemAtime and itemBtime)
1228  BufferItemUidType itemBuid(0);
1229  if (time < itemAtime)
1230  {
1231  // itemBtime < time <itemAtime
1232  itemBuid = itemAuid - 1;
1233  }
1234  else
1235  {
1236  // itemAtime < time <itemBtime
1237  itemBuid = itemAuid + 1;
1238  }
1239  if (itemBuid < this->GetOldestItemUidInBuffer() || itemBuid > this->GetLatestItemUidInBuffer())
1240  {
1241  // itemB is not available
1242  LOCAL_LOG_ERROR("vtkPlusBuffer: Cannot perform interpolation, itemB is not available " << std::fixed << " ( itemBuid: " << itemBuid << ", oldest UID: " << this->GetOldestItemUidInBuffer() << ", latest UID: " << this->GetLatestItemUidInBuffer());
1243  return PLUS_FAIL;
1244  }
1245  // Get item B details
1246  double itemBtime(0);
1247  status = this->StreamBuffer->GetTimeStamp(itemBuid, itemBtime);
1248  if (status != ITEM_OK)
1249  {
1250  LOCAL_LOG_ERROR("Cannot do interpolation: Failed to get data buffer timestamp with Uid: " << itemBuid);
1251  return PLUS_FAIL;
1252  }
1253  // If the next closest item is too far, then we don't do interpolation
1254  if (fabs(itemBtime - time) > this->GetMaxAllowedTimeDifference())
1255  {
1256  LOCAL_LOG_ERROR("vtkPlusBuffer: Cannot perform interpolation, time difference compared to itemB is too big " << std::fixed << fabs(itemBtime - time) << " ( itemBtime: " << itemBtime << ", requested time: " << time << ").");
1257  return PLUS_FAIL;
1258  }
1259  // Get the item
1260  status = this->GetStreamBufferItem(itemBuid, &itemB);
1261  if (status != ITEM_OK)
1262  {
1263  LOCAL_LOG_ERROR("vtkPlusBuffer: Failed to get data buffer item with Uid: " << itemBuid);
1264  return PLUS_FAIL;
1265  }
1266  // If there is no valid element on the other side of the requested time, then we cannot do an interpolation
1267  if (itemB.GetStatus() != TOOL_OK)
1268  {
1269  LOCAL_LOG_DEBUG("vtkPlusBuffer: Cannot get a second element (uid=" << itemBuid << ") on the other side of the requested time (" << std::fixed << time << ")");
1270  return PLUS_FAIL;
1271  }
1272 
1273  return PLUS_SUCCESS;
1274 }
1275 
1276 //----------------------------------------------------------------------------
1278 {
1279  switch (interpolation)
1280  {
1281  case EXACT_TIME:
1282  return GetStreamBufferItemFromExactTime(time, bufferItem);
1283  case INTERPOLATED:
1284  return GetInterpolatedStreamBufferItemFromTime(time, bufferItem);
1285  case CLOSEST_TIME:
1286  return GetStreamBufferItemFromClosestTime(time, bufferItem);
1287  default:
1288  LOCAL_LOG_WARNING("Unknown interpolation type: " << interpolation << ". Defaulting to exact time request.");
1289  return GetStreamBufferItemFromExactTime(time, bufferItem);
1290  }
1291 }
1292 
1293 //----------------------------------------------------------------------------
1295 {
1296  StreamBufferItem* item;
1297  auto itemStatus = this->StreamBuffer->GetBufferItemPointerFromUid(uid, item);
1298  if (itemStatus == ITEM_OK)
1299  {
1300  item->SetFrameField(key, value);
1301  }
1302  return itemStatus == ITEM_OK ? PLUS_SUCCESS : PLUS_FAIL;
1303 }
1304 
1305 //----------------------------------------------------------------------------
1307 {
1308  ItemStatus status = GetStreamBufferItemFromClosestTime(time, bufferItem);
1309  if (status != ITEM_OK)
1310  {
1311  LOCAL_LOG_WARNING("vtkPlusBuffer: Failed to get data buffer timestamp (time: " << std::fixed << time << ")");
1312  return status;
1313  }
1314 
1315  // If the time difference is not negligible then return with failure
1316  double itemTime = bufferItem->GetFilteredTimestamp(this->StreamBuffer->GetLocalTimeOffsetSec());
1317  if (fabs(itemTime - time) > NEGLIGIBLE_TIME_DIFFERENCE)
1318  {
1319  LOCAL_LOG_WARNING("vtkPlusBuffer: Cannot find an item exactly at the requested time (requested time: " << std::fixed << time << ", item time: " << itemTime << ")");
1320  return ITEM_UNKNOWN_ERROR;
1321  }
1322 
1323  return status;
1324 }
1325 
1326 //----------------------------------------------------------------------------
1328 {
1329  igsioLockGuard<StreamItemCircularBuffer> dataBufferGuardedLock(this->StreamBuffer);
1330 
1331  BufferItemUidType itemUid(0);
1332  ItemStatus status = this->StreamBuffer->GetItemUidFromTime(time, itemUid);
1333  if (status != ITEM_OK)
1334  {
1335  switch (status)
1336  {
1338  LOCAL_LOG_WARNING("vtkPlusBuffer: Cannot get any item from the buffer for time: " << std::fixed << time << ". Item is not available yet.");
1339  break;
1341  LOCAL_LOG_WARNING("vtkPlusBuffer: Cannot get any item from the buffer for time: " << std::fixed << time << ". Item is not available anymore.");
1342  break;
1343  default:
1344  break;
1345  }
1346  return status;
1347  }
1348 
1349  status = this->GetStreamBufferItem(itemUid, bufferItem);
1350  if (status != ITEM_OK)
1351  {
1352  LOCAL_LOG_ERROR("vtkPlusBuffer: Failed to get buffer item with Uid: " << itemUid);
1353  return status;
1354  }
1355 
1356  return status;
1357 }
1358 
1359 //----------------------------------------------------------------------------
1360 // Interpolate the matrix for the given timestamp from the two nearest
1361 // transforms in the buffer.
1362 // The rotation is interpolated with SLERP interpolation, and the
1363 // position is interpolated with linear interpolation.
1364 // The flags correspond to the closest element.
1366 {
1367  StreamBufferItem itemA;
1368  StreamBufferItem itemB;
1369 
1370  if (GetPrevNextBufferItemFromTime(time, itemA, itemB) != PLUS_SUCCESS)
1371  {
1372  // cannot get two neighbors, so cannot do interpolation
1373  // it may be normal (e.g., when tracker out of view), so don't return with an error
1374  ItemStatus status = GetStreamBufferItemFromClosestTime(time, bufferItem);
1375  // Update the timestamp to match the requested time
1376  bufferItem->SetFilteredTimestamp(time);
1377  bufferItem->SetUnfilteredTimestamp(time);
1378  if (status != ITEM_OK)
1379  {
1380  LOCAL_LOG_ERROR("vtkPlusBuffer: Failed to get data buffer timestamp (time: " << std::fixed << time << ")");
1381  return status;
1382  }
1383  bufferItem->SetStatus(TOOL_MISSING); // if we return at any point due to an error then it means that the interpolation is not successful, so the item is missing
1384  return ITEM_OK;
1385  }
1386 
1387  if (itemA.GetUid() == itemB.GetUid())
1388  {
1389  // exact match, no need for interpolation
1390  bufferItem->DeepCopy(&itemA);
1391  return ITEM_OK;
1392  }
1393 
1394  //============== Get item weights ==================
1395 
1396  double itemAtime(0);
1397  if (this->StreamBuffer->GetTimeStamp(itemA.GetUid(), itemAtime) != ITEM_OK)
1398  {
1399  LOCAL_LOG_ERROR("vtkPlusBuffer: Failed to get data buffer timestamp (time: " << std::fixed << time << ", uid: " << itemA.GetUid() << ")");
1400  return ITEM_UNKNOWN_ERROR;
1401  }
1402 
1403  double itemBtime(0);
1404  if (this->StreamBuffer->GetTimeStamp(itemB.GetUid(), itemBtime) != ITEM_OK)
1405  {
1406  LOCAL_LOG_ERROR("vtkPlusBuffer: Failed to get data buffer timestamp (time: " << std::fixed << time << ", uid: " << itemB.GetUid() << ")");
1407  return ITEM_UNKNOWN_ERROR;
1408  }
1409 
1410  if (fabs(itemAtime - itemBtime) < NEGLIGIBLE_TIME_DIFFERENCE)
1411  {
1412  // exact time match, no need for interpolation
1413  bufferItem->DeepCopy(&itemA);
1414  bufferItem->SetFilteredTimestamp(time);
1415  bufferItem->SetUnfilteredTimestamp(time);
1416  return ITEM_OK;
1417  }
1418 
1419  double itemAweight = fabs(itemBtime - time) / fabs(itemAtime - itemBtime);
1420  double itemBweight = 1 - itemAweight;
1421 
1422  //============== Get transform matrices ==================
1423 
1424  vtkSmartPointer<vtkMatrix4x4> itemAmatrix = vtkSmartPointer<vtkMatrix4x4>::New();
1425  if (itemA.GetMatrix(itemAmatrix) != PLUS_SUCCESS)
1426  {
1427  LOCAL_LOG_ERROR("Failed to get item A matrix");
1428  return ITEM_UNKNOWN_ERROR;
1429  }
1430  double matrixA[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
1431  double xyzA[3] = {0, 0, 0};
1432  for (int i = 0; i < 3; i++)
1433  {
1434  matrixA[i][0] = itemAmatrix->GetElement(i, 0);
1435  matrixA[i][1] = itemAmatrix->GetElement(i, 1);
1436  matrixA[i][2] = itemAmatrix->GetElement(i, 2);
1437  xyzA[i] = itemAmatrix->GetElement(i, 3);
1438  }
1439 
1440  vtkSmartPointer<vtkMatrix4x4> itemBmatrix = vtkSmartPointer<vtkMatrix4x4>::New();
1441  if (itemB.GetMatrix(itemBmatrix) != PLUS_SUCCESS)
1442  {
1443  LOCAL_LOG_ERROR("Failed to get item B matrix");
1444  return ITEM_UNKNOWN_ERROR;
1445  }
1446  double matrixB[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
1447  double xyzB[3] = {0, 0, 0};
1448  for (int i = 0; i < 3; i++)
1449  {
1450  matrixB[i][0] = itemBmatrix->GetElement(i, 0);
1451  matrixB[i][1] = itemBmatrix->GetElement(i, 1);
1452  matrixB[i][2] = itemBmatrix->GetElement(i, 2);
1453  xyzB[i] = itemBmatrix->GetElement(i, 3);
1454  }
1455 
1456  //============== Interpolate rotation ==================
1457 
1458  double matrixAquat[4] = {0, 0, 0, 0};
1459  vtkMath::Matrix3x3ToQuaternion(matrixA, matrixAquat);
1460  double matrixBquat[4] = {0, 0, 0, 0};
1461  vtkMath::Matrix3x3ToQuaternion(matrixB, matrixBquat);
1462  double interpolatedRotationQuat[4] = {0, 0, 0, 0};
1463  igsioMath::Slerp(interpolatedRotationQuat, itemBweight, matrixAquat, matrixBquat);
1464  double interpolatedRotation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
1465  vtkMath::QuaternionToMatrix3x3(interpolatedRotationQuat, interpolatedRotation);
1466 
1467  vtkSmartPointer<vtkMatrix4x4> interpolatedMatrix = vtkSmartPointer<vtkMatrix4x4>::New();
1468  for (int i = 0; i < 3; i++)
1469  {
1470  interpolatedMatrix->Element[i][0] = interpolatedRotation[i][0];
1471  interpolatedMatrix->Element[i][1] = interpolatedRotation[i][1];
1472  interpolatedMatrix->Element[i][2] = interpolatedRotation[i][2];
1473  interpolatedMatrix->Element[i][3] = xyzA[i] * itemAweight + xyzB[i] * itemBweight;
1474  }
1475 
1476  //============== Interpolate time ==================
1477 
1478  double itemAunfilteredTimestamp = itemA.GetUnfilteredTimestamp(0.0); // 0.0 because timestamps in the buffer are in local time
1479  double itemBunfilteredTimestamp = itemB.GetUnfilteredTimestamp(0.0); // 0.0 because timestamps in the buffer are in local time
1480  double interpolatedUnfilteredTimestamp = itemAunfilteredTimestamp * itemAweight + itemBunfilteredTimestamp * itemBweight;
1481 
1482  //============== Write interpolated results into the bufferItem ==================
1483 
1484  bufferItem->DeepCopy(&itemA);
1485  bufferItem->SetMatrix(interpolatedMatrix);
1486  bufferItem->SetFilteredTimestamp(time - this->StreamBuffer->GetLocalTimeOffsetSec()); // global = local + offset => local = global - offset
1487  bufferItem->SetUnfilteredTimestamp(interpolatedUnfilteredTimestamp);
1488 
1489  double angleDiffA = igsioMath::GetOrientationDifference(interpolatedMatrix, itemAmatrix);
1490  double angleDiffB = igsioMath::GetOrientationDifference(interpolatedMatrix, itemBmatrix);
1491  if (fabs(angleDiffA) > ANGLE_INTERPOLATION_WARNING_THRESHOLD_DEG && fabs(angleDiffB) > ANGLE_INTERPOLATION_WARNING_THRESHOLD_DEG)
1492  {
1493  static vtkIGSIOLogHelper helper(5.f, 5000, vtkPlusLogger::LOG_LEVEL_WARNING);
1494  if (helper.ShouldWeLog(true))
1495  {
1496  LOCAL_LOG_WARNING("Angle difference between interpolated orientations is large (" << fabs(angleDiffA) << " and " << fabs(angleDiffB) << " deg, warning threshold is " << ANGLE_INTERPOLATION_WARNING_THRESHOLD_DEG << "), interpolation may be inaccurate. Consider moving the tools slower.");
1497  }
1498  }
1499 
1500  return ITEM_OK;
1501 }
1502 
1503 //-----------------------------------------------------------------------------
1504 PlusStatus vtkPlusBuffer::CopyTransformFromTrackedFrameList(vtkIGSIOTrackedFrameList* sourceTrackedFrameList, TIMESTAMP_FILTERING_OPTION timestampFiltering, igsioTransformName& transformName)
1505 {
1506  int numberOfErrors = 0;
1507 
1508  int numberOfFrames = sourceTrackedFrameList->GetNumberOfTrackedFrames();
1509  this->SetBufferSize(numberOfFrames + 1);
1510 
1511  bool requireTimestamp = false;
1512  if (timestampFiltering == READ_FILTERED_AND_UNFILTERED_TIMESTAMPS || timestampFiltering == READ_FILTERED_IGNORE_UNFILTERED_TIMESTAMPS)
1513  {
1514  requireTimestamp = true;
1515  }
1516 
1517  bool requireUnfilteredTimestamp = false;
1518  if (timestampFiltering == READ_FILTERED_AND_UNFILTERED_TIMESTAMPS || timestampFiltering == READ_UNFILTERED_COMPUTE_FILTERED_TIMESTAMPS)
1519  {
1520  requireUnfilteredTimestamp = true;
1521  }
1522 
1523  bool requireFrameStatus = false;
1524  bool requireFrameNumber = false;
1525  if (timestampFiltering == READ_UNFILTERED_COMPUTE_FILTERED_TIMESTAMPS)
1526  {
1527  // frame status and number is required for the filtered timestamp computation
1528  requireFrameStatus = true;
1529  requireFrameNumber = true;
1530  }
1531 
1532  for (int frameNumber = 0; frameNumber < numberOfFrames; frameNumber++)
1533  {
1534 
1535  // read filtered timestamp
1536  double timestamp(0);
1537  const std::string strTimestamp = sourceTrackedFrameList->GetTrackedFrame(frameNumber)->GetFrameField("Timestamp");
1538  if (!strTimestamp.empty())
1539  {
1540  if (igsioCommon::StringToNumber<double>(strTimestamp, timestamp) != PLUS_SUCCESS && requireTimestamp)
1541  {
1542  LOCAL_LOG_ERROR("Unable to convert Timestamp '" << strTimestamp << "' to double");
1543  numberOfErrors++;
1544  continue;
1545  }
1546  }
1547  else if (requireTimestamp)
1548  {
1549  LOCAL_LOG_ERROR("Unable to read Timestamp field of frame #" << frameNumber);
1550  numberOfErrors++;
1551  continue;
1552  }
1553 
1554  // read unfiltered timestamp
1555  double unfilteredtimestamp(0);
1556  const std::string strUnfilteredTimestamp = sourceTrackedFrameList->GetTrackedFrame(frameNumber)->GetFrameField("UnfilteredTimestamp");
1557  if (!strUnfilteredTimestamp.empty())
1558  {
1559  if (igsioCommon::StringToNumber<double>(strUnfilteredTimestamp, unfilteredtimestamp) != PLUS_SUCCESS && requireUnfilteredTimestamp)
1560  {
1561  LOCAL_LOG_ERROR("Unable to convert UnfilteredTimestamp '" << strUnfilteredTimestamp << "' to double");
1562  numberOfErrors++;
1563  continue;
1564  }
1565  }
1566  else if (requireUnfilteredTimestamp)
1567  {
1568  LOCAL_LOG_ERROR("Unable to read UnfilteredTimestamp field of frame #" << frameNumber);
1569  numberOfErrors++;
1570  continue;
1571  }
1572 
1573  // read status
1574  ToolStatus toolStatus(TOOL_OK);
1575  if (sourceTrackedFrameList->GetTrackedFrame(frameNumber)->GetFrameTransformStatus(transformName, toolStatus) != PLUS_SUCCESS && requireFrameStatus)
1576  {
1577  LOCAL_LOG_ERROR("Unable to read TransformStatus field of frame #" << frameNumber);
1578  numberOfErrors++;
1579  continue;
1580  }
1581 
1582  // read frame number
1583  const std::string strFrameNumber = sourceTrackedFrameList->GetTrackedFrame(frameNumber)->GetFrameField("FrameNumber");
1584  unsigned long frmnum(0);
1585  if (!strFrameNumber.empty())
1586  {
1587  if (igsioCommon::StringToNumber<unsigned long>(strFrameNumber, frmnum) != PLUS_SUCCESS && requireFrameNumber)
1588  {
1589  LOCAL_LOG_ERROR("Unable to convert FrameNumber '" << strFrameNumber << "' to integer for frame #" << frameNumber);
1590  numberOfErrors++;
1591  continue;
1592  }
1593  }
1594  else if (requireFrameNumber)
1595  {
1596  LOCAL_LOG_ERROR("Unable to read FrameNumber field of frame #" << frameNumber);
1597  numberOfErrors++;
1598  continue;
1599  }
1600 
1601  double copiedTransform[16] = {0};
1602  if (!sourceTrackedFrameList->GetTrackedFrame(frameNumber)->GetFrameTransform(transformName, copiedTransform))
1603  {
1604  std::string strTransformName;
1605  transformName.GetTransformName(strTransformName);
1606  LOCAL_LOG_ERROR("Unable to get the " << strTransformName << " frame transform for frame #" << frameNumber);
1607  numberOfErrors++;
1608  continue;
1609  }
1610 
1611  vtkSmartPointer<vtkMatrix4x4> copiedTransformMatrix = vtkSmartPointer<vtkMatrix4x4>::New();
1612  copiedTransformMatrix->DeepCopy(copiedTransform);
1613 
1614  switch (timestampFiltering)
1615  {
1617  this->AddTimeStampedItem(copiedTransformMatrix, toolStatus, frmnum, unfilteredtimestamp, timestamp);
1618  break;
1620  this->AddTimeStampedItem(copiedTransformMatrix, toolStatus, frmnum, unfilteredtimestamp);
1621  break;
1623  this->AddTimeStampedItem(copiedTransformMatrix, toolStatus, frmnum, timestamp, timestamp);
1624  break;
1625  default:
1626  break;
1627  }
1628 
1629  }
1630 
1631  return (numberOfErrors > 0 ? PLUS_FAIL : PLUS_SUCCESS);
1632 }
1633 
1634 //-----------------------------------------------------------------------------
1635 PlusStatus vtkPlusBuffer::GetFrameSize(unsigned int& _arg1, unsigned int& _arg2, unsigned int& _arg3) const
1636 {
1637  _arg1 = this->FrameSize[0];
1638  _arg2 = this->FrameSize[1];
1639  _arg3 = this->FrameSize[2];
1640 
1641  return PLUS_SUCCESS;
1642 }
1643 
1644 //-----------------------------------------------------------------------------
1645 FrameSizeType vtkPlusBuffer::GetFrameSize() const
1646 {
1647  return this->FrameSize;
1648 }
1649 
1650 //-----------------------------------------------------------------------------
1652 {
1654 }
1655 
1656 //-----------------------------------------------------------------------------
1658 {
1660 }
1661 
1662 //----------------------------------------------------------------------------
1664 {
1666 }
1667 
1668 #undef LOCAL_LOG_ERROR
1669 #undef LOCAL_LOG_WARNING
1670 #undef LOCAL_LOG_DEBUG
ItemStatus GetBufferIndexFromTime(const double time, int &bufferIndex)
void SetTimeStampReporting(bool enable)
virtual void SetStartTime(double)
virtual ItemStatus GetBufferItemPointerFromUid(const BufferItemUidType uid, StreamBufferItem *&itemPtr)
ToolStatus GetStatus() const
const char * key
Definition: phidget22.h:5111
PlusStatus SetImageType(US_IMAGE_TYPE imageType)
DataItemTemporalInterpolationType
Definition: vtkPlusBuffer.h:45
static const double NEGLIGIBLE_TIME_DIFFERENCE
FrameSizeType FrameSize
double * timestamp
Definition: phidget22.h:3432
virtual US_IMAGE_ORIENTATION GetImageOrientation()
virtual ItemStatus GetLatestTimeStamp(double &timestamp)
virtual void SetLocalTimeOffsetSec(double)
virtual FrameSizeType GetFrameSize() const
virtual void DeepCopy(vtkPlusTimestampedCircularBuffer *buffer)
virtual void SetStartTime(double startTime)
igsioStatus PlusStatus
Definition: PlusCommon.h:40
virtual double GetLocalTimeOffsetSec()
PlusStatus SetImageOrientation(US_IMAGE_ORIENTATION imageOrientation)
virtual PlusStatus ModifyBufferItemFrameField(BufferItemUidType uid, const std::string &key, const std::string &value)
virtual void PrintSelf(ostream &os, vtkIndent indent) VTK_OVERRIDE
for i
virtual PlusStatus PrepareForNewItem(const double timestamp, BufferItemUidType &newFrameUid, int &bufferIndex)
void SetFilteredTimestamp(double filteredTimestamp)
This class stores an fixed number of timestamped items. It provides element retrieval based on timest...
virtual double GetLocalTimeOffsetSec()
US_IMAGE_ORIENTATION ImageOrientation
#define PLUS_FAIL
Definition: PlusCommon.h:43
virtual igsioCommon::VTKScalarPixelType GetPixelType()
void SetUid(BufferItemUidType uid)
#define LOCAL_LOG_WARNING(msg)
void SetValidTransformData(bool aValid)
virtual PlusStatus SetBufferSize(int n)
virtual bool GetLatestItemHasValidFieldData()
virtual void DeepCopy(vtkPlusBuffer *buffer)
virtual PlusStatus AddItem(vtkImageData *frame, US_IMAGE_ORIENTATION usImageOrientation, US_IMAGE_TYPE imageType, long frameNumber, const std::array< int, 3 > &clipRectangleOrigin, const std::array< int, 3 > &clipRectangleSize, double unfilteredTimestamp=UNDEFINED_TIMESTAMP, double filteredTimestamp=UNDEFINED_TIMESTAMP, const igsioFieldMapType *customFields=NULL)
virtual ItemStatus GetItemUidFromTime(const double time, BufferItemUidType &uid)
igsioFieldMapType GetFrameFieldMap()
virtual PlusStatus CreateFilteredTimeStampForItem(unsigned long itemIndex, double inUnfilteredTimestamp, double &outFilteredTimestamp, bool &filteredTimestampProbablyValid)
virtual BufferItemUidType GetLatestItemUidInBuffer()
double GetFilteredTimestamp(double localTimeOffsetSec)
virtual ItemStatus GetOldestTimeStamp(double &oldestTimestamp)
static igsioStatus Write(const std::string &filename, igsioTrackedFrame *frame, US_IMAGE_ORIENTATION orientationInFile=US_IMG_ORIENT_MF, bool useCompression=true, bool EnableImageDataWrite=true)
US_IMAGE_TYPE ImageType
virtual void SetAveragedItemsForFiltering(unsigned int)
PlusStatus GetPrevNextBufferItemFromTime(double time, StreamBufferItem &itemA, StreamBufferItem &itemB)
virtual void SetDescriptiveName(const char *)
virtual bool CheckFrameFormat(const FrameSizeType &frameSizeInPx, igsioCommon::VTKScalarPixelType pixelType, US_IMAGE_TYPE imgType, int numberOfScalarComponents)
#define PLUS_SUCCESS
Definition: PlusCommon.h:44
void PrintSelf(ostream &os, vtkIndent indent)
PhidgetGPS_Time * time
Definition: phidget22.h:3623
PlusStatus GetTimeStampReportTable(vtkTable *timeStampReportTable)
virtual int GetAveragedItemsForFiltering()
virtual unsigned int GetNumberOfScalarComponents()
virtual double GetMaxAllowedTimeDifference()
virtual void SetLocalTimeOffsetSec(double offsetSec)
virtual ItemStatus GetStreamBufferItemFromTime(double time, StreamBufferItem *bufferItem, DataItemTemporalInterpolationType interpolation)
void SetIndex(unsigned long index)
unsigned int NumberOfScalarComponents
PlusStatus GetMatrix(vtkMatrix4x4 *outputMatrix)
PlusStatus SetFrameSize(unsigned int x, unsigned int y, unsigned int z, bool allocateFrames=true)
unsigned long GetIndex()
PlusStatus SetPixelType(igsioCommon::VTKScalarPixelType pixelType)
virtual PlusStatus GetTimeStampReportTable(vtkTable *timeStampReportTable)
void AddToTimeStampReport(unsigned long itemIndex, double unfilteredTimestamp, double filteredTimestamp)
double GetUnfilteredTimestamp(double localTimeOffsetSec)
virtual US_IMAGE_TYPE GetImageType()
virtual bool GetLatestItemHasValidTransformData()
virtual int GetBufferSize()
int x
Definition: phidget22.h:4265
igsioVideoFrame & GetFrame()
virtual ItemStatus GetTimeStamp(BufferItemUidType uid, double &timestamp)
PlusStatus DeepCopy(StreamBufferItem *dataItem)
void SetFrameField(std::string fieldName, std::string fieldValue, igsioFrameFieldFlags flags=FRAMEFIELD_NONE)
virtual ItemStatus GetOldestTimeStamp(double &timestamp)
const char const char * value
Definition: phidget22.h:5111
StreamItemCircularBuffer * StreamBuffer
virtual int GetAveragedItemsForFiltering()
virtual void SetTimeStampReporting(bool)
int VTKScalarPixelType
Definition: PlusCommon.h:55
PlusStatus SetMatrix(vtkMatrix4x4 *matrix)
virtual ItemStatus GetIndex(const BufferItemUidType uid, unsigned long &index)
BufferItemUidType GetUid()
virtual ItemStatus GetStreamBufferItem(BufferItemUidType uid, StreamBufferItem *bufferItem)
virtual ItemStatus GetStreamBufferItemFromExactTime(double time, StreamBufferItem *bufferItem)
igsioCommon::VTKScalarPixelType PixelType
virtual ItemStatus GetBufferIndexFromTime(const double time, int &bufferIndex)
void SetUnfilteredTimestamp(double unfilteredTimestamp)
#define LOCAL_LOG_DEBUG(msg)
Direction vectors of rods y
Definition: algo3.m:15
virtual ItemStatus GetLatestTimeStamp(double &latestTimestamp)
virtual StreamBufferItem * GetBufferItemPointerFromBufferIndex(const int bufferIndex)
virtual ItemStatus GetIndex(const BufferItemUidType uid, unsigned long &index)
virtual ItemStatus GetTimeStamp(const BufferItemUidType uid, double &timestamp)
virtual void SetAveragedItemsForFiltering(int averagedItemsForFiltering)
vtkStandardNewMacro(vtkPlusBuffer)
virtual void Clear()
PlusStatus CopyImagesFromTrackedFrameList(vtkIGSIOTrackedFrameList *sourceTrackedFrameList, TIMESTAMP_FILTERING_OPTION timestampFiltering, bool copyFrameFields)
virtual BufferItemUidType GetOldestItemUidInBuffer()
bool GetTimeStampReporting()
virtual ItemStatus GetStreamBufferItemFromClosestTime(double time, StreamBufferItem *bufferItem)
int GetNumberOfBytesPerScalar()
unsigned long long BufferItemUidType
void SetStatus(ToolStatus status)
virtual double GetStartTime()
virtual PlusStatus AllocateMemoryForFrames()
static const double ANGLE_INTERPOLATION_WARNING_THRESHOLD_DEG
PlusStatus SetNumberOfScalarComponents(unsigned int numberOfScalarComponents)
int GetNumberOfBytesPerPixel()
virtual ItemStatus GetInterpolatedStreamBufferItemFromTime(double time, StreamBufferItem *bufferItem)
virtual bool GetLatestItemHasValidVideoData()
virtual PlusStatus WriteToSequenceFile(const char *filename, bool useCompression=false)
PlusStatus AddTimeStampedItem(vtkMatrix4x4 *matrix, ToolStatus status, unsigned long frameNumber, double unfilteredTimestamp, double filteredTimestamp=UNDEFINED_TIMESTAMP, const igsioFieldMapType *customFields=NULL)
PlusStatus CopyTransformFromTrackedFrameList(vtkIGSIOTrackedFrameList *sourceTrackedFrameList, TIMESTAMP_FILTERING_OPTION timestampFiltering, igsioTransformName &transformName)
#define LOCAL_LOG_ERROR(msg)