DTrackSDK  v2.9.0
example_fingertracking.cpp
1 /* DTrackSDK in C++: example_fingertracking.cpp
2  *
3  * C++ example using DTrackSDK FINGERTRACKING data.
4  *
5  * Copyright (c) 2013-2022 Advanced Realtime Tracking GmbH & Co. KG
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * 1. Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * 2. Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * 3. Neither the name of copyright holder nor the names of its contributors
16  * may be used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
20  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
21  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
23  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
24  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
25  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
27  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29  *
30  * Purpose:
31  * - example without DTrack2/DTRACK3 remote commands: just collects frames
32  * - output of various calculated FINGERTRACKING data
33  * - please start measurement manually e.g. in DTrack frontend application
34  * - for DTrackSDK v2.6.0 (or newer)
35  */
36 
37 #include "DTrackSDK.hpp"
38 
39 #include <iostream>
40 #include <sstream>
41 
42 #include "SampleMath.h"
43 
44 // global DTrackSDK
45 static DTrackSDK* dt = NULL;
46 
47 // sample finger structure with complete pose data of all joints and phalanxes (3 poses plus 1 position)
49 {
50  SampleLoc locRoot; // position of root finger joint
51  SampleRot rotRoot; // rotation of inner phalanx
52 
53  SampleLoc locMiddle; // position of middle finger joint
54  SampleRot rotMiddle; // rotation of middle phalanx
55 
56  SampleLoc locOuter; // position of outer finger joint
57  SampleRot rotOuter; // rotation of outer phalanx
58 
59  SampleLoc locTip; // position of finger tip
60 };
61 
62 static void calculateSampleFinger( const DTrackHand* hand, int indFinger, SampleFinger& sampleFinger );
63 
64 // prototypes
65 static void output_to_console();
66 static bool data_error_to_console();
67 
68 
72 int main( int argc, char** argv )
73 {
74  if ( argc != 2 )
75  {
76  std::cout << "Usage: example_fingertracking <data port>" << std::endl;
77  return -1;
78  }
79 
80  std::istringstream portstream( argv[ 1 ] );
81  unsigned short port;
82  portstream >> port; // data port
83 
84  if ( portstream.fail() )
85  {
86  std::cout << "invalid port '" << argv[ 1 ] << "'" << std::endl;
87  return -2;
88  }
89 
90  // initialization:
91 
92  dt = new DTrackSDK( port );
93 
94  if ( ! dt->isDataInterfaceValid() )
95  {
96  std::cout << "DTrackSDK init error" << std::endl;
97  return -3;
98  }
99  std::cout << "listening at local data port " << dt->getDataPort() << std::endl;
100 
101 // dt->setDataTimeoutUS( 3000000 ); // NOTE: change here timeout for receiving tracking data, if necessary
102 // dt->setDataBufferSize( 100000 ); // NOTE: change here buffer size for receiving tracking data, if necessary
103 
104  // measurement:
105 
106  int count = 0;
107  while ( count++ < 1000 ) // collect 1000 frames
108  {
109  if ( dt->receive() )
110  {
111  output_to_console();
112  }
113  else
114  {
115  data_error_to_console();
116  }
117  }
118 
119  delete dt; // clean up
120  return 0;
121 }
122 
123 
127 static void output_to_console()
128 {
129  std::cout.precision( 3 );
130  std::cout.setf( std::ios::fixed, std::ios::floatfield );
131 
132  std::cout << std::endl << "frame " << dt->getFrameCounter() << " ts " << dt->getTimeStamp()
133  << " nhand " << dt->getNumHand()
134  << std::endl;
135 
136  // A.R.T. FINGERTRACKING hands:
137  for ( int i = 0; i < dt->getNumHand(); i++ )
138  {
139  const DTrackHand* hand = dt->getHand( i );
140  if ( hand == NULL )
141  {
142  std::cout << "DTrackSDK fatal error: invalid FINGERTRACKING id " << i << std::endl;
143  break;
144  }
145 
146  if ( ! hand->isTracked() )
147  {
148  std::cout << "hand " << hand->id << " not tracked" << std::endl;
149  }
150  else
151  {
152  std::cout << "hand " << hand->id << " qu " << hand->quality
153  << " lr " << ( ( hand->lr == 0 ) ? "left" : "right") << " nf " << hand->nfinger
154  << " " << SampleLoc( hand->loc ) << " " << SampleRot( hand->rot ) << std::endl;
155 
156  // calculating position for tip and poses for all joints of the fingers (in ART hand coordinate system):
157 
158  for ( int j = 0; j < hand->nfinger; j++ )
159  {
160  SampleFinger sampleFinger;
161  calculateSampleFinger( hand, j, sampleFinger );
162 
163  std::cout << " finger " << j << " tip " << sampleFinger.locTip << std::endl;
164  std::cout << " outer " << sampleFinger.locOuter << " " << sampleFinger.rotOuter << std::endl;
165  std::cout << " middle " << sampleFinger.locMiddle << " " << sampleFinger.rotMiddle << std::endl;
166  std::cout << " root " << sampleFinger.locRoot << " " << sampleFinger.rotRoot << std::endl;
167  }
168  }
169  std::cout << std::endl;
170  }
171 }
172 
173 
177 static void calculateSampleFinger( const DTrackHand* hand, int indFinger, SampleFinger& sampleFinger )
178 {
179  // finger tip:
180 
181  sampleFinger.locTip = SampleLoc( hand->finger[ indFinger ].loc );
182 
183  // outer finger phalanx:
184 
185  sampleFinger.rotOuter = SampleRot( hand->finger[ indFinger ].rot );
186 
187  SampleLoc phalanxOuter( -hand->finger[ indFinger ].lengthphalanx[ 0 ], 0.0, 0.0 );
188  sampleFinger.locOuter = sampleFinger.rotOuter * phalanxOuter + sampleFinger.locTip;
189 
190  // middle finger phalanx:
191 
192  SampleRot jointOuter = SampleRot::rotationY( hand->finger[ indFinger ].anglephalanx[ 0 ] );
193  sampleFinger.rotMiddle = sampleFinger.rotOuter * jointOuter;
194 
195  SampleLoc phalanxMiddle( -hand->finger[ indFinger ].lengthphalanx[ 1 ], 0.0, 0.0 );
196  sampleFinger.locMiddle = sampleFinger.rotMiddle * phalanxMiddle + sampleFinger.locOuter;
197 
198  // inner finger phalanx:
199 
200  SampleRot jointInner = SampleRot::rotationY( hand->finger[ indFinger ].anglephalanx[ 1 ] );
201  sampleFinger.rotRoot = sampleFinger.rotMiddle * jointInner;
202 
203  SampleLoc phalanxInner( -hand->finger[ indFinger ].lengthphalanx[ 2 ], 0.0, 0.0 );
204  sampleFinger.locRoot = sampleFinger.rotRoot * phalanxInner + sampleFinger.locMiddle;
205 }
206 
207 
213 static bool data_error_to_console()
214 {
216  {
217  std::cout << "--- timeout while waiting for tracking data" << std::endl;
218  return false;
219  }
220 
221  if ( dt->getLastDataError() == DTrackSDK::ERR_NET )
222  {
223  std::cout << "--- error while receiving tracking data" << std::endl;
224  return false;
225  }
226 
227  if ( dt->getLastDataError() == DTrackSDK::ERR_PARSE )
228  {
229  std::cout << "--- error while parsing tracking data" << std::endl;
230  return false;
231  }
232 
233  return true;
234 }
235 
double getTimeStamp() const
Get timestamp since midnight.
bool receive()
Receive and process one tracking data packet.
Definition: DTrackSDK.cpp:441
unsigned short getDataPort() const
Get UDP data port where tracking data is received.
Definition: DTrackSDK.cpp:321
int lr
Left (0) or right (1) hand.
int getNumHand() const
Get number of calibrated A.R.T. FINGERTRACKING hands (as far as known).
Minimum class for a rotation.
Definition: SampleMath.h:89
A.R.T. FINGERTRACKING hand data (6DOF + fingers).
int nfinger
Number of fingers (maximum 5)
double rot[9]
Rotation matrix of outermost phalanx (column-wise)
double loc[3]
Location of tip (in [mm])
Minimum class for a position.
Definition: SampleMath.h:43
DTrack SDK main class derived from DTrackParser.
Definition: DTrackSDK.hpp:77
double anglephalanx[2]
Angle between phalanxes (order: outermost, innermost; in [deg])
double lengthphalanx[3]
Length of phalanxes (order: outermost, middle, innermost; in [mm])
unsigned int getFrameCounter() const
Get frame counter.
double quality
Quality (0.0 <= qu <= 1.0, no tracking if -1.0)
double loc[3]
Location of back of the hand (in [mm])
const DTrackHand * getHand(int id) const
Get A.R.T. FINGERTRACKING hand data.
Timeout occured.
Definition: DTrackSDK.hpp:93
int id
ID number (starting with 0)
Error while parsing command.
Definition: DTrackSDK.hpp:95
bool isDataInterfaceValid() const
Returns if UDP socket is open to receive tracking data on local machine.
Definition: DTrackSDK.cpp:310
Errors getLastDataError() const
Get last error at receiving tracking data (data transmission).
Definition: DTrackSDK.cpp:549
double rot[9]
Rotation matrix of back of the hand (column-wise)
struct DTrackSDK_Datatypes::DTrackHand::DTrackFinger finger[DTRACKSDK_HAND_MAX_FINGER]
Finger data (order: thumb, index finger, middle finger, ...)
bool isTracked() const
Returns if hand is currently tracked.
Network error.
Definition: DTrackSDK.hpp:94