Medical Imaging Interaction Toolkit  2016.11.0
Medical Imaging Interaction Toolkit
mitkClaronInterface.cpp
Go to the documentation of this file.
1 /*===================================================================
2 
3 The Medical Imaging Interaction Toolkit (MITK)
4 
5 Copyright (c) German Cancer Research Center,
6 Division of Medical and Biological Informatics.
7 All rights reserved.
8 
9 This software is distributed WITHOUT ANY WARRANTY; without
10 even the implied warranty of MERCHANTABILITY or FITNESS FOR
11 A PARTICULAR PURPOSE.
12 
13 See LICENSE.txt or http://www.mitk.org for details.
14 
15 ===================================================================*/
16 
17 #include <mitkClaronInterface.h>
18 #include <string>
19 #include <MTC.h>
20 #include <math.h>
21 #include <mitkNumericTypes.h>
22 
24 {
25  isTracking = false;
26  sprintf(calibrationDir,"No calibration dir set yet");
27  sprintf(markerDir,"No marker dir set yet");
28 }
29 
31 {
32 
33 }
34 
35 void mitk::ClaronInterface::Initialize(std::string calibrationDir, std::string toolFilesDir)
36 {
37  sprintf(this->calibrationDir, calibrationDir.c_str());
38  sprintf(this->markerDir,toolFilesDir.c_str());
39  this->IdentifiedMarkers = 0;
40  this->PoseXf = 0;
41  this->CurrCamera = 0;
42  this->IdentifyingCamera = 0;
43 }
44 
45 
47 {
48  isTracking = false;
49  MTC( Cameras_AttachAvailableCameras(calibrationDir) ); //Connect to camera
50  if (Cameras_Count() < 1)
51  {
52  printf("No camera found!\n");
53  return false;
54  }
55 
56  try
57  {
58  //Step 1: initialize cameras
59  MTC(Cameras_HistogramEqualizeImagesSet(true)); //set the histogram equalizing
60  MTC( Cameras_ItemGet(0, &CurrCamera) ); //Obtain a handle to the first/only camera in the array
61 
62  MITK_INFO<<markerDir;
63  //Step 2: Load the marker templates
64  MTC( Markers_LoadTemplates(markerDir) ); //Path to directory where the marker templates are
65  printf("Loaded %d marker templates\n",Markers_TemplatesCount());
66 
67  //Step 3: Wait for 20 frames
68  for (int i=0; i<20; i++)//the first 20 frames are auto-adjustment frames, we ignore them
69  {
70  MTC( Cameras_GrabFrame(NULL) ); //Grab a frame (all cameras together)
71  MTC( Markers_ProcessFrame(NULL) ); //Proces the frame(s) to obtain measurements
72  }
73 
74  //Step 4: Initialize IdentifiedMarkers and PoseXf
75  IdentifiedMarkers = Collection_New();
76  PoseXf = Xform3D_New();
77 
78  //now we are tracking...
79 
80  /* MTHome is not in use. The following code has to be activated if you want to use MTHome!
81  //Initialize MTHome
82  if ( getMTHome (MTHome, sizeof(MTHome)) < 0 )
83  {
84  // No Environment
85  printf("MTHome environment variable is not set!\n");
86  }*/
87  }
88  catch(...)
89  {
90  Cameras_Detach();
91  printf(" Error while connecting MicronTracker!\n -------------------------------");
92  return false;
93  }
94 
95  isTracking = true;
96  return true;
97 }
98 
100 {
101  if (isTracking)
102  {
103  //free up the resources
104  Collection_Free(IdentifiedMarkers);
105  Xform3D_Free(PoseXf);
106 
107  //stop the camera
108  Cameras_Detach();
109 
110  //now tracking is stopped
111  isTracking = false;
112  return true;
113  }
114  else
115  {
116  return false;
117  }
118 }
119 
120 std::vector<mitk::claronToolHandle> mitk::ClaronInterface::GetAllActiveTools()
121 {
122  //Set returnvalue
123  std::vector<claronToolHandle> returnValue;
124 
125  //Here, MTC internally maintains the measurement results.
126  //Those results can be accessed until the next call to Markers_ProcessFrame, when they
127  //are updated to reflect the next frame's content.
128  //First, we will obtain the collection of the markers that were identified.
129  MTC( Markers_IdentifiedMarkersGet(NULL, IdentifiedMarkers) );
130 
131  //Now we iterate on the identified markers and add them to the returnvalue
132  for (int j=1; j<=Collection_Count(IdentifiedMarkers); j++)
133  {
134  // Obtain the marker's handle, and use it to obtain the pose in the current camera's space
135  // using our Xform3D object, PoseXf.
136  mtHandle Marker = Collection_Int(IdentifiedMarkers, j);
137  returnValue.push_back(Marker);
138  }
139  return returnValue;
140 }
141 
143 {
144  MTC( Cameras_GrabFrame(NULL) ); //Grab a frame
145  MTC( Markers_ProcessFrame(NULL) ); //Process the frame(s)
146 }
147 
149 {
150  std::vector<double> returnValue;
151  double Position[3];
152  mtHandle t2m = Xform3D_New(); // tooltip to marker xform handle
153  mtHandle t2c = Xform3D_New(); // tooltip to camera xform handle
154  mtHandle m2c = Xform3D_New(); // marker to camera xform handle
155 
156  //Get m2c
157  MTC( Marker_Marker2CameraXfGet (c, CurrCamera, m2c, &IdentifyingCamera) );
158  //Get t2m
159  MTC( Marker_Tooltip2MarkerXfGet (c, t2m ));
160  //Transform both to t2c
161  MTC(Xform3D_Concatenate(t2m,m2c,t2c));
162 
163  //Get position
164  MTC( Xform3D_ShiftGet(t2c, Position) );
165 
166  // Here we have to negate the X- and Y-coordinates because of a bug of the
167  // MTC-library.
168  returnValue.push_back(-Position[0]);
169  returnValue.push_back(-Position[1]);
170  returnValue.push_back(Position[2]);
171 
172  return returnValue;
173 }
174 
176 {
177  std::vector<double> returnValue;
178  double Position[3];
179  MTC( Marker_Marker2CameraXfGet (c, CurrCamera, PoseXf, &IdentifyingCamera) );
180  MTC( Xform3D_ShiftGet(PoseXf, Position) );
181 
182  // Here we have to negate the X- and Y-coordinates because of a bug of the
183  // MTC-library.
184  returnValue.push_back(-Position[0]);
185  returnValue.push_back(-Position[1]);
186  returnValue.push_back(Position[2]);
187 
188  return returnValue;
189 }
190 
191 
193 {
194  std::vector<double> returnValue;
195 
196  mtHandle t2m = Xform3D_New(); // tooltip to marker xform handle
197  mtHandle t2c = Xform3D_New(); // tooltip to camera xform handle
198  mtHandle m2c = Xform3D_New(); // marker to camera xform handle
199 
200  //Get m2c
201  MTC( Marker_Marker2CameraXfGet (c, CurrCamera, m2c, &IdentifyingCamera) );
202  //Get t2m
203  MTC( Marker_Tooltip2MarkerXfGet (c, t2m ));
204  //Transform both to t2c
205  MTC(Xform3D_Concatenate(t2m,m2c,t2c));
206 
207  //get the Claron-Quaternion
208  double Quarternions[4];
209  MTC( Xform3D_RotQuaternionsGet(t2c, Quarternions) );
210  mitk::Quaternion claronQuaternion;
211 
212  //note: claron quarternion has different order than the mitk quarternion
213  claronQuaternion[3] = Quarternions[0];
214  claronQuaternion[0] = Quarternions[1];
215  claronQuaternion[1] = Quarternions[2];
216  claronQuaternion[2] = Quarternions[3];
217 
218  // Here we have to make a -90�-turn around the Y-axis because of a bug of the
219  // MTC-library.
220  mitk::Quaternion minusNinetyDegreeY;
221  minusNinetyDegreeY[3] = sqrt(2.0)/2.0;
222  minusNinetyDegreeY[0] = 0;
223  minusNinetyDegreeY[1] = -1.0/(sqrt(2.0));
224  minusNinetyDegreeY[2] = 0;
225 
226  //calculate the result...
227  mitk::Quaternion erg = (minusNinetyDegreeY*claronQuaternion);
228 
229  returnValue.push_back(erg[3]);
230  returnValue.push_back(erg[0]);
231  returnValue.push_back(erg[1]);
232  returnValue.push_back(erg[2]);
233 
234  return returnValue;
235 }
236 
238 {
239  std::vector<double> returnValue;
240 
241  double Quarternions[4];
242  MTC( Marker_Marker2CameraXfGet (c, CurrCamera, PoseXf, &IdentifyingCamera) );
243  MTC( Xform3D_RotQuaternionsGet(PoseXf, Quarternions) );
244 
245  //here we have to compensate a bug in the MTC-lib. (difficult to understand)
246  mitk::Quaternion claronQuaternion;
247 
248  //note: claron quarternion has different order than the mitk quarternion
249  claronQuaternion[3] = Quarternions[0];
250  claronQuaternion[0] = Quarternions[1];
251  claronQuaternion[1] = Quarternions[2];
252  claronQuaternion[2] = Quarternions[3];
253 
254  // Here we have to make a -90�-turn around the Y-axis because of a bug of the
255  // MTC-library.
256  mitk::Quaternion minusNinetyDegreeY;
257  minusNinetyDegreeY[3] = sqrt(2.0)/2.0;
258  minusNinetyDegreeY[0] = 0;
259  minusNinetyDegreeY[1] = -1.0/(sqrt(2.0));
260  minusNinetyDegreeY[2] = 0;
261 
262  //calculate the result...
263  mitk::Quaternion erg = (minusNinetyDegreeY*claronQuaternion);
264 
265  returnValue.push_back(erg[3]);
266  returnValue.push_back(erg[0]);
267  returnValue.push_back(erg[1]);
268  returnValue.push_back(erg[2]);
269 
270  return returnValue;
271 }
272 
273 
274 
276 {
277  char MarkerName[MT_MAX_STRING_LENGTH];
278  MTC( Marker_NameGet(c, MarkerName, MT_MAX_STRING_LENGTH, 0) );
279  std::string* returnValue = new std::string(MarkerName);
280  return returnValue->c_str();
281 }
282 
284 {
285  return this->isTracking;
286 }
287 
289  {
290  return true;
291  }
char calibrationDir[512]
Variable which holds the directory which should contain the file BumbleBee_6400420.calib. This directory is needed by the MTC library.
#define MITK_INFO
Definition: mitkLogMacros.h:22
ClaronInterface()
standard constructor
std::vector< double > GetTipPosition(claronToolHandle c)
std::vector< double > GetTipQuaternions(claronToolHandle c)
int claronToolHandle
#define MTC(func)
~ClaronInterface()
standard destructor
const char * GetName(claronToolHandle c)
std::vector< double > GetPosition(claronToolHandle c)
bool StopTracking()
Clears all resources. After this method have been called the system isn't ready to track any longer...
bool StartTracking()
Opens the connection to the device and makes it ready to track tools.
void GrabFrame()
Grabs a frame from the camera.
std::vector< double > GetQuaternions(claronToolHandle c)
vnl_quaternion< ScalarType > Quaternion
bool isTracking
Variable is true if the device is tracking at the moment, false if not.
char markerDir[512]
Variable which holds a directory with some tool files in it. All this tools are trackable when the pa...
int mtHandle
void Initialize(std::string calibrationDir, std::string toolFilesDir)
Initialization of claroninterface.
std::vector< claronToolHandle > GetAllActiveTools()