PlusLib  2.9.0
Software library for tracked ultrasound image acquisition, calibration, and processing.
vtkPlusClarius.cxx
Go to the documentation of this file.
1 /*=Plus=header=begin======================================================
2  Program: Plus
3  Copyright (c) UBC Biomedical Signal and Image Computing Laboratory. All rights reserved.
4  See License.txt for details.
5 =========================================================Plus=header=end*/
6 
7 // Local includes
8 #include "PlusConfigure.h"
9 #include "PixelCodec.h"
10 #include "vtkPlusChannel.h"
11 #include "vtkPlusDataSource.h"
13 #include "vtkPlusClarius.h"
14 
15 // IGSIO includes
16 #include <vtkIGSIOAccurateTimer.h>
17 #include <igsioMath.h>
18 
19 // VTK includes
20 #include <vtk_zlib.h>
21 #include <vtkImageData.h>
22 #include <vtkImageImport.h>
23 #include <vtkMatrix4x4.h>
24 #include <vtkObjectFactory.h>
25 #include <vtkSmartPointer.h>
26 #include <vtkTransform.h>
27 #include <vtkXMLUtilities.h>
28 
29 // std includes
30 #include <stdio.h>
31 #include <stdlib.h>
32 #include <string>
33 #include <iostream>
34 #include <atomic>
35 #include <thread>
36 #include <vector>
37 #include <fstream>
38 
39 // vtkxio includes
40 #include "MadgwickAhrsAlgo.h"
41 #include "MahonyAhrsAlgo.h"
42 
43 // OpenCV includes
44 #include <opencv2/imgproc.hpp>
45 #include <opencv2/core/mat.hpp>
46 #include <opencv2/opencv.hpp>
47 
48 #include <stdio.h>
49 #ifdef _MSC_VER
50 #else
51 #include <unistd.h>
52 #endif
53 
54 // Clarius API
55 #include <cast.h>
56 
57 #define BLOCKINGCALL nullptr
58 #define DEFAULT_FRAME_WIDTH 640
59 #define DEFAULT_FRAME_HEIGHT 480
60 #define DEFAULT_PATH_TO_SEC_KEY "/tmp/"
61 
62 //-------------------------------------------------------------------------------------------------
63 // vtkInternal
64 //-------------------------------------------------------------------------------------------------
65 
66 class vtkPlusClarius::vtkInternal
67 {
68 public:
69  vtkInternal(vtkPlusClarius* external);
70 
71  virtual ~vtkInternal() = default;
72  friend class vtkPlusClarius;
73 
74 protected:
75 
77  // Clarius callback functions
78 
79  static void ErrorFn(const char* err);
80  static void FreezeFn(int val);
81  static void ProgressFn(int progress);
82  static void ButtonFn(CusButton button, int clicks);
83 
87  static void NewProcessedImageFn(const void* newImage, const CusProcessedImageInfo* nfo, int npos, const CusPosInfo* pos);
88  static void NewRawImageFn(const void* newImage, const CusRawImageInfo* nfo, int npos, const CusPosInfo* pos);
89 
94  static void ConnectReturnFn(int udpPort, int swRevMatch);
95 
99  static void RawDataRequestFn(int rawDataSize, const char* extension);
100 
104  static void RawDataWriteFn(int rawDataSize);
105 
109  void AllocateRawData(int size);
110 
114  PlusStatus ReceiveRawData(int dataSize);
115 
119  PlusStatus WritePosesToCsv(const CusProcessedImageInfo* nfo, int npos, const CusPosInfo* pos, int frameNum, double systemTime, double convertedTime);
120 
124  PlusStatus ProcessPositionInfo(int nfo, const CusPosInfo* posInfo);
125 
129  static double NanoSecondsToSeconds(long long int ns);
130  static long long int SecondsToNanoSeconds(double s);
131 
132 protected:
133  vtkPlusClarius* External;
134 
135  vtkPlusDataSource* AccelerometerTool;
136  vtkPlusDataSource* GyroscopeTool;
137  vtkPlusDataSource* MagnetometerTool;
138  vtkPlusDataSource* TiltSensorTool;
139  vtkPlusDataSource* FilteredTiltSensorTool;
140  vtkPlusDataSource* OrientationSensorTool;
141 
142  vtkNew<vtkMatrix4x4> LastAccelerometerToTrackerTransform;
143  vtkNew<vtkMatrix4x4> LastGyroscopeToTrackerTransform;
144  vtkNew<vtkMatrix4x4> LastMagnetometerToTrackerTransform;
145  vtkNew<vtkMatrix4x4> LastTiltSensorToTrackerTransform;
146  vtkNew<vtkMatrix4x4> LastFilteredTiltSensorToTrackerTransform;
147  vtkNew<vtkMatrix4x4> LastOrientationSensorToTrackerTransform;
148 
149  AhrsAlgo* FilteredTiltSensorAhrsAlgo;
150  AhrsAlgo* AhrsAlgo;
151 
157  bool AhrsUseMagnetometer;
158 
160  double AhrsLastUpdateTime;
161  double FilteredTiltSensorAhrsLastUpdateTime;
162 
163  std::ofstream RawImuDataStream;
164  bool IsReceivingRawData;
165 
166  int RawDataSize;
167  void* RawDataPointer;
168 
169  std::string PathToSecKey; // path to security key, required by the clarius api
170 
171  double SystemStartTimestampSeconds;
172  double ClariusStartTimestampSeconds;
173  double ClariusLastTimestamp;
174  int FrameNumber;
175 
176  int UdpPort;
177 
178  bool Initialized;
179 };
180 
181 //-------------------------------------------------------------------------------------------------
182 vtkPlusClarius::vtkInternal::vtkInternal(vtkPlusClarius* ext)
183  : External(ext)
184  , AccelerometerTool(NULL)
185  , GyroscopeTool(NULL)
186  , MagnetometerTool(NULL)
187  , TiltSensorTool(NULL)
188  , FilteredTiltSensorTool(NULL)
189  , OrientationSensorTool(NULL)
190  , AhrsAlgo(new MadgwickAhrsAlgo())
191  , AhrsUseMagnetometer(true)
192  , AhrsLastUpdateTime(-1)
193  , FilteredTiltSensorAhrsLastUpdateTime(-1)
194  , FilteredTiltSensorAhrsAlgo(new MadgwickAhrsAlgo())
195  , RawDataSize(0)
196  , RawDataPointer(NULL)
197  , IsReceivingRawData(false)
198  , PathToSecKey(DEFAULT_PATH_TO_SEC_KEY)
199  , SystemStartTimestampSeconds(0.0)
200  , ClariusStartTimestampSeconds(0.0)
201  , ClariusLastTimestamp(0.0)
202  , FrameNumber(0)
203  , UdpPort(-1)
204  , Initialized(false)
205 {
206 }
207 
208 //----------------------------------------------------------------------------
209 double vtkPlusClarius::vtkInternal::NanoSecondsToSeconds(long long int ns)
210 {
211  return ns / 1000000000.0;
212 }
213 
214 
215 //----------------------------------------------------------------------------
216 long long int vtkPlusClarius::vtkInternal::SecondsToNanoSeconds(double s)
217 {
218  return static_cast<long long int>(1000000000 * s);
219 }
220 
221 //----------------------------------------------------------------------------
222 void vtkPlusClarius::vtkInternal::ConnectReturnFn(int udpPort, int swRevMatch)
223 {
225  if (device == NULL)
226  {
227  LOG_ERROR("Clarius instance is NULL");
228  return;
229  }
230  vtkInternal* self = device->Internal;
231 
232  if (swRevMatch != CUS_SUCCESS)
233  {
234  LOG_ERROR("...Clarius version mismatch");
235  return;
236  }
237 
238  self->UdpPort = udpPort;
239  device->Connected = 1;
240  if (self->UdpPort != -1)
241  {
242  LOG_DEBUG("... Clarius device connected, streaming port: " << self->UdpPort);
243  }
244  else
245  {
246  LOG_ERROR("... Clarius device connected but could not get valid udp port");
247  }
248 }
249 
250 
251 //----------------------------------------------------------------------------
255 void vtkPlusClarius::vtkInternal::ErrorFn(const char* err)
256 {
257  LOG_ERROR("error: " << err);
258 }
259 
260 //----------------------------------------------------------------------------
263 void vtkPlusClarius::vtkInternal::FreezeFn(int val)
264 {
265  if (val)
266  {
267  LOG_INFO("Clarius Frozen");
268  }
269 
270  else
271  {
272  LOG_INFO("Clarius Imaging");
273  }
274 }
275 
276 //----------------------------------------------------------------------------
279 void vtkPlusClarius::vtkInternal::ProgressFn(int progress)
280 {
281  LOG_DEBUG("Download: " << progress << "%");
282 }
283 
284 //----------------------------------------------------------------------------
288 void vtkPlusClarius::vtkInternal::ButtonFn(CusButton btn, int clicks)
289 {
290  LOG_DEBUG("button: " << btn << "clicks: " << clicks << "%");
291 }
292 
293 //----------------------------------------------------------------------------
300 void vtkPlusClarius::vtkInternal::NewProcessedImageFn(const void* newImage, const CusProcessedImageInfo* nfo, int npos, const CusPosInfo* pos)
301 {
302  LOG_TRACE("vtkPlusClarius::NewProcessedImageFn");
304  if (device == NULL)
305  {
306  LOG_ERROR("Clarius instance is NULL");
307  return;
308  }
309  vtkInternal* self = device->Internal;
310 
311 
312  LOG_TRACE("New image (" << newImage << "): " << nfo->width << " x " << nfo->height << " @ " << nfo->bitsPerPixel
313  << "bits. @ " << nfo->micronsPerPixel << " microns per pixel. imu points: " << npos);
314 
315  // Check if still connected
316  if (!device->IsConnected())
317  {
318  LOG_ERROR("Trouble connecting to Clarius Device. IpAddress = " << device->IpAddress
319  << " port = " << device->TcpPort);
320  return;
321  }
322 
323  if (newImage == NULL)
324  {
325  LOG_ERROR("No frame received by the device");
326  return;
327  }
328 
329  // check if there exist active data source;
330  vtkPlusDataSource* bModeSource;
331  std::vector<vtkPlusDataSource*> bModeSources;
333  if (!bModeSources.empty())
334  {
335  bModeSource = bModeSources[0];
336  }
337  else
338  {
339  LOG_WARNING("Processed image was received, however no output B-Mode video source was found.");
340  return;
341  }
342  // Set Image Properties
343  bModeSource->SetInputFrameSize(nfo->width, nfo->height, 1);
344 
345  vtkSmartPointer<vtkImageData> rgbImage = vtkSmartPointer<vtkImageData>::New();
346  rgbImage->SetExtent(0, nfo->width - 1, 0, nfo->height - 1, 0, 0);
347  rgbImage->SetOrigin(0.0, 0.0, 0.0);
348  rgbImage->SetSpacing(1.0, 1.0, 1.0);
349  rgbImage->AllocateScalars(VTK_UNSIGNED_CHAR, 3);
350  PixelCodec::BGRA32ToRGB24(nfo->width, nfo->height, (unsigned char*)newImage, (unsigned char*)rgbImage->GetScalarPointer());
351 
352  vtkSmartPointer<vtkImageData> outputImage = rgbImage;
353  US_IMAGE_TYPE outputUSImageType = US_IMG_RGB_COLOR;
354  bModeSource->SetNumberOfScalarComponents(3);
355  bModeSource->SetPixelType(VTK_UNSIGNED_CHAR);
356  if (bModeSource->GetImageType() == US_IMG_BRIGHTNESS)
357  {
358  bModeSource->SetNumberOfScalarComponents(1);
359 
360  vtkSmartPointer<vtkImageData> grayscaleImage = vtkSmartPointer<vtkImageData>::New();
361  grayscaleImage->SetExtent(0, nfo->width - 1, 0, nfo->height - 1, 0, 1);
362  grayscaleImage->SetOrigin(0.0, 0.0, 0.0);
363  grayscaleImage->SetSpacing(1.0, 1.0, 1.0);
364  grayscaleImage->AllocateScalars(VTK_UNSIGNED_CHAR, 1);
365  outputImage = grayscaleImage;
366 
369  nfo->width,
370  nfo->height,
371  (unsigned char*)rgbImage->GetScalarPointer(),
372  (unsigned char*)grayscaleImage->GetScalarPointer());
373  outputUSImageType = US_IMG_BRIGHTNESS;
374  }
375 
376  // the clarius timestamp is in nanoseconds
377  self->ClariusLastTimestamp = vtkInternal::NanoSecondsToSeconds(nfo->tm);
378  // Get system time (elapsed time since last reboot), return Internal system time in seconds
379  double systemTime = vtkIGSIOAccurateTimer::GetSystemTime();
380  if (device->FrameNumber == 0)
381  {
382  self->SystemStartTimestampSeconds = systemTime;
383  self->ClariusStartTimestampSeconds = self->ClariusLastTimestamp;
384  }
385 
386  // The timestamp that each image is tagged with is
387  // (system_start_time + current_clarius_time - clarius_start_time)
388  double convertedTimestamp = self->SystemStartTimestampSeconds + (self->ClariusLastTimestamp - self->ClariusStartTimestampSeconds);
389  if (npos != 0)
390  {
391  self->WritePosesToCsv(nfo, npos, pos, device->FrameNumber, systemTime, convertedTimestamp);
392  }
393 
394  if (device->WriteImagesToDisk)
395  {
396  // create cvimg to write to disk
397  cv::Mat cvimg = cv::Mat(nfo->width, nfo->height, CV_8UC4);
398  cvimg.data = cvimg.data = (unsigned char*)newImage;
399  if (cv::imwrite("Clarius_Image" + std::to_string(self->ClariusLastTimestamp) + ".bmp", cvimg) == false)
400  {
401  LOG_ERROR("ERROR writing clarius image" + std::to_string(self->ClariusLastTimestamp) + " to disk");
402  }
403  }
404 
405  igsioFieldMapType customField;
406  customField["micronsPerPixel"] = std::make_pair(igsioFrameFieldFlags::FRAMEFIELD_FORCE_SERVER_SEND, std::to_string(nfo->micronsPerPixel));
407  bModeSource->AddItem(
408  outputImage->GetScalarPointer(),
409  bModeSource->GetInputImageOrientation(), // refer to this url: http://perk-software.cs.queensu.ca/plus/doc/nightly/dev/UltrasoundImageOrientation.html for reference;
410  // Set to UN to keep the orientation of the image the same as on tablet
411  bModeSource->GetInputFrameSize(),
412  bModeSource->GetPixelType(),
413  bModeSource->GetNumberOfScalarComponents(),
414  outputUSImageType,
415  0,
416  device->FrameNumber,
417  convertedTimestamp,
418  convertedTimestamp,
419  &customField
420  );
421 
422  self->ProcessPositionInfo(npos, pos);
423 
424  device->FrameNumber++;
425 }
426 
427 //----------------------------------------------------------------------------
434 void vtkPlusClarius::vtkInternal::NewRawImageFn(const void* newImage, const CusRawImageInfo* nfo, int npos, const CusPosInfo* pos)
435 {
436  LOG_TRACE("vtkPlusClarius::NewRawImageFn");
438  if (device == NULL)
439  {
440  LOG_ERROR("Clarius instance is NULL");
441  return;
442  }
443  vtkInternal* self = device->Internal;
444 
445  if (!nfo->rf)
446  {
447  LOG_TRACE("New raw image is envelope, not Rf.");
448  return;
449  }
450 
451  LOG_TRACE("New raw image (" << newImage << "): " << nfo->lines << " lines using " << nfo->samples << " samples, @ " << nfo->bitsPerSample << " bits."
452  << nfo->axialSize << " axial microns per sample, " << nfo->lateralSize << " lateral microns per line.");
453 
454  // Check if still connected
455  if (!device->IsConnected())
456  {
457  LOG_ERROR("Trouble connecting to Clarius Device. IpAddress = " << device->IpAddress
458  << " port = " << device->TcpPort);
459  return;
460  }
461 
462  if (newImage == NULL)
463  {
464  LOG_ERROR("No frame received by the device");
465  return;
466  }
467 
468  vtkPlusDataSource* rfModeSource = nullptr;
469  std::vector<vtkPlusDataSource*> rfModeSources;
471  if (!rfModeSources.empty())
472  {
473  rfModeSource = rfModeSources[0];
474  }
475  else
476  {
477  LOG_WARNING("Raw image was received, however no output RF video source was found.");
478  return;
479  }
480 
481  // Set Image Properties
482  int pixelType = VTK_UNSIGNED_CHAR;
483  int frameBufferBytesPerSample = (nfo->bitsPerSample / 8);
484 
485  switch (frameBufferBytesPerSample)
486  {
487  case VTK_SIZEOF_LONG_LONG:
488  pixelType = VTK_LONG_LONG;
489  break;
490  case VTK_SIZEOF_INT:
491  pixelType = VTK_INT;
492  break;
493  case VTK_SIZEOF_SHORT:
494  pixelType = VTK_SHORT;
495  break;
496  case VTK_SIZEOF_CHAR:
497  default:
498  pixelType = VTK_CHAR;
499  break;
500  }
501  rfModeSource->SetInputFrameSize(nfo->samples, nfo->lines, 1);
502  rfModeSource->SetPixelType(pixelType);
503  rfModeSource->SetImageType(US_IMG_RF_REAL);
504  rfModeSource->SetOutputImageOrientation(US_IMG_ORIENT_MF);
505  rfModeSource->SetNumberOfScalarComponents(1);
506 
507  int frameSizeInBytes = nfo->lines * nfo->samples * frameBufferBytesPerSample;
508 
509  // the clarius timestamp is in nanoseconds
510  self->ClariusLastTimestamp = vtkInternal::NanoSecondsToSeconds(nfo->tm);
511  // Get system time (elapsed time since last reboot), return Internal system time in seconds
512  double systemTime = vtkIGSIOAccurateTimer::GetSystemTime();
513  if (self->FrameNumber == 0)
514  {
515  self->SystemStartTimestampSeconds = systemTime;
516  self->ClariusStartTimestampSeconds = self->ClariusLastTimestamp;
517  }
518 
519  double convertedTimestamp = self->SystemStartTimestampSeconds + (self->ClariusLastTimestamp - self->ClariusStartTimestampSeconds);
520  rfModeSource->AddItem(
521  (void*)newImage, // pointer to char array
522  rfModeSource->GetInputImageOrientation(), // refer to this url: http://perk-software.cs.queensu.ca/plus/doc/nightly/dev/UltrasoundImageOrientation.html for reference;
523  // Set to UN to keep the orientation of the image the same as on tablet
524  rfModeSource->GetInputFrameSize(),
525  pixelType,
526  rfModeSource->GetNumberOfScalarComponents(),
527  rfModeSource->GetImageType(),
528  0,
529  device->FrameNumber,
530  convertedTimestamp,
531  convertedTimestamp);
532 
533  self->ProcessPositionInfo(npos, pos);
534 
535  device->FrameNumber++;
536 }
537 
538 //----------------------------------------------------------------------------
539 PlusStatus vtkPlusClarius::vtkInternal::ProcessPositionInfo(int npos, const CusPosInfo* pos)
540 {
542  if (!device)
543  {
544  LOG_ERROR("Invalid device");
545  return PLUS_FAIL;
546  }
547  vtkInternal* self = device->Internal;
548 
549  // Iterate through all of the incoming position info.
550  for (int i = 0; i < npos; i++)
551  {
552  const CusPosInfo currentPos = pos[i];
553 
554  // the clarius timestamp is in nanoseconds
555  // The timestamp that each pose is tagged with is
556  // (system_start_time + current_clarius_time - clarius_start_time)
557  double poseTimeStampSeconds = vtkInternal::NanoSecondsToSeconds(currentPos.tm);
558  poseTimeStampSeconds = self->SystemStartTimestampSeconds + (poseTimeStampSeconds - self->ClariusStartTimestampSeconds);
559 
560  double acceleration[3] = { currentPos.ax , currentPos.ay , currentPos.az };
561  if (self->AccelerometerTool != NULL)
562  {
563  self->LastAccelerometerToTrackerTransform->Identity();
564  self->LastAccelerometerToTrackerTransform->SetElement(0, 3, acceleration[0]);
565  self->LastAccelerometerToTrackerTransform->SetElement(1, 3, acceleration[1]);
566  self->LastAccelerometerToTrackerTransform->SetElement(2, 3, acceleration[2]);
567  device->ToolTimeStampedUpdateWithoutFiltering(self->AccelerometerTool->GetId(), self->LastAccelerometerToTrackerTransform, TOOL_OK, poseTimeStampSeconds, poseTimeStampSeconds);
568  }
569 
570  double angularRate[3] = { currentPos.gx , currentPos.gy , currentPos.gz };
571  if (self->GyroscopeTool != NULL)
572  {
573  self->LastGyroscopeToTrackerTransform->Identity();
574  self->LastGyroscopeToTrackerTransform->SetElement(0, 3, angularRate[0]);
575  self->LastGyroscopeToTrackerTransform->SetElement(1, 3, angularRate[1]);
576  self->LastGyroscopeToTrackerTransform->SetElement(2, 3, angularRate[2]);
577  device->ToolTimeStampedUpdateWithoutFiltering(self->GyroscopeTool->GetId(), self->LastGyroscopeToTrackerTransform, TOOL_OK, poseTimeStampSeconds, poseTimeStampSeconds);
578  }
579 
580  double magneticField[3] = { currentPos.mx , currentPos.my , currentPos.mz };
581  if (self->MagnetometerTool != NULL)
582  {
583  // magnetometer data is valid
584  self->LastMagnetometerToTrackerTransform->Identity();
585  self->LastMagnetometerToTrackerTransform->SetElement(0, 3, magneticField[0]);
586  self->LastMagnetometerToTrackerTransform->SetElement(1, 3, magneticField[1]);
587  self->LastMagnetometerToTrackerTransform->SetElement(2, 3, magneticField[2]);
588  device->ToolTimeStampedUpdateWithoutFiltering(self->MagnetometerTool->GetId(), self->LastMagnetometerToTrackerTransform, TOOL_OK, poseTimeStampSeconds, poseTimeStampSeconds);
589  }
590 
591  double orientationQuat[4] = { currentPos.qw, currentPos.qx, currentPos.qy, currentPos.qz };
592  if (self->OrientationSensorTool != NULL)
593  {
594  double orientationMatrix[3][3] = {
595  {0.0, 0.0, 0.0},
596  {0.0, 0.0, 0.0},
597  {0.0, 0.0, 0.0}
598  };
599  vtkMath::QuaternionToMatrix3x3(orientationQuat, orientationMatrix);
600  for (int c = 0; c < 3; c++)
601  {
602  for (int r = 0; r < 3; r++)
603  {
604  self->LastOrientationSensorToTrackerTransform->SetElement(r, c, orientationMatrix[r][c]);
605  }
606  }
607  device->ToolTimeStampedUpdateWithoutFiltering(self->OrientationSensorTool->GetId(), self->LastOrientationSensorToTrackerTransform, TOOL_OK, poseTimeStampSeconds, poseTimeStampSeconds);
608  }
609 
610  if (self->TiltSensorTool != NULL)
611  {
612  double downVector_Sensor[4] = { acceleration[0], acceleration[1], acceleration[2], 0 }; // provided by the sensor
613  vtkMath::Normalize(downVector_Sensor);
614 
615  igsioMath::ConstrainRotationToTwoAxes(downVector_Sensor, device->TiltSensorWestAxisIndex, self->LastTiltSensorToTrackerTransform);
616 
617  device->ToolTimeStampedUpdateWithoutFiltering(self->TiltSensorTool->GetId(), self->LastTiltSensorToTrackerTransform, TOOL_OK, poseTimeStampSeconds, poseTimeStampSeconds);
618  }
619 
620  if (self->FilteredTiltSensorTool != NULL)
621  {
622  self->FilteredTiltSensorAhrsAlgo->UpdateIMUWithTimestamp(
623  vtkMath::RadiansFromDegrees(angularRate[0]), vtkMath::RadiansFromDegrees(angularRate[1]), vtkMath::RadiansFromDegrees(angularRate[2]),
624  acceleration[0], acceleration[1], acceleration[2], poseTimeStampSeconds);
625 
626  double rotQuat[4] = { 0.0, 0.0, 0.0, 0.0 };
627  self->AhrsAlgo->GetOrientation(rotQuat[0], rotQuat[1], rotQuat[2], rotQuat[3]);
628 
629  double rotMatrix[3][3] = {
630  {0.0, 0.0, 0.0},
631  {0.0, 0.0, 0.0},
632  {0.0, 0.0, 0.0}
633  };
634  vtkMath::QuaternionToMatrix3x3(rotQuat, rotMatrix);
635 
636  double filteredDownVector_Sensor[4] = { rotMatrix[2][0], rotMatrix[2][1], rotMatrix[2][2], 0.0 };
637  vtkMath::Normalize(filteredDownVector_Sensor);
638 
639  igsioMath::ConstrainRotationToTwoAxes(filteredDownVector_Sensor, device->FilteredTiltSensorWestAxisIndex, self->LastFilteredTiltSensorToTrackerTransform);
640 
641  device->ToolTimeStampedUpdateWithoutFiltering(self->FilteredTiltSensorTool->GetId(), self->LastFilteredTiltSensorToTrackerTransform, TOOL_OK, poseTimeStampSeconds, poseTimeStampSeconds);
642 
643  // write back the results to the FilteredTiltSensor_AHRS algorithm
644  for (int c = 0; c < 3; c++)
645  {
646  for (int r = 0; r < 3; r++)
647  {
648  rotMatrix[r][c] = self->LastFilteredTiltSensorToTrackerTransform->GetElement(r, c);
649  }
650  }
651  double filteredTiltSensorRotQuat[4] = { 0 };
652  vtkMath::Matrix3x3ToQuaternion(rotMatrix, filteredTiltSensorRotQuat);
653  self->FilteredTiltSensorAhrsAlgo->SetOrientation(filteredTiltSensorRotQuat[0], filteredTiltSensorRotQuat[1], filteredTiltSensorRotQuat[2], filteredTiltSensorRotQuat[3]);
654  }
655  }
656 
657  return PLUS_SUCCESS;
658 }
659 
660 //----------------------------------------------------------------------------
661 PlusStatus vtkPlusClarius::vtkInternal::WritePosesToCsv(const CusProcessedImageInfo* nfo, int npos, const CusPosInfo* pos, int frameNum, double systemTime, double convertedTime)
662 {
663  LOG_TRACE("vtkPlusClarius::WritePosesToCsv");
664  if (npos != 0)
665  {
666  LOG_TRACE("timestamp in nanoseconds CusPosInfo" << pos[0].tm);
667  std::string posInfo = "";
668  for (auto i = 0; i < npos; i++)
669  {
670  posInfo += (std::to_string(frameNum) + ",");
671  posInfo += (std::to_string(systemTime) + ",");
672  posInfo += (std::to_string(convertedTime) + ",");
673  posInfo += (std::to_string(nfo->tm) + ",");
674  posInfo += (std::to_string(pos[i].tm) + ",");
675  posInfo += (std::to_string(pos[i].ax) + ",");
676  posInfo += (std::to_string(pos[i].ay) + ",");
677  posInfo += (std::to_string(pos[i].az) + ",");
678  posInfo += (std::to_string(pos[i].gx) + ",");
679  posInfo += (std::to_string(pos[i].gy) + ",");
680  posInfo += (std::to_string(pos[i].gz) + ",");
681  posInfo += (std::to_string(pos[i].mx) + ",");
682  posInfo += (std::to_string(pos[i].my) + ",");
683  posInfo += (std::to_string(pos[i].mz) + ",");
684  posInfo += "\n";
685  }
686 
687  // write the string to file
688  this->RawImuDataStream.open(this->External->ImuOutputFileName, std::ofstream::app);
689  if (this->RawImuDataStream.is_open() == false)
690  {
691  LOG_ERROR("Error opening file for raw imu data");
692  return PLUS_FAIL;
693  }
694 
695  this->RawImuDataStream << posInfo;
696  this->RawImuDataStream.close();
697  return PLUS_SUCCESS;
698  }
699 
700  return PLUS_SUCCESS;
701 }
702 
703 
704 //----------------------------------------------------------------------------
705 void vtkPlusClarius::vtkInternal::RawDataRequestFn(int rawDataSize, const char* extension)
706 {
708  if (!device)
709  {
710  LOG_ERROR("Invalid device");
711  return;
712  }
713  vtkInternal* self = device->Internal;
714 
715  if (rawDataSize < 0)
716  {
717  self->IsReceivingRawData = false;
718  LOG_ERROR("Error requesting raw data!");
719  return;
720  }
721 
722  if (rawDataSize == 0)
723  {
724  self->IsReceivingRawData = false;
725  LOG_TRACE("No data to read!");
726  return;
727  }
728 
729  self->ReceiveRawData(rawDataSize);
730 }
731 
732 //----------------------------------------------------------------------------
733 PlusStatus vtkPlusClarius::vtkInternal::ReceiveRawData(int dataSize)
734 {
735  LOG_INFO("Receiving " << dataSize << " bytes of raw data");
736 
737  CusReturnFn returnFunction = (CusReturnFn)(&vtkInternal::RawDataWriteFn);
738  this->AllocateRawData(dataSize);
739  cusCastReadRawData(&this->RawDataPointer, returnFunction);
740  return PLUS_SUCCESS;
741 }
742 
743 //----------------------------------------------------------------------------
744 void vtkPlusClarius::vtkInternal::RawDataWriteFn(int retCode)
745 {
747  if (!device)
748  {
749  LOG_ERROR("Invalid device");
750  return;
751  }
752  vtkInternal* self = device->Internal;
753 
754  self->IsReceivingRawData = false;
755 
756  if (retCode < 0)
757  {
758  LOG_ERROR("Could not read raw data!");
759  return;
760  }
761 
762  LOG_INFO("Raw data received successfully!");
763 
764  std::string filename = device->GetRawDataOutputFilename();
765  if (filename.empty())
766  {
767  filename = vtkIGSIOAccurateTimer::GetDateAndTimeString() + "_ClariusData.tar";
768  }
769 
770  if (device->CompressRawData && vtksys::SystemTools::GetFilenameLastExtension(filename) != ".gz")
771  {
772  filename = filename + ".gz";
773  }
774 
775  if (!vtksys::SystemTools::FileIsFullPath(filename.c_str()))
776  {
777 
778  filename = vtkPlusConfig::GetInstance()->GetOutputDirectory() + "/" + filename;
779  }
780 
781  if (device->CompressRawData)
782  {
783  gzFile file = gzopen(filename.c_str(), "wb");
784  gzwrite(file, self->RawDataPointer, self->RawDataSize);
785  gzclose(file);
786  }
787  else
788  {
789  FILE* file = fopen(filename.c_str(), "wb");
790  fwrite((char*)self->RawDataPointer, 1, self->RawDataSize, file);
791  fclose(file);
792  }
793 
794  LOG_INFO("Raw data saved as: " << filename);
795 }
796 
797 //----------------------------------------------------------------------------
798 void vtkPlusClarius::vtkInternal::AllocateRawData(int dataSize)
799 {
800  if (this->RawDataPointer)
801  {
802  delete[] this->RawDataPointer;
803  this->RawDataPointer = nullptr;
804  }
805 
806  if (dataSize > 0)
807  {
808  this->RawDataPointer = new char[dataSize];
809  }
810  this->RawDataSize = dataSize;
811 }
812 
813 
814 //----------------------------------------------------------------------------
816 
817 //----------------------------------------------------------------------------
819 {
820  if (vtkPlusClarius::Instance == NULL)
821  {
823  }
825 }
826 
827 //----------------------------------------------------------------------------
829  : Internal(new vtkInternal(this))
830  , TcpPort(-1)
831  , IpAddress("192.168.1.1")
834  , ImuEnabled(false)
835  , ImuOutputFileName("ClariusImuData.csv")
836  , WriteImagesToDisk(false)
837  , CompressRawData(false)
838  , AhrsAlgorithmGain{ 1.5, 0.0 }
842 {
843  LOG_TRACE("vtkPlusClarius: Constructor");
844  this->StartThreadForInternalUpdates = true;
846 }
847 
848 //----------------------------------------------------------------------------
850 {
851  this->Internal->AllocateRawData(-1);
852 
853  if (this->Recording)
854  {
855  this->StopRecording();
856  }
857 
858  if (this->Connected)
859  {
860  cusCastDisconnect(BLOCKINGCALL);
861  }
862 
863  if (this->Internal->Initialized)
864  {
865  int destroyed = cusCastDestroy();
866  if (destroyed != 0)
867  {
868  LOG_ERROR("Error destoying the listener");
869  }
870  }
871 
872  if (this->Internal)
873  {
874  delete this->Internal;
875  }
876 
877  this->Instance = NULL;
878 }
879 
880 //----------------------------------------------------------------------------
882 {
883  LOG_TRACE("vtkPlusClarius: GetInstance()");
884  if (Instance != NULL)
885  {
886  return Instance;
887  }
888 
889  else
890  {
891  LOG_ERROR("Instance is null, creating new instance");
892  Instance = new vtkPlusClarius();
893  return Instance;
894  }
895 }
896 
897 //----------------------------------------------------------------------------
898 void vtkPlusClarius::PrintSelf(ostream& os, vtkIndent indent)
899 {
900  this->Superclass::PrintSelf(os, indent);
901  os << indent << "ipAddress" << this->IpAddress << std::endl;
902  os << indent << "tcpPort" << this->TcpPort << std::endl;
903  os << indent << "UdpPort" << this->Internal->UdpPort << std::endl;
904  os << indent << "FrameNumber" << this->FrameNumber << std::endl;
905  os << indent << "FrameWidth" << this->FrameWidth << std::endl;
906  os << indent << "FrameHeight" << this->FrameHeight << std::endl;
907 }
908 
909 //----------------------------------------------------------------------------
910 PlusStatus vtkPlusClarius::ReadConfiguration(vtkXMLDataElement* rootConfigElement)
911 {
912  LOG_TRACE("vtkPlusClarius::ReadConfiguration");
913  XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_READING(deviceConfig, rootConfigElement);
914 
915  XML_READ_STRING_ATTRIBUTE_REQUIRED(IpAddress, deviceConfig);
916  XML_READ_SCALAR_ATTRIBUTE_REQUIRED(int, TcpPort, deviceConfig);
917  // if not specified, the default value for FrameWidth is 640 and FrameHeight is 480 according to clarius;
918  XML_READ_SCALAR_ATTRIBUTE_OPTIONAL(int, FrameWidth, deviceConfig);
919  XML_READ_SCALAR_ATTRIBUTE_OPTIONAL(int, FrameHeight, deviceConfig);
920  XML_READ_BOOL_ATTRIBUTE_OPTIONAL(ImuEnabled, deviceConfig);
921  XML_READ_BOOL_ATTRIBUTE_OPTIONAL(WriteImagesToDisk, deviceConfig);
922  if (this->ImuEnabled)
923  {
924  XML_READ_STRING_ATTRIBUTE_REQUIRED(ImuOutputFileName, deviceConfig);
925  }
926 
927  int tiltSensorWestAxisIndex = 0;
928  if (deviceConfig->GetScalarAttribute("TiltSensorWestAxisIndex", tiltSensorWestAxisIndex))
929  {
930  if (tiltSensorWestAxisIndex < 0 || tiltSensorWestAxisIndex > 2)
931  {
932  LOG_ERROR("TiltSensorWestAxisIndex is invalid. Specified value: " << tiltSensorWestAxisIndex << ". Valid values: 0, 1, 2. Keep using the default value: "
933  << this->TiltSensorWestAxisIndex);
934  }
935  else
936  {
937  this->TiltSensorWestAxisIndex = tiltSensorWestAxisIndex;
938  }
939  }
940 
942  if (deviceConfig->GetScalarAttribute("FilteredTiltSensorWestAxisIndex", FilteredTiltSensorWestAxisIndex))
943  {
944  if (FilteredTiltSensorWestAxisIndex < 0 || FilteredTiltSensorWestAxisIndex > 2)
945  {
946  LOG_ERROR("FilteredTiltSensorWestAxisIndex is invalid. Specified value: " << FilteredTiltSensorWestAxisIndex << ". Valid values: 0, 1, 2. Keep using the default value: "
947  << this->FilteredTiltSensorWestAxisIndex);
948  }
949  else
950  {
951  this->FilteredTiltSensorWestAxisIndex = FilteredTiltSensorWestAxisIndex;
952  }
953  }
954 
955  XML_READ_VECTOR_ATTRIBUTE_OPTIONAL(double, 2, AhrsAlgorithmGain, deviceConfig);
956  XML_READ_VECTOR_ATTRIBUTE_OPTIONAL(double, 2, FilteredTiltSensorAhrsAlgorithmGain, deviceConfig);
957 
958  const char* ahrsAlgoName = deviceConfig->GetAttribute("AhrsAlgorithm");
959  if (ahrsAlgoName != NULL)
960  {
961  if (STRCASECMP("MADGWICK_MARG", ahrsAlgoName) == 0 || STRCASECMP("MADGWICK_IMU", ahrsAlgoName) == 0)
962  {
963  if (dynamic_cast<MadgwickAhrsAlgo*>(this->Internal->AhrsAlgo) == 0)
964  {
965  // not the requested type
966  // delete the old algo and create a new one with the correct type
967  delete this->Internal->AhrsAlgo;
968  this->Internal->AhrsAlgo = new MadgwickAhrsAlgo;
969  }
970  if (STRCASECMP("MADGWICK_MARG", ahrsAlgoName) == 0)
971  {
972  this->Internal->AhrsUseMagnetometer = true;
973  }
974  else
975  {
976  this->Internal->AhrsUseMagnetometer = false;
977  }
978  }
979  else if (STRCASECMP("MAHONY_MARG", ahrsAlgoName) == 0 || STRCASECMP("MAHONY_IMU", ahrsAlgoName) == 0)
980  {
981  if (dynamic_cast<MahonyAhrsAlgo*>(this->Internal->AhrsAlgo) == 0)
982  {
983  // not the requested type
984  // delete the old algo and create a new one with the correct type
985  delete this->Internal->AhrsAlgo;
986  this->Internal->AhrsAlgo = new MahonyAhrsAlgo;
987  }
988  if (STRCASECMP("MAHONY_MARG", ahrsAlgoName) == 0)
989  {
990  this->Internal->AhrsUseMagnetometer = true;
991  }
992  else
993  {
994  this->Internal->AhrsUseMagnetometer = false;
995  }
996  }
997  else
998  {
999  LOG_ERROR("Unable to recognize AHRS algorithm type: " << ahrsAlgoName << ". Supported types: MADGWICK_MARG, MAHONY_MARG, MADGWICK_IMU, MAHONY_IMU");
1000  return PLUS_FAIL;
1001  }
1002  }
1003  const char* FilteredTiltSensorAhrsAlgoName = deviceConfig->GetAttribute("FilteredTiltSensorAhrsAlgorithm");
1004  if (FilteredTiltSensorAhrsAlgoName != NULL)
1005  {
1006  if (STRCASECMP("MADGWICK_IMU", FilteredTiltSensorAhrsAlgoName) == 0)
1007  {
1008  if (dynamic_cast<MadgwickAhrsAlgo*>(this->Internal->FilteredTiltSensorAhrsAlgo) == 0)
1009  {
1010  // not the requested type
1011  // delete the old algo and create a new one with the correct type
1012  delete this->Internal->FilteredTiltSensorAhrsAlgo;
1013  this->Internal->FilteredTiltSensorAhrsAlgo = new MadgwickAhrsAlgo;
1014  }
1015  }
1016  else if (STRCASECMP("MAHONY_IMU", FilteredTiltSensorAhrsAlgoName) == 0)
1017  {
1018  if (dynamic_cast<MahonyAhrsAlgo*>(this->Internal->FilteredTiltSensorAhrsAlgo) == 0)
1019  {
1020  // not the requested type
1021  // delete the old algo and create a new one with the correct type
1022  delete this->Internal->FilteredTiltSensorAhrsAlgo;
1023  this->Internal->FilteredTiltSensorAhrsAlgo = new MahonyAhrsAlgo;
1024  }
1025  }
1026  else
1027  {
1028  LOG_ERROR("Unable to recognize AHRS algorithm type for Filtered Tilt: " << FilteredTiltSensorAhrsAlgoName << ". Supported types: MADGWICK_IMU, MAHONY_IMU");
1029  return PLUS_FAIL;
1030  }
1031  }
1032 
1033  return PLUS_SUCCESS;
1034 }
1035 
1036 //----------------------------------------------------------------------------
1037 PlusStatus vtkPlusClarius::WriteConfiguration(vtkXMLDataElement* rootConfigElement)
1038 {
1039  LOG_TRACE("vtkPlusClarius::WriteConfiguration");
1040  XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_WRITING(deviceConfig, rootConfigElement);
1041 
1042  XML_WRITE_STRING_ATTRIBUTE(IpAddress, deviceConfig);
1043  deviceConfig->SetIntAttribute("TcpPort", this->TcpPort);
1044  deviceConfig->SetIntAttribute("FrameWidth", this->FrameWidth);
1045  deviceConfig->SetIntAttribute("FrameHeight", this->FrameHeight);
1046  XML_WRITE_BOOL_ATTRIBUTE(ImuEnabled, deviceConfig);
1047  if (this->ImuEnabled)
1048  {
1049  XML_WRITE_STRING_ATTRIBUTE(ImuOutputFileName, deviceConfig);
1050  }
1051  return PLUS_SUCCESS;
1052 }
1053 
1054 //----------------------------------------------------------------------------
1056 {
1057  LOG_TRACE("vtkPlusClarius::NotifyConfigured");
1058 
1060  if (device->OutputChannels.size() > 1)
1061  {
1062  LOG_WARNING("vtkPlusClarius is expecting one output channel and there are " <<
1063  this->OutputChannels.size() << " channels. First output channel will be used.");
1064  }
1065 
1066  if (device->OutputChannels.empty())
1067  {
1068  LOG_ERROR("No output channels defined for vtkPlusClarius. Cannot proceed.");
1069  this->CorrectlyConfigured = false;
1070  return PLUS_FAIL;
1071  }
1072 
1073  std::vector<vtkPlusDataSource*> sources;
1074  sources = device->GetVideoSources();
1075  if (sources.size() > 1)
1076  {
1077  LOG_WARNING("More than one output video source found. First will be used");
1078  }
1079  if (sources.size() == 0)
1080  {
1081  LOG_ERROR("Video source required in configuration. Cannot proceed.");
1082  return PLUS_FAIL;
1083  }
1084 
1085  // Check if output channel has data source
1086  vtkPlusDataSource* aSource(NULL);
1087  if (device->OutputChannels[0]->GetVideoSource(aSource) != PLUS_SUCCESS)
1088  {
1089  LOG_ERROR("Unable to retrieve the video source in the vtkPlusClarius device.");
1090  return PLUS_FAIL;
1091  }
1092 
1093  return PLUS_SUCCESS;
1094 }
1095 
1096 //----------------------------------------------------------------------------
1098 {
1099  LOG_TRACE("vtkPlusClarius: Probe");
1100  if (this->Internal->UdpPort == -1)
1101  {
1102  return PLUS_FAIL;
1103  }
1104 
1105  return PLUS_SUCCESS;
1106 };
1107 
1108 //----------------------------------------------------------------------------
1109 std::string vtkPlusClarius::vtkPlusClarius::GetSdkVersion()
1110 {
1111  std::ostringstream version;
1112  version << "Sdk version not available" << "\n";
1113  return version.str();
1114 }
1115 
1116 //----------------------------------------------------------------------------
1118 {
1119  LOG_DEBUG("vtkPlusClarius: InternalConnect");
1120 
1121  if (this->ImuEnabled)
1122  {
1123  this->Internal->RawImuDataStream.open(this->ImuOutputFileName, std::ofstream::app);
1124  this->Internal->RawImuDataStream << "FrameNum,SystemTimestamp,ConvertedTimestamp,ImageTimestamp,ImuTimeStamp,ax,ay,az,gx,gy,gz,mx,my,mz,\n";
1125 
1126  this->Internal->RawImuDataStream.close();
1127  }
1128 
1129  this->Internal->AccelerometerTool = NULL;
1130  this->GetToolByPortName("Accelerometer", this->Internal->AccelerometerTool);
1131 
1132  this->Internal->GyroscopeTool = NULL;
1133  this->GetToolByPortName("Gyroscope", this->Internal->GyroscopeTool);
1134 
1135  this->Internal->MagnetometerTool = NULL;
1136  this->GetToolByPortName("Magnetometer", this->Internal->MagnetometerTool);
1137 
1138  this->Internal->TiltSensorTool = NULL;
1139  this->GetToolByPortName("TiltSensor", this->Internal->TiltSensorTool);
1140 
1141  this->Internal->FilteredTiltSensorTool = NULL;
1142  this->GetToolByPortName("FilteredTiltSensor", this->Internal->FilteredTiltSensorTool);
1143 
1144  this->Internal->OrientationSensorTool = NULL;
1145  this->GetToolByPortName("OrientationSensor", this->Internal->OrientationSensorTool);
1146 
1147  // Initialize Clarius Listener Before Connecting
1148  if (!this->IsConnected())
1149  {
1150  // Callbacks
1151  CusNewProcessedImageFn processedImageCallbackPtr = static_cast<CusNewProcessedImageFn>(&vtkInternal::NewProcessedImageFn);
1152  CusNewRawImageFn rawDataCallBackPtr = static_cast<CusNewRawImageFn>(&vtkInternal::NewRawImageFn);
1153  CusFreezeFn freezeCallBackFnPtr = static_cast<CusFreezeFn>(&vtkInternal::FreezeFn);
1154  CusButtonFn buttonCallBackFnPtr = static_cast<CusButtonFn>(&vtkInternal::ButtonFn);
1155  CusProgressFn progressCallBackFnPtr = static_cast<CusProgressFn>(&vtkInternal::ProgressFn);
1156  CusErrorFn errorCallBackFnPtr = static_cast<CusErrorFn>(&vtkInternal::ErrorFn);
1157 
1158  // No B-mode data sources. Disable B mode callback.
1159  std::vector<vtkPlusDataSource*> bModeSources;
1161  if (bModeSources.empty())
1162  {
1163  processedImageCallbackPtr = nullptr;
1164  }
1165 
1166  // No RF-mode data sources. Disable RF mode callback.
1167  std::vector<vtkPlusDataSource*> rfModeSources;
1169  if (rfModeSources.empty())
1170  {
1171  rawDataCallBackPtr = nullptr;
1172  }
1173 
1174  try
1175  {
1176  if (cusCastInit(0, NULL, this->Internal->PathToSecKey.c_str(),
1177  processedImageCallbackPtr,
1178  rawDataCallBackPtr,
1179  nullptr, // ClariusSpectralImageInfo
1180  nullptr, // CusNewImuDataFn
1181  freezeCallBackFnPtr,
1182  buttonCallBackFnPtr,
1183  progressCallBackFnPtr,
1184  errorCallBackFnPtr,
1185  this->FrameWidth,
1186  this->FrameHeight) < 0)
1187  {
1188  return PLUS_FAIL;
1189  }
1190  this->Internal->Initialized = true;
1191  }
1192  catch (const std::runtime_error& re)
1193  {
1194  LOG_ERROR("Runtime error: " << re.what());
1195  return PLUS_FAIL;
1196  }
1197  catch (const std::exception& ex)
1198  {
1199  LOG_ERROR("Error occurred: " << ex.what());
1200  return PLUS_FAIL;
1201  }
1202  catch (...)
1203  {
1204  LOG_ERROR("Unknown failure occured");
1205  return PLUS_FAIL;
1206  }
1207 
1208  // Attempt to connect;
1209  const char* ip = this->IpAddress.c_str();
1210  try
1211  {
1212  CusConnectFn returnFunction = (CusConnectFn)(&vtkInternal::ConnectReturnFn);
1213  cusCastConnect(ip, this->TcpPort, "research", returnFunction);
1214 
1215  // Wait for the udp port to be determined.
1216  int maxConnectionAttempts = 20;
1217  for (int i = 0; i < 20; ++i)
1218  {
1219  // Attempt to get the udp port.
1220  const double connectionDelaySeconds = 1.0;
1221  vtkIGSIOAccurateTimer::DelayWithEventProcessing(connectionDelaySeconds);
1222 
1223  if (this->Internal->UdpPort != -1)
1224  {
1225  vtkIGSIOAccurateTimer::DelayWithEventProcessing(connectionDelaySeconds);
1226  break;
1227  }
1228  }
1229  }
1230  catch (const std::runtime_error& re)
1231  {
1232  LOG_ERROR("Runtime error: " << re.what());
1233  return PLUS_FAIL;
1234  }
1235  catch (const std::exception& ex)
1236  {
1237  LOG_ERROR("Error occurred: " << ex.what());
1238  return PLUS_FAIL;
1239  }
1240  catch (...)
1241  {
1242  LOG_ERROR("Unknown failure occured");
1243  return PLUS_FAIL;
1244  }
1245 
1246  if (this->Internal->UdpPort != -1)
1247  {
1248  if (cusCastSetOutputSize(this->FrameWidth, this->FrameHeight) < 0)
1249  {
1250  LOG_DEBUG("Clarius Output size can not be set, falling back to default 640*480");
1253  }
1254  }
1255  else
1256  {
1257  this->Connected = 1;
1258  this->InternalDisconnect();
1259  this->Connected = 0;
1260  return PLUS_FAIL;
1261  }
1262 
1263  if (!this->IsConnected())
1264  {
1265  LOG_ERROR("Could not connect to scanner at ip: " << ip << " port number: " << this->TcpPort);
1266  return PLUS_FAIL;
1267  }
1268  }
1269  else
1270  {
1271  LOG_DEBUG("Scanner already connected to IP address=" << this->IpAddress
1272  << " TCP Port Number =" << this->TcpPort << "Streaming Image at UDP Port=" << this->Internal->UdpPort);
1273  this->Connected = 1;
1274  return PLUS_SUCCESS;
1275  }
1276  return PLUS_SUCCESS;
1277 }
1278 
1279 //----------------------------------------------------------------------------
1281 {
1282  LOG_DEBUG("vtkPlusClarius: InternalDisconnect");
1284  if (device->GetConnected())
1285  {
1286  if (cusCastDisconnect(nullptr) < 0)
1287  {
1288  LOG_ERROR("could not disconnect from scanner");
1289  return PLUS_FAIL;
1290  }
1291  else
1292  {
1293  device->Connected = 0;
1294  LOG_DEBUG("Clarius device is now disconnected");
1295  return PLUS_SUCCESS;
1296  }
1297  }
1298  else
1299  {
1300  LOG_DEBUG("...Clarius device already disconnected");
1301  return PLUS_SUCCESS;
1302  }
1303 };
1304 
1305 //----------------------------------------------------------------------------
1307 {
1308  if (lastNSeconds <= 0)
1309  {
1310  return this->RequestRawData(0, 0);
1311  }
1312 
1313  LOG_INFO("Requesting raw data for last " << lastNSeconds << " seconds");
1314  long long int endTimestamp = vtkInternal::SecondsToNanoSeconds(this->Internal->ClariusLastTimestamp);
1315  long long int lastNNanoSeconds = vtkInternal::SecondsToNanoSeconds(lastNSeconds);
1316  long long int startTimestamp = std::max((long long)0, endTimestamp - lastNNanoSeconds);
1317  return this->RequestRawData(startTimestamp, endTimestamp);
1318 }
1319 
1320 //----------------------------------------------------------------------------
1321 PlusStatus vtkPlusClarius::RequestRawData(long long int startTimestamp, long long int endTimestamp)
1322 {
1323  if (this->Internal->IsReceivingRawData)
1324  {
1325  LOG_ERROR("Receive data already in progress!");
1326  return PLUS_FAIL;
1327  }
1328 
1329  if (startTimestamp < 0 || endTimestamp < 0)
1330  {
1331  LOG_ERROR("Start and end timestamps must be > 0 nanoseconds");
1332  }
1333 
1334  if (startTimestamp == 0 && endTimestamp == 0)
1335  {
1336  LOG_INFO("Requesting all available raw data");
1337  }
1338  else
1339  {
1340  LOG_INFO("Requesting raw data between " << startTimestamp << "ns and " << endTimestamp << "ns");
1341  }
1342 
1343  this->Internal->IsReceivingRawData = true;
1344 
1345  CusRawRequestFn returnFunction = (CusRawRequestFn)(&vtkInternal::RawDataRequestFn);
1346  cusCastRequestRawData(startTimestamp, endTimestamp, 0, returnFunction);
1347  return PLUS_SUCCESS;
1348 }
virtual void PrintSelf(ostream &os, vtkIndent indent) VTK_OVERRIDE
static const std::string RFMODE_PORT_NAME
Definition: vtkPlusDevice.h:68
PlusStatus RequestRawData(long long startTimestampNanoSeconds, long long endTimestampNanoSeconds)
virtual PlusStatus SetOutputImageOrientation(US_IMAGE_ORIENTATION imageOrientation)
static void BGRA32ToRGB24(int width, int height, unsigned char *s, unsigned char *d)
Definition: PixelCodec.h:260
US_IMAGE_TYPE GetImageType()
virtual PlusStatus InternalDisconnect()
#define XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_WRITING(deviceConfig, rootConfigElement)
virtual PlusStatus ToolTimeStampedUpdateWithoutFiltering(const std::string &aToolSourceId, vtkMatrix4x4 *matrix, ToolStatus status, double unfilteredtimestamp, double filteredtimestamp, const igsioFieldMapType *customFields=NULL)
igsioStatus PlusStatus
Definition: PlusCommon.h:40
static vtkPlusClarius * New()
static const std::string BMODE_PORT_NAME
Definition: vtkPlusDevice.h:67
PlusStatus SetInputFrameSize(unsigned int x, unsigned int y, unsigned int z)
std::vector< vtkPlusDataSource * > GetVideoSources() const
virtual PlusStatus AddItem(vtkImageData *frame, US_IMAGE_ORIENTATION usImageOrientation, US_IMAGE_TYPE imageType, long frameNumber, double unfilteredTimestamp=UNDEFINED_TIMESTAMP, double filteredTimestamp=UNDEFINED_TIMESTAMP, const igsioFieldMapType *customFields=NULL)
bool RequirePortNameInDeviceSetConfiguration
#define DEFAULT_PATH_TO_SEC_KEY
std::string to_string(ClariusAvailability avail)
virtual unsigned int GetNumberOfScalarComponents()
PlusStatus SetImageType(US_IMAGE_TYPE imageType)
for i
double(* angularRate)[3]
Definition: phidget22.h:3659
#define PLUS_FAIL
Definition: PlusCommon.h:43
PlusStatus GetVideoSourcesByPortName(const char *aPortName, std::vector< vtkPlusDataSource * > &sources)
PlusStatus SetPixelType(igsioCommon::VTKScalarPixelType pixelType)
static vtkPlusConfig * GetInstance()
virtual PlusStatus ReadConfiguration(vtkXMLDataElement *config)
virtual igsioCommon::VTKScalarPixelType GetPixelType()
virtual PlusStatus WriteConfiguration(vtkXMLDataElement *config)
#define DEFAULT_FRAME_HEIGHT
double AhrsAlgorithmGain[2]
unsigned long FrameNumber
vtkInternal * Internal
#define PLUS_SUCCESS
Definition: PlusCommon.h:44
PlusStatus GetToolByPortName(const char *aPortName, vtkPlusDataSource *&aSource)
virtual PlusStatus Probe()
PlusStatus RequestLastNSecondsRawData(double lastNSeconds)
std::string GetOutputDirectory()
static vtkPlusClarius * GetInstance()
std::string ImuOutputFileName
double magneticField
Definition: phidget22.h:3700
virtual PlusStatus StopRecording()
#define XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_READING(deviceConfig, rootConfigElement)
unsigned int TcpPort
virtual PlusStatus InternalConnect()
#define DEFAULT_FRAME_WIDTH
virtual US_IMAGE_ORIENTATION GetInputImageOrientation()
virtual bool IsConnected() const
std::string IpAddress
bool StartThreadForInternalUpdates
static PlusStatus ConvertToGray(int inputCompression, int width, int height, unsigned char *s, unsigned char *d)
Definition: PixelCodec.h:146
virtual int GetConnected() const
ChannelContainer OutputChannels
PlusStatus SetNumberOfScalarComponents(unsigned int numberOfScalarComponents)
#define BLOCKINGCALL
double acceleration
Definition: phidget22.h:3290
Interface to the Clarius ultrasound scans This class talks with a Clarius Scanner over the Clarius AP...
static vtkPlusClarius * Instance
FrameSizeType GetInputFrameSize() const
double FilteredTiltSensorAhrsAlgorithmGain[2]
void PrintSelf(ostream &os, vtkIndent indent) VTK_OVERRIDE
virtual PlusStatus NotifyConfigured()
int FilteredTiltSensorWestAxisIndex
bool CorrectlyConfigured
Interface to a 3D positioning tool, video source, or generalized data stream.