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