1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
|
/// @file matleap.cpp
/// @brief leap motion controller interface
/// @author Jeff Perry <jeffsp@gmail.com>
/// @version 1.0
/// @date 2013-09-12
#include "matleap.h"
#include <array>
// Under Windows, a Leap::Controller must be allocated after the MEX
// startup code has completed. Also, a Leap::Controller must be
// deleted in the function specified by mexAtExit after all global
// destructors are called. If the Leap::Controller is not allocated
// and freed in this way, the MEX function will crash and cause MATLAB
// to hang or close abruptly. Linux and OS/X don't have these
// constraints, and you can just create a global Leap::Controller
// instance.
// Global instance pointer
matleap::frame_grabber *fg = 0;
int version = 4; // 1: orig, 2: with arm info, 3: with more hand info
// Exit function
void matleap_exit() {
fg->close_connection();
delete fg;
fg = 0;
}
/// @brief process interface arguments
///
/// @param nlhs matlab mex output interface
/// @param plhs[] matlab mex output interface
/// @param nrhs matlab mex input interface
/// @param prhs[] matlab mex input interface
///
/// @return command number
int get_command(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
int command;
// check inputs
switch (nrhs) {
case 1:
command = *mxGetPr(prhs[0]);
break;
case 0:
mexErrMsgTxt("Not enough input arguments");
default:
mexErrMsgTxt("Too many input arguments");
}
// check that inputs agree with command
switch (command) {
case -1: {
// set debug command requires 0 outputs
if (nlhs != 0)
mexErrMsgTxt("Wrong number of outputs specified");
}
break;
case 0: {
// get version command requires 1 outputs
if (nlhs != 0 && nlhs != 1)
mexErrMsgTxt("Wrong number of outputs specified");
}
break;
case 1: {
// frame grab command only requires one input
if (nrhs > 1)
mexErrMsgTxt("Too many inputs specified");
// frame grab command requires exactly one output
if (nlhs != 0 && nlhs != 1)
mexErrMsgTxt("Wrong number of outputs specified");
}
break;
default:
mexErrMsgTxt("An unknown command was specified");
}
return command;
}
/// @brief helper function
///
/// @param v values to fill array with
///
/// @return created and filled array
mxArray *create_and_fill(const Leap::Vector &v) {
mxArray *p = mxCreateNumericMatrix(1, 3, mxDOUBLE_CLASS, mxREAL);
*(mxGetPr(p) + 0) = v.x;
*(mxGetPr(p) + 1) = v.y;
*(mxGetPr(p) + 2) = v.z;
return p;
}
/// @brief get a frame from the leap controller
///
/// @param nlhs matlab mex output interface
/// @param plhs[] matlab mex output interface
void get_frame(int nlhs, mxArray *plhs[]) {
// get the frame
const matleap::frame &f = fg->get_frame();
// create matlab frame struct
const char *frame_field_names[] =
{
"id",
"timestamp",
"gesture",
"hands",
"version"
};
int frame_fields = sizeof(frame_field_names) / sizeof(*frame_field_names);
plhs[0] = mxCreateStructMatrix(1, 1, frame_fields, frame_field_names);
// fill the frame struct
mxSetFieldByNumber(plhs[0], 0, 0, mxCreateDoubleScalar(f.id));
mxSetFieldByNumber(plhs[0], 0, 1, mxCreateDoubleScalar(f.timestamp));
mxSetFieldByNumber(plhs[0], 0, 2, mxCreateDoubleScalar(f.has_gesture ? 1 : 0));
if (f.detectedHands > 0) {
const char *hand_field_names[] =
{
"id", // 0
"confidence", // 2
"visible_time",
"pinch_strength",
"grab_strength",
"palm",
"digits"
};
int hand_fields = sizeof(hand_field_names) / sizeof(*hand_field_names);
mxArray *p = mxCreateStructMatrix(1, f.detectedHands, hand_fields, hand_field_names);
mxSetFieldByNumber(plhs[0], 0, 3, p);
// 3 because hands is the third (fourth) field name in
// the overall struct we are creating.
for (int i = 0; i < f.detectedHands; ++i) {
// one by one, get the fields for the hand
mxSetFieldByNumber(p, i, 0, mxCreateDoubleScalar(f.hands[i].id()));
mxSetFieldByNumber(p, i, 1, mxCreateDoubleScalar(f.hands[i].confidence()));
mxSetFieldByNumber(p, i, 2, mxCreateDoubleScalar(f.hands[i].timeVisible()));
mxSetFieldByNumber(p, i, 3, mxCreateDoubleScalar(f.hands[i].pinchStrength()));
mxSetFieldByNumber(p, i, 4, mxCreateDoubleScalar(f.hands[i].grabStrength()));
// palm
const char *palm_field_names[] =
{
"position",
"stabilized_position",
"velocity",
"normal",
"width",
"direction",
};
int palm_fields = sizeof(palm_field_names) / sizeof(*palm_field_names);
mxArray *palm = mxCreateStructMatrix(1, 1, palm_fields, palm_field_names);
mxSetFieldByNumber(p, i, 5, palm);
mxSetFieldByNumber(palm, 0, 0, create_and_fill(f.hands[i].palmPosition()));
mxSetFieldByNumber(palm, 0, 1, create_and_fill(f.hands[i].stabilizedPalmPosition()));
mxSetFieldByNumber(palm, 0, 2, create_and_fill(f.hands[i].palmVelocity()));
mxSetFieldByNumber(palm, 0, 3, create_and_fill(f.hands[i].palmNormal()));
mxSetFieldByNumber(palm, 0, 4, mxCreateDoubleScalar(f.hands[i].palmWidth()));
mxSetFieldByNumber(palm, 0, 5, create_and_fill(f.hands[i].direction()));
// get bones for all fingers
const char *digit_field_names[] =
{
"finger_id", // 0
"is_extended",
"bones",
};
int digit_fields = sizeof(digit_field_names) / sizeof(*digit_field_names);
Leap::Finger digits[5];
std::copy(f.hands[i].fingers().begin(), f.hands[i].fingers().end(), std::begin(digits));
mxArray *d = mxCreateStructMatrix(1, 5, digit_fields, digit_field_names);
mxSetFieldByNumber(p, i, 6, d);
for (int d_it = 0; d_it < 5; d_it++) {
mxSetFieldByNumber(d, d_it, 0, mxCreateDoubleScalar(digits[d_it].id()));
mxSetFieldByNumber(d, d_it, 1, mxCreateDoubleScalar(digits[d_it].isExtended()));
const char *bone_field_names[] =
{
"prev_joint", // 0
"next_joint", // 1
"width",// 2
"rotation"
};
int bone_fields = sizeof(bone_field_names) / sizeof(*bone_field_names);
mxArray *bones = mxCreateStructMatrix(1, 4, bone_fields, bone_field_names);
mxSetFieldByNumber(d, d_it, 2, bones);
auto const boneParts = {Leap::Bone::TYPE_METACARPAL, Leap::Bone::TYPE_PROXIMAL,
Leap::Bone::TYPE_INTERMEDIATE, Leap::Bone::TYPE_DISTAL};
Leap::Bone bone;
for (auto bi : boneParts) {
bone = digits[d_it].bone(bi);
mxSetFieldByNumber(bones, bi, 0, create_and_fill(bone.prevJoint())); // 0
mxSetFieldByNumber(bones, bi, 1, create_and_fill(bone.nextJoint())); // 1
mxSetFieldByNumber(bones, bi, 2, mxCreateDoubleScalar(bone.width())); // 2
mxSetFieldByNumber(bones, bi, 3, create_and_fill(bone.direction()));
}
}
} // re: for f.hands.count()
} // re: if f.hands.count() > 0
mxSetFieldByNumber(plhs[0], 0, 5, mxCreateDoubleScalar(version));
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
if (!fg) {
fg = new matleap::frame_grabber;
if (fg == 0)
mexErrMsgTxt("Cannot allocate a frame grabber");
fg->open_connection();
mexAtExit(matleap_exit);
}
switch (get_command(nlhs, plhs, nrhs, prhs)) {
// turn on debug
case -1:
fg->set_debug(true);
return;
// get version
case 0:
plhs[0] = mxCreateNumericMatrix(1, 2, mxDOUBLE_CLASS, mxREAL);
*(mxGetPr(plhs[0]) + 0) = MAJOR_REVISION;
*(mxGetPr(plhs[0]) + 1) = MINOR_REVISION;
return;
// get frame
case 1:
get_frame(nlhs, plhs);
return;
default:
// this is a logic error
mexErrMsgTxt("unknown error: please contact developer");
}
}
|
