7 #include "PlusConfigure.h" 10 #include "vtkDoubleArray.h" 11 #include "vtkIGSIORecursiveCriticalSection.h" 13 #include "vtkVariantArray.h" 19 : Mutex(vtkIGSIORecursiveCriticalSection::New())
22 , CurrentTimeStamp(0.0)
23 , LocalTimeOffsetSec(0.0)
25 , AveragedItemsForFiltering(20)
26 , MaxAllowedFilteringTimeDifference(0.5)
27 , TimeStampReportTable(NULL)
28 , TimeStampReporting(false)
29 , TimeStampLogging(false)
31 , NegligibleTimeDifferenceSec(1e-5)
46 if (this->
Mutex != NULL)
48 this->
Mutex->Delete();
65 os << indent <<
"BufferSize: " << this->
GetBufferSize() <<
"\n";
66 os << indent <<
"NumberOfItems: " << this->
NumberOfItems <<
"\n";
69 os << indent <<
"Latest Item Uid: " << this->
LatestItemUid <<
"\n";
75 igsioLockGuard< vtkPlusTimestampedCircularBuffer > bufferGuardedLock(
this);
79 LOG_DEBUG(
"Need to skip newly added frame - new timestamp (" << std::fixed <<
timestamp <<
") is not newer than the last one (" << this->CurrentTimeStamp <<
")!");
107 if (newBufferSize < 0)
109 LOG_ERROR(
"SetBufferSize: invalid buffer size");
113 igsioLockGuard< vtkPlusTimestampedCircularBuffer > bufferGuardedLock(
this);
115 if (newBufferSize == this->
GetBufferSize() && newBufferSize != 0)
122 for (
int i = 0;
i < newBufferSize;
i++)
135 const int numberOfNewBufferObjects = newBufferSize - this->
GetBufferSize();
136 for (
int i = 0;
i < numberOfNewBufferObjects; ++
i)
147 for (
int i = 0;
i < oldBufferSize - newBufferSize; ++
i)
176 LOG_WARNING(
"Buffer item is not in the buffer (Uid: " << uid <<
")!");
182 LOG_WARNING(
"Buffer item is not in the buffer (Uid: " << uid <<
")!");
203 LOG_ERROR(
"Failed to get buffer item with buffer index - index is out of range (bufferIndex: " << bufferIndex <<
").");
212 igsioLockGuard< vtkPlusTimestampedCircularBuffer > bufferGuardedLock(
this);
217 filteredTimestamp = 0;
227 igsioLockGuard< vtkPlusTimestampedCircularBuffer > bufferGuardedLock(
this);
232 unfilteredTimestamp = 0;
242 igsioLockGuard< vtkPlusTimestampedCircularBuffer > bufferGuardedLock(
this);
254 igsioLockGuard< vtkPlusTimestampedCircularBuffer > bufferGuardedLock(
this);
266 igsioLockGuard< vtkPlusTimestampedCircularBuffer > bufferGuardedLock(
this);
278 igsioLockGuard< vtkPlusTimestampedCircularBuffer > bufferGuardedLock(
this);
293 igsioLockGuard< vtkPlusTimestampedCircularBuffer > bufferGuardedLock(
this);
300 LOG_WARNING(
"Buffer item is not in the buffer (time: " << std::fixed <<
time <<
")!");
317 igsioLockGuard< vtkPlusTimestampedCircularBuffer > bufferGuardedLock(
this);
332 if (loBufferIndex < 0)
340 if (hiBufferIndex < 0)
352 else if (
time > thi + this->NegligibleTimeDifferenceSec)
373 int mid = (lo + hi) / 2;
377 if (midBufferIndex < 0)
434 bool cannotComputeIdealFrameRateDueToInvalidFrameNumbers =
false;
436 std::vector<double> framePeriods;
445 unsigned long framenum(0);
457 unsigned long prevframenum(0);
463 double frameperiod = (
time - prevtime);
464 int frameDiff = framenum - prevframenum;
470 frameperiod /= (1.0 * frameDiff);
475 cannotComputeIdealFrameRateDueToInvalidFrameNumbers =
true;
481 framePeriods.push_back(frameperiod);
485 if (cannotComputeIdealFrameRateDueToInvalidFrameNumbers)
487 LOG_WARNING(
"Cannot compute ideal frame rate acurately, as frame numbers are invalid or missing");
490 const int numberOfFramePeriods = framePeriods.size();
491 if (numberOfFramePeriods < 1)
493 LOG_WARNING(
"Failed to compute frame rate. Not enough samples.");
497 double samplingPeriod(0);
498 for (
int i = 0;
i < numberOfFramePeriods;
i++)
500 samplingPeriod += framePeriods[
i];
502 samplingPeriod /= 1.0 * numberOfFramePeriods;
505 if (samplingPeriod != 0)
507 frameRate = 1.0 / samplingPeriod;
510 if (framePeriodStdevSecPtr != NULL)
514 double sumOfXiMeanDiffSquare = 0;
515 for (
int i = 0;
i < numberOfFramePeriods;
i++)
517 sumOfXiMeanDiffSquare += (framePeriods[
i] - samplingPeriod) * (framePeriods[
i] - samplingPeriod);
519 double framePeriodStdev = sqrt(sumOfXiMeanDiffSquare / numberOfFramePeriods);
520 (*framePeriodStdevSecPtr) = framePeriodStdev;
532 filteredTimestampProbablyValid =
true;
564 if (this->AveragedItemsForFiltering < 2 || this->FilterContainersNumberOfValidElements < this->
AveragedItemsForFiltering)
566 outFilteredTimestamp = inUnfilteredTimestamp;
596 double covarianceXY = 0;
597 double varianceX = 0;
602 varianceX += xiMinusXmean * xiMinusXmean;
604 double a = covarianceXY / varianceX;
605 double b = yMean - a * xMean;
607 outFilteredTimestamp = a * itemIndex +
b;
620 filteredTimestampProbablyValid =
false;
621 LOG_DEBUG(
"Difference between unfiltered timestamp is larger than the threshold. The unfiltered timestamp may be incorrect." 622 <<
" Unfiltered timestamp: " << inUnfilteredTimestamp <<
", filtered timestamp: " << outFilteredTimestamp <<
", difference: " << fabs(outFilteredTimestamp - inUnfilteredTimestamp) <<
", threshold: " << this->
MaxAllowedFilteringTimeDifference <<
"." 634 if (timeStampReportTable == NULL)
636 LOG_ERROR(
"Failed to get timestamp report table from buffer - input table is NULL!");
642 LOG_ERROR(
"Failed to get timestamp report table from buffer - table is NULL!");
666 const char* colFrameNumberName =
"FrameNumber";
667 vtkSmartPointer<vtkDoubleArray> colFrameNumber = vtkSmartPointer<vtkDoubleArray>::New();
668 colFrameNumber->SetName(colFrameNumberName);
671 const char* colUnfilteredTimestampName =
"UnfilteredTimestamp";
672 vtkSmartPointer<vtkDoubleArray> colUnfilteredTimestamp = vtkSmartPointer<vtkDoubleArray>::New();
673 colUnfilteredTimestamp->SetName(colUnfilteredTimestampName);
676 const char* colFilteredTimestampName =
"FilteredTimestamp";
677 vtkSmartPointer<vtkDoubleArray> colFilteredTimestamp = vtkSmartPointer<vtkDoubleArray>::New();
678 colFilteredTimestamp->SetName(colFilteredTimestampName);
683 vtkSmartPointer<vtkVariantArray> timeStampReportTableRow = vtkSmartPointer<vtkVariantArray>::New();
685 timeStampReportTableRow->InsertNextValue(itemIndex);
686 timeStampReportTableRow->InsertNextValue(unfilteredTimestamp - this->
StartTime);
687 timeStampReportTableRow->InsertNextValue(filteredTimestamp - this->
StartTime);
virtual BufferItemUidType GetOldestItemUidInBuffer()
virtual ItemStatus GetBufferItemPointerFromUid(const BufferItemUidType uid, StreamBufferItem *&itemPtr)
unsigned int FilterContainersOldestIndex
vtkStandardNewMacro(vtkPlusTimestampedCircularBuffer)
double MaxAllowedFilteringTimeDifference
vtkIGSIORecursiveCriticalSection * Mutex
virtual void DeepCopy(vtkPlusTimestampedCircularBuffer *buffer)
virtual double GetFrameRate(bool ideal=false, double *framePeriodStdevSecPtr=NULL)
virtual PlusStatus PrepareForNewItem(const double timestamp, BufferItemUidType &newFrameUid, int &bufferIndex)
This class stores an fixed number of timestamped items. It provides element retrieval based on timest...
vnl_vector< double > FilterContainerIndexVector
virtual PlusStatus SetBufferSize(int n)
virtual int GetBufferSize()
unsigned int AveragedItemsForFiltering
Initial rotation matrix b
virtual ItemStatus GetItemUidFromTime(const double time, BufferItemUidType &uid)
virtual PlusStatus CreateFilteredTimeStampForItem(unsigned long itemIndex, double inUnfilteredTimestamp, double &outFilteredTimestamp, bool &filteredTimestampProbablyValid)
double GetFilteredTimestamp(double localTimeOffsetSec)
virtual BufferItemUidType GetLatestItemUidInBuffer()
void PrintSelf(ostream &os, vtkIndent indent)
PlusStatus GetTimeStampReportTable(vtkTable *timeStampReportTable)
virtual bool GetLatestItemHasValidFieldData()
void AddToTimeStampReport(unsigned long itemIndex, double unfilteredTimestamp, double filteredTimestamp)
vtkPlusTimestampedCircularBuffer()
double GetUnfilteredTimestamp(double localTimeOffsetSec)
vnl_vector< double > FilterContainerTimestampVector
virtual ItemStatus GetFilteredTimeStamp(const BufferItemUidType uid, double &filteredTimestamp)
unsigned int FilterContainersNumberOfValidElements
vtkTable * TimeStampReportTable
double NegligibleTimeDifferenceSec
std::deque< StreamBufferItem > BufferItemContainer
virtual bool GetLatestItemHasValidTransformData()
virtual ItemStatus GetBufferIndexFromTime(const double time, int &bufferIndex)
virtual StreamBufferItem * GetBufferItemPointerFromBufferIndex(const int bufferIndex)
virtual ItemStatus GetIndex(const BufferItemUidType uid, unsigned long &index)
virtual ItemStatus GetTimeStamp(const BufferItemUidType uid, double ×tamp)
BufferItemUidType LatestItemUid
virtual ItemStatus GetUnfilteredTimeStamp(const BufferItemUidType uid, double &unfilteredTimestamp)
unsigned long long BufferItemUidType
double LocalTimeOffsetSec
~vtkPlusTimestampedCircularBuffer()
virtual bool GetLatestItemHasValidVideoData()