PlusLib  2.9.0
Software library for tracked ultrasound image acquisition, calibration, and processing.
vtkPlusOpticalMarkerTracker.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 #define MM_PER_M 1000
8 
9 // Local includes
10 #include "PixelCodec.h"
11 #include "PlusConfigure.h"
12 #include "vtkPlusDataSource.h"
14 
15 // VTK includes
16 #include <vtkExtractVOI.h>
17 #include <vtkImageData.h>
18 #include <vtkImageImport.h>
19 #include <vtkMath.h>
20 #include <vtkMatrix4x4.h>
21 #include <vtkObjectFactory.h>
22 
23 // OS includes
24 #include <fstream>
25 #include <iostream>
26 #include <set>
27 
28 // aruco includes
29 #include <markerdetector.h>
30 #include <cameraparameters.h>
31 #include <dictionary.h>
32 #include <posetracker.h>
33 
34 // OpenCV includes
35 #include <opencv2/highgui.hpp>
36 #include <opencv2/calib3d.hpp>
37 #include <opencv2/core.hpp>
38 
39 //----------------------------------------------------------------------------
40 
42 
43 namespace
44 {
45  class TrackedTool
46  {
47  public:
48  enum TOOL_MARKER_TYPE
49  {
50  SINGLE_MARKER,
51  MARKER_MAP
52  };
53 
54  TrackedTool(int markerId, float markerSizeMm, const std::string& toolSourceId)
55  : ToolMarkerType(SINGLE_MARKER)
56  , MarkerId(markerId)
57  , MarkerSizeMm(markerSizeMm)
58  , ToolSourceId(toolSourceId)
59  {
60  }
61  TrackedTool(const std::string& markerMapFile, const std::string& toolSourceId)
62  : ToolMarkerType(MARKER_MAP)
63  , MarkerMapFile(markerMapFile)
64  , ToolSourceId(toolSourceId)
65  {
66  }
67 
68  int MarkerId;
69  TOOL_MARKER_TYPE ToolMarkerType;
70  float MarkerSizeMm;
71 
72  std::string MarkerMapFile;
73  std::string ToolSourceId;
74  std::string ToolName;
75  aruco::MarkerPoseTracker MarkerPoseTracker;
76  vtkSmartPointer<vtkMatrix4x4> transformMatrix = vtkSmartPointer<vtkMatrix4x4>::New();
77  };
78 }
79 //----------------------------------------------------------------------------
80 class vtkPlusOpticalMarkerTracker::vtkInternal
81 {
82 public:
84 
85  vtkInternal(vtkPlusOpticalMarkerTracker* external)
86  : External(external)
87  , MarkerDetector(std::make_shared<aruco::MarkerDetector>())
88  , CameraParameters(std::make_shared<aruco::CameraParameters>())
89  , MarkerFound(false)
90  {
91  }
92 
93  virtual ~vtkInternal()
94  {
95  MarkerDetector = nullptr;
96  CameraParameters = nullptr;
97  }
98 
99  PlusStatus BuildTransformMatrix(vtkSmartPointer<vtkMatrix4x4> transformMatrix, const cv::Mat& Rvec, const cv::Mat& Tvec);
100 
101  std::string CameraCalibrationFile;
102  TRACKING_METHOD TrackingMethod;
103  std::string MarkerDictionary;
104  std::vector<TrackedTool> Tools;
105  double LastProcessedInputDataTimestamp;
106  bool MarkerFound;
107 
109  std::shared_ptr<aruco::MarkerDetector> MarkerDetector;
110  std::shared_ptr<aruco::CameraParameters> CameraParameters;
111  std::vector<aruco::Marker> Markers;
112 };
113 
114 //----------------------------------------------------------------------------
116  : vtkPlusDevice()
117  , Internal(new vtkInternal(this))
118 {
119  this->FrameNumber = 0;
120  this->StartThreadForInternalUpdates = true;
121 }
122 
123 //----------------------------------------------------------------------------
125 {
126  delete Internal;
127  Internal = nullptr;
128 }
129 
130 //----------------------------------------------------------------------------
131 void vtkPlusOpticalMarkerTracker::PrintSelf(ostream& os, vtkIndent indent)
132 {
133  this->Superclass::PrintSelf(os, indent);
134 }
135 
136 
137 //----------------------------------------------------------------------------
138 PlusStatus vtkPlusOpticalMarkerTracker::ReadConfiguration(vtkXMLDataElement* rootConfigElement)
139 {
140  // TODO: Improve error checking
141  XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_READING(deviceConfig, rootConfigElement);
142 
143  XML_READ_STRING_ATTRIBUTE_NONMEMBER_REQUIRED(CameraCalibrationFile, this->Internal->CameraCalibrationFile, deviceConfig);
144  XML_READ_ENUM2_ATTRIBUTE_NONMEMBER_OPTIONAL(TrackingMethod, this->Internal->TrackingMethod, deviceConfig, "OPTICAL", TRACKING_OPTICAL, "OPTICAL_AND_DEPTH", TRACKING_OPTICAL_AND_DEPTH);
145  XML_READ_STRING_ATTRIBUTE_NONMEMBER_REQUIRED(MarkerDictionary, this->Internal->MarkerDictionary, deviceConfig);
146 
147  XML_FIND_NESTED_ELEMENT_REQUIRED(dataSourcesElement, deviceConfig, "DataSources");
148  for (int nestedElementIndex = 0; nestedElementIndex < dataSourcesElement->GetNumberOfNestedElements(); nestedElementIndex++)
149  {
150  vtkXMLDataElement* toolDataElement = dataSourcesElement->GetNestedElement(nestedElementIndex);
151  if (STRCASECMP(toolDataElement->GetName(), "DataSource") != 0)
152  {
153  // if this is not a data source element, skip it
154  continue;
155  }
156  if (toolDataElement->GetAttribute("Type") != NULL && STRCASECMP(toolDataElement->GetAttribute("Type"), "Tool") != 0)
157  {
158  // if this is not a Tool element, skip it
159  continue;
160  }
161 
162  const char* toolId = toolDataElement->GetAttribute("Id");
163  if (toolId == NULL)
164  {
165  // tool doesn't have ID needed to generate transform
166  LOG_ERROR("Failed to initialize OpticalMarkerTracking tool: DataSource Id is missing");
167  continue;
168  }
169 
170  igsioTransformName toolTransformName(toolId, this->GetToolReferenceFrameName());
171  std::string toolSourceId = toolTransformName.GetTransformName();
172 
173  if (toolDataElement->GetAttribute("MarkerId") != NULL && toolDataElement->GetAttribute("MarkerSizeMm") != NULL)
174  {
175  // this tool is tracked by a single marker
176  int MarkerId;
177  toolDataElement->GetScalarAttribute("MarkerId", MarkerId);
178  float MarkerSizeMm;
179  toolDataElement->GetScalarAttribute("MarkerSizeMm", MarkerSizeMm);
180  TrackedTool newTool(MarkerId, MarkerSizeMm, toolSourceId);
181  this->Internal->Tools.push_back(newTool);
182  }
183  else if (toolDataElement->GetAttribute("MarkerMapFile") != NULL)
184  {
185  // this tool is tracked by a marker map
186  // TODO: Implement marker map tracking.
187  }
188  else
189  {
190  LOG_ERROR("Incorrectly formatted tool data source.");
191  }
192  }
193 
194  return PLUS_SUCCESS;
195 }
196 
197 //----------------------------------------------------------------------------
198 PlusStatus vtkPlusOpticalMarkerTracker::WriteConfiguration(vtkXMLDataElement* rootConfigElement)
199 {
200  XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_WRITING(deviceConfig, rootConfigElement);
201 
202  if (!this->Internal->CameraCalibrationFile.empty())
203  {
204  deviceConfig->SetAttribute("CameraCalibrationFile", this->Internal->CameraCalibrationFile.c_str());
205  }
206  if (!this->Internal->MarkerDictionary.empty())
207  {
208  deviceConfig->SetAttribute("MarkerDictionary", this->Internal->MarkerDictionary.c_str());
209  }
210  switch (this->Internal->TrackingMethod)
211  {
212  case TRACKING_OPTICAL:
213  deviceConfig->SetAttribute("TrackingMethod", "OPTICAL");
214  break;
216  deviceConfig->SetAttribute("TrackingMethod", "OPTICAL_AND_DEPTH");
217  break;
218  default:
219  LOG_ERROR("Unknown tracking method passed to vtkPlusOpticalMarkerTracker::WriteConfiguration");
220  return PLUS_FAIL;
221  }
222 
223  //TODO: Write data for custom attributes
224 
225  return PLUS_SUCCESS;
226 }
227 
228 //----------------------------------------------------------------------------
230 {
231  // get calibration file path && check file exists
232  std::string calibFilePath = vtkPlusConfig::GetInstance()->GetDeviceSetConfigurationPath(this->Internal->CameraCalibrationFile);
233  LOG_INFO("Use aruco camera calibration file located at: " << calibFilePath);
234  if (!vtksys::SystemTools::FileExists(calibFilePath.c_str(), true))
235  {
236  LOG_ERROR("Unable to find aruco camera calibration file at: " << calibFilePath);
237  return PLUS_FAIL;
238  }
239 
240  try
241  {
242  this->Internal->CameraParameters->readFromXMLFile(calibFilePath);
243  }
244  catch (cv::Exception e)
245  {
246  LOG_ERROR("Unable to read calibration file: " << e.msg.c_str());
247  return PLUS_FAIL;
248  }
249 
250  this->Internal->MarkerDetector->setDictionary(this->Internal->MarkerDictionary);
251 
252  // threshold tuning numbers from aruco_test
253  aruco::MarkerDetector::Params params;
254  params._thresParam1 = 7;
255  params._thresParam2 = 7;
256  params._thresParam1_range = 2;
257  this->Internal->MarkerDetector->setParams(params);
258 
259  bool lowestRateKnown = false;
260  double lowestRate = 30; // just a usual value (FPS)
261  for (ChannelContainerConstIterator it = begin(this->InputChannels); it != end(this->InputChannels); ++it)
262  {
263  vtkPlusChannel* anInputStream = (*it);
264  if (anInputStream->GetOwnerDevice()->GetAcquisitionRate() < lowestRate || !lowestRateKnown)
265  {
266  lowestRate = anInputStream->GetOwnerDevice()->GetAcquisitionRate();
267  lowestRateKnown = true;
268  }
269  }
270  if (lowestRateKnown)
271  {
272  this->AcquisitionRate = lowestRate;
273  }
274  else
275  {
276  LOG_WARNING("vtkPlusOpticalMarkerTracker acquisition rate is not known");
277  }
278 
279  this->Internal->LastProcessedInputDataTimestamp = 0.0;
280  return PLUS_SUCCESS;
281 }
282 
283 //----------------------------------------------------------------------------
285 {
286  if (this->InputChannels.size() != 1)
287  {
288  LOG_ERROR("ImageProcessor device requires exactly 1 input stream (that contains video data). Check configuration.");
289  return PLUS_FAIL;
290  }
291 
292  return PLUS_SUCCESS;
293 }
294 
295 //----------------------------------------------------------------------------
296 PlusStatus vtkPlusOpticalMarkerTracker::vtkInternal::BuildTransformMatrix(vtkSmartPointer<vtkMatrix4x4> transformMatrix, const cv::Mat& Rvec, const cv::Mat& Tvec)
297 {
298  transformMatrix->Identity();
299  cv::Mat Rmat(3, 3, CV_32FC1);
300  try
301  {
302  cv::Rodrigues(Rvec, Rmat);
303  }
304  catch (...)
305  {
306  return PLUS_FAIL;
307  }
308 
309  for (int x = 0; x <= 2; x++)
310  {
311  // Depending on aruco version (and likely opencv version), the translation vector is either a column
312  // or a row vector
313  transformMatrix->SetElement(x, 3, MM_PER_M * ((Tvec.rows == 3) ? Tvec.at<float>(x, 0) : Tvec.at<float>(0, x)));
314  for (int y = 0; y <= 2; y++)
315  {
316  transformMatrix->SetElement(x, y, Rmat.at<float>(x, y));
317  }
318  }
319 
320  return PLUS_SUCCESS;
321 }
322 
323 //----------------------------------------------------------------------------
325 {
326  // Get image to tracker transform from the tracker (only request 1 frame, the latest)
327  if (!this->InputChannels[0]->GetVideoDataAvailable())
328  {
329  LOG_TRACE("Processed data is not generated, as no video data is available yet. Device ID: " << this->GetDeviceId());
330  return PLUS_SUCCESS;
331  }
332 
333  double oldestTrackingTimestamp(0);
334  if (this->InputChannels[0]->GetOldestTimestamp(oldestTrackingTimestamp) == PLUS_SUCCESS)
335  {
336  if (this->Internal->LastProcessedInputDataTimestamp > oldestTrackingTimestamp)
337  {
338  LOG_INFO("Processed image generation started. No tracking data was available between " << this->Internal->LastProcessedInputDataTimestamp << "-" << oldestTrackingTimestamp <<
339  "sec, therefore no processed images were generated during this time period.");
340  this->Internal->LastProcessedInputDataTimestamp = oldestTrackingTimestamp;
341  }
342  }
343 
344  igsioTrackedFrame trackedFrame;
345  if (this->InputChannels[0]->GetTrackedFrame(trackedFrame) != PLUS_SUCCESS)
346  {
347  LOG_ERROR("Error while getting latest tracked frame. Last recorded timestamp: " << std::fixed << this->Internal->LastProcessedInputDataTimestamp << ". Device ID: " << this->GetDeviceId());
348  this->Internal->LastProcessedInputDataTimestamp = vtkIGSIOAccurateTimer::GetSystemTime(); // forget about the past, try to add frames that are acquired from now on
349  return PLUS_FAIL;
350  }
351 
352  LOG_TRACE("Image to be processed: timestamp=" << trackedFrame.GetTimestamp());
353 
354  // get dimensions & data
355  FrameSizeType dim = trackedFrame.GetFrameSize();
356  igsioVideoFrame* frame = trackedFrame.GetImageData();
357 
358  // converting trackedFrame (vtkImageData) to cv::Mat
359  cv::Mat image(dim[1], dim[0], CV_8UC3, cv::Scalar(0, 0, 255));
360 
361  // Plus image uses RGB and OpenCV uses BGR, swapping is only necessary for colored markers
362  //PixelCodec::RgbBgrSwap(dim[0], dim[1], (unsigned char*)frame->GetScalarPointer(), image.data);
363  image.data = (unsigned char*)frame->GetScalarPointer();
364 
365  // detect markers in frame
366  this->Internal->MarkerDetector->detect(image, this->Internal->Markers);
367 
368  if (!this->Internal->MarkerFound && this->Internal->Markers.size() > 0)
369  {
370  this->Internal->MarkerFound = true;
371  }
372 
373  if (!this->Internal->MarkerFound)
374  {
375  // Try flipping the incoming image horizontally and trying again
376  // This is a very common obstacle
377  cv::flip(image, image, 1); // 0 flip vert, > 0 flip horz, < 0 flip both (eewwwwwww)
378  this->Internal->MarkerDetector->detect(image, this->Internal->Markers);
379  if (this->Internal->Markers.size() > 0)
380  {
381  // We have a flip problem!
382  vtkPlusDataSource* source(nullptr);
383  this->InputChannels[0]->GetVideoSource(source);
384  source->SetInputImageOrientation(igsioCommon::HorizontalFlip(source->GetInputImageOrientation()));
385  LOG_WARNING("Autodetected horizontal image flip problem. It has been corrected for this session. " \
386  "Be sure to save your config file or update your existing file to the new orientation: " \
387  << igsioCommon::GetStringFromUsImageOrientation(source->GetInputImageOrientation()));
388  this->Internal->MarkerFound = true;
389  }
390  }
391 
392  // iterate through tools updating tracking
393  for (std::vector<TrackedTool>::iterator toolIt = begin(this->Internal->Tools); toolIt != end(this->Internal->Tools); ++toolIt)
394  {
395  bool toolInFrame = false;
396  const double unfilteredTimestamp = vtkIGSIOAccurateTimer::GetSystemTime();
397  for (std::vector<aruco::Marker>::iterator markerIt = begin(this->Internal->Markers); markerIt != end(this->Internal->Markers); ++markerIt)
398  {
399  if (toolIt->MarkerId == markerIt->id)
400  {
401  //marker is in frame
402  toolInFrame = true;
403 
404  if (toolIt->MarkerPoseTracker.estimatePose(*markerIt, *this->Internal->CameraParameters, toolIt->MarkerSizeMm / MM_PER_M, 4))
405  {
406  // pose successfully estimated, update transform
407  cv::Mat Rvec = toolIt->MarkerPoseTracker.getRvec();
408  cv::Mat Tvec = toolIt->MarkerPoseTracker.getTvec();
409  this->Internal->BuildTransformMatrix(toolIt->transformMatrix, Rvec, Tvec);
410  ToolTimeStampedUpdate(toolIt->ToolSourceId, toolIt->transformMatrix, TOOL_OK, this->FrameNumber, unfilteredTimestamp);
411  }
412  else
413  {
414  // pose estimation failed
415  // TODO: add frame num, marker id, etc. Make this error more helpful. Is there a way to handle it?
416  LOG_ERROR("Pose estimation failed. Tool " << toolIt->ToolSourceId << " with marker " << toolIt->MarkerId << ".");
417  }
418  break;
419  }
420  }
421  if (!toolInFrame)
422  {
423  // tool not in frame
424  ToolTimeStampedUpdate(toolIt->ToolSourceId, toolIt->transformMatrix, TOOL_OUT_OF_VIEW, this->FrameNumber, unfilteredTimestamp);
425  }
426  }
427 
428  this->FrameNumber++;
429 
430  return PLUS_SUCCESS;
431 }
virtual void PrintSelf(ostream &os, vtkIndent indent) VTK_OVERRIDE
Abstract interface for tracker and video devices.
Definition: vtkPlusDevice.h:60
const char * source
Definition: phidget22.h:2461
#define XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_WRITING(deviceConfig, rootConfigElement)
igsioStatus PlusStatus
Definition: PlusCommon.h:40
ChannelContainer InputChannels
virtual std::string GetDeviceId() const
virtual PlusStatus ToolTimeStampedUpdate(const std::string &aToolSourceId, vtkMatrix4x4 *matrix, ToolStatus status, unsigned long frameNumber, double unfilteredtimestamp, const igsioFieldMapType *customFields=NULL)
Virtual device that tracks fiducial markers on the input channel in real time.
double AcquisitionRate
vtkStandardNewMacro(vtkPlusOpticalMarkerTracker)
#define PLUS_FAIL
Definition: PlusCommon.h:43
DataSourceContainer Tools
static vtkPlusConfig * GetInstance()
virtual double GetAcquisitionRate() const
#define MM_PER_M
virtual PlusStatus WriteConfiguration(vtkXMLDataElement *)
unsigned long FrameNumber
#define PLUS_SUCCESS
Definition: PlusCommon.h:44
ChannelContainer::const_iterator ChannelContainerConstIterator
Definition: vtkPlusDevice.h:35
virtual PlusStatus ReadConfiguration(vtkXMLDataElement *)
#define XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_READING(deviceConfig, rootConfigElement)
int x
Definition: phidget22.h:4265
bool StartThreadForInternalUpdates
virtual void PrintSelf(ostream &os, vtkIndent indent) VTK_OVERRIDE
Contains an optional timestamped circular buffer containing the video images and a number of timestam...
std::string GetDeviceSetConfigurationPath(const std::string &subPath)
Direction vectors of rods y
Definition: algo3.m:15
std::string GetToolReferenceFrameName() const
vtkPlusDevice * GetOwnerDevice() const
Interface to a 3D positioning tool, video source, or generalized data stream.