?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
temp commit
[SARndbox.git] / HandExtractor.cpp
blob6d4fc8841077439c200c709dc6f064556e1029ca
1 /***********************************************************************
2 HandExtractor - Class to identify hands from a depth image.
3 Copyright (c) 2015-2016 Oliver Kreylos
4 Copyright (c) 2019 Scottsdale Community College
5
6 This file is part of the Augmented Reality Sandbox (SARndbox).
7
8 The Augmented Reality Sandbox is free software; you can redistribute it
9 and/or modify it under the terms of the GNU General Public License as
10 published by the Free Software Foundation; either version 2 of the
11 License, or (at your option) any later version.
12
13 The Augmented Reality Sandbox is distributed in the hope that it will be
14 useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 General Public License for more details.
17
18 You should have received a copy of the GNU General Public License along
19 with the Augmented Reality Sandbox; if not, write to the Free Software
20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
21 ***********************************************************************/
22
23 #include "HandExtractor.h"
24
25 #include <Misc/Utility.h>
26 #include <Misc/FunctionCalls.h>
27 #include <Math/Math.h>
28 #include <Math/Interval.h>
29 #include <Geometry/Vector.h>
30
31 // DEBUGGING
32 #include <iostream>
33
34 namespace {
35
36 /**************
37 Helper classes:
38 **************/
39
40 struct Span { // Helper structure to extract foreground blobs from a depth image
41 /* Elements: */
42 public:
43 unsigned int y; // Row index of the span
44 unsigned int start; // Starting column of span
45 unsigned int end; // Ending column of span
46 unsigned int parent; // Span's parent span
47 unsigned int numPixels; // Number of pixels in the span's subtree
48 unsigned int blobId; // Blob ID of a root span
49 };
50
51 struct BlobOrigin { // Helper structure to store a point on the border of a foreground blob
52 /* Elements: */
53 public:
54 bool assigned; // Flag if the blob origin has already been assigned
55 unsigned int x, y; // Coordinates of blob origin in depth frame
56 const unsigned short* biPtr; // Pointer to blob origin in blob ID image
57 };
58
59 struct Corner { // Helper class to store corners in blob images
60 /* Elements: */
61 public:
62 int cornerType; // Corner type, +1: finger tip, -1: finger nook
63 unsigned start; // Boundary pixel index at which the corner started
64 int x, y; // Corner position in depth frame
65 };
66
67 typedef Math::Interval<float> Interval;
68 typedef Geometry::Point<float, 2> Point2;
69 typedef Geometry::Vector<float, 2> Vector2;
70
71 /****************
72 Helper functions:
73 ****************/
74
75 void drawLine(Images::RGBImage& image, const Point2& p0, const Point2& p1,
76 const Images::RGBImage::Color& color) {
77 int w = int(image.getWidth());
78 int h = int(image.getHeight());
79 int x0 = int(Math::floor(p0[0]));
80 int y0 = int(Math::floor(p0[1]));
81 int x1 = int(Math::floor(p1[0]));
82 int y1 = int(Math::floor(p1[1]));
83 int dx = x1 - x0;
84 int dy = y1 - y0;
85 ptrdiff_t stride = w;
86 if(Math::abs(dx) > Math::abs(dy)) {
87 /* X direction leads: */
88 if(dx < 0) {
89 x0 = x1;
90 y0 = y1;
91 dx = -dx;
92 dy = -dy;
93 }
94 Images::RGBImage::Color* lPtr = image.modifyPixels() + y0 * stride + x0;
95 int yf = dx / 2;
96 int y = 0;
97 for(int x = 0; x <= dx; ++x, ++lPtr) {
98 if(x0 + x >= 0 && x0 + x < w && y0 + y >= 0 && y0 + y < h)
99 *lPtr = color;
100 yf += dy;
101 if(yf >= dx) {
102 ++y;
103 lPtr += stride;
104 yf -= dx;
105 } else if(yf <= -dx) {
106 --y;
107 lPtr -= stride;
108 yf += dx;
109 }
110 }
111 } else {
112 /* Y direction leads: */
113 if(dy < 0) {
114 x0 = x1;
115 y0 = y1;
116 dx = -dx;
117 dy = -dy;
118 }
119 Images::RGBImage::Color* lPtr = image.modifyPixels() + y0 * stride + x0;
120 int xf = dy / 2;
121 int x = 0;
122 for(int y = 0; y <= dy; ++y, lPtr += stride) {
123 if(x0 + x >= 0 && x0 + x < w && y0 + y >= 0 && y0 + y < h)
124 *lPtr = color;
125 xf += dx;
126 if(xf >= dy) {
127 ++x;
128 ++lPtr;
129 xf -= dy;
130 } else if(xf <= -dy) {
131 --x;
132 --lPtr;
133 xf += dy;
134 }
135 }
136 }
137 }
138
139 void drawCircle(Images::RGBImage& image, const Point2& center, float radius,
140 const Images::RGBImage::Color& color) {
141 Images::RGBImage::Color* imgBase = image.modifyPixels();
142 int size[2];
143 size[0] = int(image.getSize(0));
144 size[1] = int(image.getSize(1));
145
146 /* Draw the circle: */
147 int cx = int(Math::floor(center[0]));
148 int cy = int(Math::floor(center[1]));
149 int r = int(Math::floor(radius + 0.5f));
150 ptrdiff_t stride = ptrdiff_t(size[0]);
151 #if 0 // Draw a filled rain circle
152 int ymin = cy - r >= 0 ? cy - r : 0;
153 int ymax = cy + r <= size[1] - 1 ? cy + r : size[1] - 1;
154 for(int y = ymin; y <= ymax; ++y) {
155 int ry = int(Math::floor(Math::sqrt(float(r * r) - float((y - cy) * (y - cy))) + 0.5f));
156 int xmin = cx - ry >= 0 ? cx - ry : 0;
157 int xmax = cx + ry <= size[0] - 1 ? cx + ry : size[0] - 1;
158 Images::RGBImage::Color* iPtr = imgBase + (y * stride + xmin);
159 for(int x = xmin; x <= xmax; ++x, ++iPtr)
160 *iPtr = color;
161 }
162 #else // Draw a hollow rain circle
163 Images::RGBImage::Color* imgCenter = imgBase + (cy * stride + cx);
164 for(int y = 0;; ++y) {
165 int x = int(Math::floor(Math::sqrt(float(r * r) - float(y * y)) + 0.5f));
166 if(x < y)
167 break;
168 if(cy + y < size[1]) {
169 if(cx + x < size[0])
170 imgCenter[y * stride + x] = color;
171 if(cx - x >= 0)
172 imgCenter[y * stride - x] = color;
173 }
174 if(cy + x < size[1]) {
175 if(cx + y < size[0])
176 imgCenter[x * stride + y] = color;
177 if(cx - y >= 0)
178 imgCenter[x * stride - y] = color;
179 }
180 if(cy - y >= 0) {
181 if(cx + x < size[0])
182 imgCenter[-y * stride + x] = color;
183 if(cx - x >= 0)
184 imgCenter[-y * stride - x] = color;
185 }
186 if(cy - x >= 0) {
187 if(cx + y < size[0])
188 imgCenter[-x * stride + y] = color;
189 if(cx - y >= 0)
190 imgCenter[-x * stride - y] = color;
191 }
192 }
193 #endif
194 }
195
196 }
197
198 /**************************************
199 Static elements of class HandExtractor:
200 **************************************/
201
202 const unsigned short HandExtractor::invalidBlobId = 0xffffU;
203 const int HandExtractor::walkDx[8] = { 1, 1, 0, -1, -1, -1, 0, 1};
204 const int HandExtractor::walkDy[8] = { 0, 1, 1, 1, 0, -1, -1, -1};
205
206 /******************************
207 Methods of class HandExtractor:
208 ******************************/
209
210 void* HandExtractor::extractorThreadMethod(void) {
211 unsigned int lastInputFrameVersion = 0;
212
213 while(true) {
214 Kinect::FrameBuffer frame;
215 {
216 Threads::MutexCond::Lock inputLock(inputCond);
217
218 /* Wait until a new frame arrives or the program shuts down: */
219 while(runExtractorThread && lastInputFrameVersion == inputFrameVersion)
220 inputCond.wait(inputLock);
221
222 /* Bail out if the program is shutting down: */
223 if(!runExtractorThread)
224 break;
225
226 /* Work on the new frame: */
227 frame = inputFrame;
228 lastInputFrameVersion = inputFrameVersion;
229 }
230
231 /* Prepare a new output hand list: */
232 HandList& newHandList = extractedHands.startNewValue();
233
234 /* Extract hands from the new input frame: */
235 extractHands(frame.getData<DepthPixel>(), newHandList, 0);
236
237 /* Finalize the new extracted hands list in the output buffer: */
238 extractedHands.postNewValue();
239
240 /* Pass the new output frame to the registered receiver: */
241 if(handsExtractedFunction != 0)
242 (*handsExtractedFunction)(newHandList);
243 }
244
245 return 0;
246 }
247
248 HandExtractor::HandExtractor(const unsigned int sDepthFrameSize[2],
249 const HandExtractor::PixelDepthCorrection* sPixelDepthCorrection,
250 const PTransform& sDepthProjection)
251 : pixelDepthCorrection(sPixelDepthCorrection), depthProjection(sDepthProjection),
252 inputFrameVersion(0), runExtractorThread(false),
253 maxFgDepth(0x07ffU - 1U), maxDepthDist(1), minBlobSize(1500), maxBlobSize(150000),
254 blobIdImage(0),
255 snakeLength(50), snake(0),
256 maxCornerEnterDist(28), minCenterDist(10), minCornerExitDist(32),
257 minHandProbability(0.15f),
258 handsExtractedFunction(0) {
259 /* Copy the depth frame size: */
260 for(int i = 0; i < 2; ++i)
261 depthFrameSize[i] = sDepthFrameSize[i];
262
263 /* Allocate the blob ID image: */
264 blobIdImage = new unsigned short[(depthFrameSize[1] + 2) * (depthFrameSize[0] + 2)];
265 biStride = depthFrameSize[0] + 2;
266
267 /* Initialize the border of the blob ID image: */
268 unsigned short* biPtr = blobIdImage;
269 for(unsigned int x = 1; x < depthFrameSize[0] + 2; ++x, ++biPtr)
270 *biPtr = invalidBlobId;
271 for(unsigned int y = 1; y < depthFrameSize[1] + 2; ++y, biPtr += biStride)
272 * biPtr = invalidBlobId;
273 for(unsigned int x = 1; x < depthFrameSize[0] + 2; ++x, --biPtr)
274 *biPtr = invalidBlobId;
275 for(unsigned int y = 1; y < depthFrameSize[1] + 2; ++y, biPtr -= biStride)
276 * biPtr = invalidBlobId;
277
278 /* Calculate the array of edge walking pointer offsets: */
279 for(int i = 0; i < 8; ++i)
280 walkOffsets[i] = walkDy[i] * biStride + walkDx[i];
281
282 /* Initialize the edge walking snake: */
283 setSnakeLength(snakeLength);
284
285 /* Start the hand extraction thread: */
286 runExtractorThread = true;
287 extractorThread.start(this, &HandExtractor::extractorThreadMethod);
288 }
289
290 HandExtractor::~HandExtractor(void) {
291 /* Shut down the extraction thread: */
292 {
293 Threads::MutexCond::Lock inputLock(inputCond);
294 runExtractorThread = false;
295 inputCond.signal();
296 }
297 extractorThread.join();
298
299 delete[] blobIdImage;
300 delete[] snake;
301 }
302
303 void HandExtractor::setMaxFgDepth(DepthPixel newMaxFgDepth) {
304 maxFgDepth = newMaxFgDepth;
305 }
306
307 void HandExtractor::setMaxDepthDist(unsigned int newMaxDepthDist) {
308 maxDepthDist = newMaxDepthDist;
309 }
310
311 void HandExtractor::setBlobSizeRange(unsigned int newMinBlobSize, unsigned int newMaxBlobSize) {
312 minBlobSize = newMinBlobSize;
313 maxBlobSize = newMaxBlobSize;
314 }
315
316 void HandExtractor::setSnakeLength(unsigned int newSnakeLength) {
317 snakeLength = newSnakeLength;
318
319 /* Re-allocate the snake array: */
320 delete[] snake;
321 snake = new EdgePixel[snakeLength];
322 }
323
324 void HandExtractor::setCornerDists(int newMaxCornerEnterDist, int newMinCenterDist,
325 int newMinCornerExitDist) {
326 maxCornerEnterDist = newMaxCornerEnterDist;
327 minCenterDist = newMinCenterDist;
328 minCornerExitDist = newMinCornerExitDist;
329 }
330
331 void HandExtractor::extractHands(const HandExtractor::DepthPixel* depthFrame,
332 HandExtractor::HandList& hands, Images::RGBImage* blobImage) {
333 Images::RGBImage::Color* imgPtr = 0;
334 if(blobImage != 0) {
335 /* Create the result image: */
336 blobImage->clear(Images::RGBImage::Color(0, 0, 0));
337 imgPtr = blobImage->replacePixels();
338 }
339
340 /* Extract all four-connected foreground blobs from the given depth frame: */
341 std::vector<Span> spans;
342 unsigned int numSpans = 0;
343 unsigned int lastRowSpan = 0;
344 const DepthPixel* dfRowPtr = depthFrame;
345 for(unsigned int y = 0; y < depthFrameSize[1]; ++y, dfRowPtr += depthFrameSize[0]) {
346 const DepthPixel* dfPtr = dfRowPtr;
347 unsigned int rowSpan = numSpans;
348 unsigned int x = 0;
349 while(true) {
350 /* Find the beginning of the next foreground span: */
351 for(; x < depthFrameSize[0] && *dfPtr > maxFgDepth; ++x, ++dfPtr)
352 ;
353 if(x >= depthFrameSize[0])
354 break;
355
356 /* Start a new foreground span: */
357 Span newSpan;
358 newSpan.y = y;
359 newSpan.start = x;
360
361 /* Trace out the current foreground span: */
362 DepthPixel lastDepth = *dfPtr;
363 ++x;
364 ++dfPtr;
365 for(; x < depthFrameSize[0] && *dfPtr <= maxFgDepth && *dfPtr + maxDepthDist >= lastDepth
366 && *dfPtr <= lastDepth + maxDepthDist; ++x, ++dfPtr)
367 lastDepth = *dfPtr;
368
369 /* Finalize and store the new foreground span: */
370 newSpan.end = x;
371 newSpan.parent = numSpans;
372 newSpan.numPixels = newSpan.end - newSpan.start;
373 newSpan.blobId = invalidBlobId;
374 spans.push_back(newSpan);
375 ++numSpans;
376
377 /* Skip any spans from the previous row that were just passed by: */
378 for(; lastRowSpan < rowSpan && spans[lastRowSpan].end < newSpan.start; ++lastRowSpan)
379 ;
380
381 /* Check if the current span links up with any from the previous row: */
382 for(unsigned int lrs = lastRowSpan; lrs < rowSpan && spans[lrs].start <= newSpan.end; ++lrs) {
383 /* Check if the two spans have depth in common: */
384 unsigned int o1 = Misc::max(newSpan.start, spans[lrs].start);
385 unsigned int o2 = Misc::min(newSpan.end, spans[lrs].end);
386 const DepthPixel* lrsPtr1 = dfRowPtr + o1;
387 const DepthPixel* lrsPtr0 = lrsPtr1 - depthFrameSize[0];
388 bool canLink = false;
389 for(unsigned int o = o1; o < o2 && !canLink; ++o, ++lrsPtr0, ++lrsPtr1)
390 canLink = *lrsPtr0 + maxDepthDist >= *lrsPtr1 && *lrsPtr0 <= *lrsPtr1 + maxDepthDist;
391
392 /* Merge the two spans if they can link: */
393 if(canLink) {
394 /* Find the roots of the two spans' respective subtrees: */
395 unsigned int root1 = lrs;
396 while(root1 != spans[root1].parent)
397 root1 = spans[root1].parent;
398 unsigned int root2 = numSpans - 1;
399 while(root2 != spans[root2].parent)
400 root2 = spans[root2].parent;
401
402 if(root1 < root2) {
403 /* Make the first span the new root: */
404 spans[root2].parent = root1;
405 spans[root1].numPixels += spans[root2].numPixels;
406 } else if(root1 > root2) {
407 /* Make the second span the new root: */
408 spans[root1].parent = root2;
409 spans[root2].numPixels += spans[root1].numPixels;
410 }
411 }
412 }
413 }
414
415 /* Skip any leftover spans from the previous row: */
416 lastRowSpan = rowSpan;
417 }
418
419 /* Assign consecutive blob IDs to all root spans: */
420 unsigned int nextBlobId = 0;
421 for(unsigned int i = 0; i < numSpans; ++i) {
422 /* Check if the span is a root span: */
423 if(spans[i].parent == i) {
424 if(spans[i].numPixels >= minBlobSize && spans[i].numPixels <= maxBlobSize) {
425 spans[i].blobId = nextBlobId;
426 ++nextBlobId;
427 }
428 } else {
429 /* Find the root of the span's subtree: */
430 unsigned int root = spans[i].parent;
431 while(root != spans[root].parent)
432 root = spans[root].parent;
433
434 /* Assign the span's blob ID from the root: */
435 spans[i].blobId = spans[root].blobId;
436 }
437 }
438
439 #if 0
440
441 /* Create the result color image: */
442 static const Images::RGBImage::Color blobColors[18] = {
443 Images::RGBImage::Color(255, 0, 0),
444 Images::RGBImage::Color(255, 255, 0),
445 Images::RGBImage::Color(0, 255, 0),
446 Images::RGBImage::Color(0, 255, 255),
447 Images::RGBImage::Color(0, 0, 255),
448 Images::RGBImage::Color(255, 0, 255),
449 Images::RGBImage::Color(128, 0, 0),
450 Images::RGBImage::Color(128, 128, 0),
451 Images::RGBImage::Color(0, 128, 0),
452 Images::RGBImage::Color(0, 128, 128),
453 Images::RGBImage::Color(0, 0, 128),
454 Images::RGBImage::Color(128, 0, 128),
455 Images::RGBImage::Color(255, 128, 128),
456 Images::RGBImage::Color(255, 255, 128),
457 Images::RGBImage::Color(128, 255, 128),
458 Images::RGBImage::Color(128, 255, 255),
459 Images::RGBImage::Color(128, 128, 255),
460 Images::RGBImage::Color(255, 128, 255)
461 };
462
463 for(unsigned int i = 0; i < numSpans; ++i) {
464 /* Find the span's root: */
465 int root = i;
466 while(spans[root].parent != root)
467 root = spans[root].parent;
468
469 if(spans[root].blobId != invalidBlobId) {
470 /* Fill in the span: */
471 Images::RGBImage::Color* cPtr = result.modifyPixelRow(spans[i].y) + spans[i].start;
472 for(int x = spans[i].start; x < spans[i].end; ++x, ++cPtr)
473 *cPtr = blobColors[spans[root].blobId % 18];
474 }
475 }
476
477 #endif
478
479 /* Create an array of blob origin points: */
480 BlobOrigin* blobOrigins = new BlobOrigin[nextBlobId];
481 for(unsigned int i = 0; i < nextBlobId; ++i)
482 blobOrigins[i].assigned = false;
483
484 /* Create the blob ID image: */
485 unsigned short* biRowPtr = blobIdImage + biStride + 1;
486 unsigned int spanIndex = 0;
487 for(unsigned int y = 0; y < depthFrameSize[1]; ++y, biRowPtr += biStride) {
488 /* Process all spans and spaces between spans in the current row: */
489 unsigned int x = 0;
490 unsigned short* biPtr = biRowPtr;
491 while(true) {
492 /* Find the start of the next span in the current row: */
493 unsigned int nextSpanStart = depthFrameSize[0];
494 if(spanIndex < numSpans && spans[spanIndex].y == y)
495 nextSpanStart = spans[spanIndex].start;
496
497 /* Assign the invalid blob IDs until the start of the next span: */
498 for(; x < nextSpanStart; ++x, ++biPtr)
499 *biPtr = invalidBlobId;
500
501 /* Bail out if the current row is done: */
502 if(x == depthFrameSize[0])
503 break;
504
505 /* Check if the current span's blob is valid, and encountered for the first time: */
506 unsigned int blobId = spans[spanIndex].blobId;
507 if(blobId < nextBlobId && !blobOrigins[blobId].assigned) {
508 /* Store the beginning of the current span as the blob's origin: */
509 blobOrigins[blobId].assigned = true;
510 blobOrigins[blobId].x = x;
511 blobOrigins[blobId].y = y;
512 blobOrigins[blobId].biPtr = biPtr;
513 }
514
515 /* Assign the current span's blob ID: */
516 for(; x < spans[spanIndex].end; ++x, ++biPtr)
517 *biPtr = blobId;
518
519 /* Go to the next span: */
520 ++spanIndex;
521 }
522 }
523
524 /* Initialize the result list: */
525 hands.clear();
526
527 /* Walk around the edges of all foreground blobs in counter-clockwise order and decide whether they are hand-shaped: */
528 EdgePixel* snakeEnd = snake + snakeLength;
529 int enterDist2 = Math::sqr(maxCornerEnterDist);
530 int centerDist2 = Math::sqr(minCenterDist);
531 int exitDist2 = Math::sqr(minCornerExitDist);
532 std::vector<Corner> corners;
533 corners.reserve(10);
534 for(unsigned int blobId = 0; blobId < nextBlobId; ++blobId) {
535 /* Initialize the edge-walking snake: */
536 EdgePixel* snakeHead = snake;
537 snakeHead->x = int(blobOrigins[blobId].x);
538 snakeHead->y = int(blobOrigins[blobId].y);
539 snakeHead->biPtr = blobOrigins[blobId].biPtr;
540 unsigned int walkDir =
541 0; // The blob origin is the bottom-left pixel of the blob, so 0 is the correct initial walking direction
542 for(unsigned int i = 1; i < snakeLength; ++i) {
543 /* Turn 90 degrees clockwise: */
544 walkDir = (walkDir + 6) & 0x7U;
545
546 /* Turn counter-clockwise until the next step stays in the same blob: */
547 while(snakeHead->biPtr[walkOffsets[walkDir]] != blobId)
548 walkDir = (walkDir + 1) & 0x7U;
549
550 /* Walk one step along the blob edge: */
551 snakeHead[1].x = snakeHead->x + walkDx[walkDir];
552 snakeHead[1].y = snakeHead->y + walkDy[walkDir];
553 snakeHead[1].biPtr = snakeHead->biPtr + walkOffsets[walkDir];
554
555 /* Move the snake head forward: */
556 ++snakeHead;
557 }
558 EdgePixel* snakeTail = snake;
559 EdgePixel* snakeMid = snake + snakeLength / 2;
560
561 /* Walk the snake exactly once around the blob: */
562 Corner corner;
563 corner.cornerType = 0;
564 int cornerDist2 = 0;
565 unsigned int pixelIndex = 0;
566 int firstCornerDist2 = 0;
567 unsigned int firstCornerStart = 0;
568 do {
569 /* Check if the current snake sits on a corner: */
570 int newCornerType = 0;
571 int headTailDist2 = Math::sqr(snakeHead->x - snakeTail->x) + Math::sqr(
572 snakeHead->y - snakeTail->y);
573 int centerElevation2 = 0;
574 if(headTailDist2 <= enterDist2) {
575 /* Determine the type of corner by comparing the snake's center point against the line defined by its head and tail: */
576 int nx = snakeTail->y - snakeHead->y;
577 int ny = snakeHead->x - snakeTail->x;
578 int d = nx * (snakeMid->x - snakeTail->x) + ny * (snakeMid->y - snakeTail->y);
579 if(Math::sqr(d) >= centerDist2 * headTailDist2) {
580 /* Enter corner state: */
581 if(d < 0)
582 newCornerType = 1; // Finger tip
583 else
584 newCornerType = -1; // Finger nook
585 if(headTailDist2 > 0)
586 centerElevation2 = Math::sqr(d) / headTailDist2;
587 else
588 centerElevation2 = Math::sqr(snakeMid->x - snakeTail->x) + Math::sqr(snakeMid->y - snakeTail->y);
589 }
590 }
591
592 /* Check if the snake changed corner type since the last step: */
593 if(corner.cornerType != newCornerType) {
594 if(corner.cornerType != 0) {
595 /* If the previous corner is the first, remember its corner distance: */
596 if(corners.empty())
597 firstCornerDist2 = cornerDist2;
598
599 /* Store the previous corner: */
600 corners.push_back(corner);
601 }
602
603 if(newCornerType != 0) {
604 /* Start a new corner: */
605 corner.start = pixelIndex;
606 corner.x = snakeMid->x;
607 corner.y = snakeMid->y;
608 cornerDist2 = centerElevation2;
609
610 /* If this is the first corner, remember its starting pixel: */
611 if(corners.empty())
612 firstCornerStart = pixelIndex;
613 }
614
615 /* Change the type of the current corner: */
616 corner.cornerType = newCornerType;
617 } else if(corner.cornerType != 0 && cornerDist2 < centerElevation2) {
618 /* Update the current corner: */
619 corner.x = snakeMid->x;
620 corner.y = snakeMid->y;
621 cornerDist2 = centerElevation2;
622 }
623
624 if(imgPtr != 0) {
625 /* Draw the snake's center point: */
626 Images::RGBImage::Color* cPtr = imgPtr + (snakeMid->y * depthFrameSize[0] + snakeMid->x);
627 if(corner.cornerType == 1)
628 *cPtr = Images::RGBImage::Color(96, 160, 96);
629 else if(corner.cornerType == -1)
630 *cPtr = Images::RGBImage::Color(160, 96, 160);
631 else {
632 #if 1
633 *cPtr = Images::RGBImage::Color(128, 128, 128);
634 #else
635 for(int i = 0; i < 3; ++i)
636 (*cPtr)[i] = (*cPtr)[i] + (255U - (*cPtr)[i]) / 2;
637 #endif
638 }
639 }
640
641 /* Walk one step along the blob edge: */
642 walkDir = (walkDir + 6) & 0x7U; // Turn 90 degrees counter-clockwise
643 while(snakeHead->biPtr[walkOffsets[walkDir]] != blobId)
644 walkDir = (walkDir + 1) & 0x7U;
645 snakeTail->x = snakeHead->x + walkDx[walkDir];
646 snakeTail->y = snakeHead->y + walkDy[walkDir];
647 snakeTail->biPtr = snakeHead->biPtr + walkOffsets[walkDir];
648
649 /* Move the snake head forward: */
650 snakeHead = snakeTail;
651 if(++snakeMid == snakeEnd)
652 snakeMid = snake;
653 if(++snakeTail == snakeEnd)
654 snakeTail = snake;
655
656 ++pixelIndex;
657 } while(snakeTail->biPtr != blobOrigins[blobId].biPtr);
658
659 if(corner.cornerType != 0) {
660 if(!corners.empty() && firstCornerStart == 0 && corners.front().cornerType == corner.cornerType) {
661 /* Merge the first and last corners: */
662 if(firstCornerDist2 < cornerDist2) {
663 corners.front().x = corner.x;
664 corners.front().y = corner.y;
665 }
666 } else {
667 /* Store the last corner: */
668 corners.push_back(corner);
669 }
670 }
671
672 if(imgPtr != 0) {
673 /* Draw all corners: */
674 for(std::vector<Corner>::iterator cIt = corners.begin(); cIt != corners.end(); ++cIt) {
675 Images::RGBImage::Color* cPtr = imgPtr + (cIt->y * depthFrameSize[0] + cIt->x);
676 if(cIt->cornerType == 1)
677 *cPtr = Images::RGBImage::Color(0, 255, 0);
678 else if(cIt->cornerType == -1)
679 *cPtr = Images::RGBImage::Color(255, 0, 255);
680 }
681 }
682
683 /* Check if the extracted set of corners matches a hand model: */
684 float maxProb = minHandProbability;
685 Point2 center = Point2::origin; // Hand center point
686 float depth = 0.0f; // Hand's average depth value
687 float radius = 0.0f; // Hand radius
688 size_t numCorners = corners.size();
689 int handType = 1; // default to rain hand
690 if(numCorners >=
691 8) { // At least four finger tips, three nooks, and a thumb tip (thumb nook optional)
692 for(size_t i = 0; i < numCorners; ++i) {
693 /* Check if the current corner starts a sequence of four tips interleaved with three nooks: */
694 Corner& t0 = corners[i];
695 Corner& n1 = corners[(i + 1) % numCorners];
696 Corner& t1 = corners[(i + 2) % numCorners];
697 Corner& n2 = corners[(i + 3) % numCorners];
698 Corner& t2 = corners[(i + 4) % numCorners];
699 Corner& n3 = corners[(i + 5) % numCorners];
700 Corner& t3 = corners[(i + 6) % numCorners];
701 if(t0.cornerType == 1 &&
702 n1.cornerType == -1 && t1.cornerType == 1 &&
703 n2.cornerType == -1 && t2.cornerType == 1 &&
704 n3.cornerType == -1 && t3.cornerType == 1) {
705 /* Construct a hand model: */
706 Point2 tp0(float(t0.x) + 0.5f, float(t0.y) + 0.5f);
707 Point2 np1(float(n1.x) + 0.5f, float(n1.y) + 0.5f);
708 Point2 tp1(float(t1.x) + 0.5f, float(t1.y) + 0.5f);
709 Point2 np2(float(n2.x) + 0.5f, float(n2.y) + 0.5f);
710 Point2 tp2(float(t2.x) + 0.5f, float(t2.y) + 0.5f);
711 Point2 np3(float(n3.x) + 0.5f, float(n3.y) + 0.5f);
712 Point2 tp3(float(t3.x) + 0.5f, float(t3.y) + 0.5f);
713
714 /* Calculate the range of finger tip distances: */
715 Interval tipDistance(Geometry::dist(tp0, tp1));
716 tipDistance.addValue(Geometry::dist(tp1, tp2));
717 tipDistance.addValue(Geometry::dist(tp2, tp3));
718
719 /* Calculate the range of finger nook distances: */
720 Interval nookDistance(Geometry::dist(np1, np2));
721 nookDistance.addValue(Geometry::dist(np2, np3));
722
723 /* Calculate finger root points: */
724 Vector2 curve = Geometry::mid(np1, np3) - np2;
725 Point2 rp0 = np1 + (np1 - np2) * 0.5f + curve;
726 Point2 rp1 = Geometry::mid(np1, np2);
727 Point2 rp2 = Geometry::mid(np2, np3);
728 Point2 rp3 = np3 + (np3 - np2) * 0.5f + curve;
729
730 /* Calculate the range of finger lengths: */
731 Interval fingerLength(Geometry::dist(tp0, rp0));
732 fingerLength.addValue(Geometry::dist(tp1, rp1));
733 fingerLength.addValue(Geometry::dist(tp2, rp2));
734 fingerLength.addValue(Geometry::dist(tp3, rp3));
735
736 /* Calculate the probability that this is a hand: */
737 float prob = 1.0f;
738 prob *= Math::sqr(tipDistance.getMin() / tipDistance.getMax());
739 prob *= nookDistance.getMin() / nookDistance.getMax();
740 prob *= fingerLength.getMin() / fingerLength.getMax();
741
742 if(maxProb < prob) {
743 /* Calculate finger length to nook distance ratio: */
744 float fdNdRatio = Math::mid(Geometry::dist(tp1, rp1), Geometry::dist(tp2,
745 rp2)) / Math::mid(Geometry::dist(np1, np2), Geometry::dist(np2, np3));
746
747 /* Calculate the hand center and radius: */
748 float centerOffset = 1.0f / fdNdRatio;
749 center = Geometry::mid(Geometry::mid(rp0 + (rp0 - tp0) * centerOffset,
750 rp1 + (rp1 - tp1) * centerOffset),
751 Geometry::mid(rp2 + (rp2 - tp2) * centerOffset, rp3 + (rp3 - tp3) * centerOffset));
752 center = Geometry::mid(rp1 + (rp1 - tp1) * centerOffset, rp2 + (rp2 - tp2) * centerOffset);
753 radius = (Geometry::dist(center, tp0) + Geometry::dist(center, tp1) + Geometry::dist(center,
754 tp2) + Geometry::dist(center, tp3)) * 0.25f;
755
756 /* Calculate the hand's average depth in depth-corrected depth image space: */
757 depth = 0.0f;
758 if(pixelDepthCorrection != 0) {
759 ptrdiff_t t0Off = t0.y * depthFrameSize[0] + t0.x;
760 depth += pixelDepthCorrection[t0Off].correct(float(depthFrame[t0Off]));
761 ptrdiff_t n1Off = n1.y * depthFrameSize[0] + n1.x;
762 depth += pixelDepthCorrection[n1Off].correct(float(depthFrame[n1Off]));
763 ptrdiff_t t1Off = t1.y * depthFrameSize[0] + t1.x;
764 depth += pixelDepthCorrection[t1Off].correct(float(depthFrame[t1Off]));
765 ptrdiff_t n2Off = n2.y * depthFrameSize[0] + n2.x;
766 depth += pixelDepthCorrection[n2Off].correct(float(depthFrame[n2Off]));
767 ptrdiff_t t2Off = t2.y * depthFrameSize[0] + t2.x;
768 depth += pixelDepthCorrection[t2Off].correct(float(depthFrame[t2Off]));
769 ptrdiff_t n3Off = n3.y * depthFrameSize[0] + n3.x;
770 depth += pixelDepthCorrection[n3Off].correct(float(depthFrame[n3Off]));
771 ptrdiff_t t3Off = t3.y * depthFrameSize[0] + t3.x;
772 depth += pixelDepthCorrection[t3Off].correct(float(depthFrame[t3Off]));
773 } else {
774 depth += float(depthFrame[t0.y * depthFrameSize[0] + t0.x]);
775 depth += float(depthFrame[n1.y * depthFrameSize[0] + n1.x]);
776 depth += float(depthFrame[t1.y * depthFrameSize[0] + t1.x]);
777 depth += float(depthFrame[n2.y * depthFrameSize[0] + n2.x]);
778 depth += float(depthFrame[t2.y * depthFrameSize[0] + t2.x]);
779 depth += float(depthFrame[n3.y * depthFrameSize[0] + n3.x]);
780 depth += float(depthFrame[t3.y * depthFrameSize[0] + t3.x]);
781 }
782 depth /= 7.0f;
783
784 maxProb = prob;
785
786 if(imgPtr != 0) {
787 /* Draw the hand: */
788 drawLine(*blobImage, tp0, rp0, Images::RGBImage::Color(255, 255, 255));
789 drawLine(*blobImage, tp1, rp1, Images::RGBImage::Color(255, 255, 255));
790 drawLine(*blobImage, tp2, rp2, Images::RGBImage::Color(255, 255, 255));
791 drawLine(*blobImage, tp3, rp3, Images::RGBImage::Color(255, 255, 255));
792 drawCircle(*blobImage, center, radius, Images::RGBImage::Color(255, 255, 255));
793 }
794 }
795 }
796 }
797 }
798
799 else if (numCorners == 3) { // Two tips and a nook make a V where the water drains away.
800 for (size_t i = 0; i < numCorners; ++i) {
801 /* Check if the current corner starts a sequence of two tips interleaved with one nook: */
802 Corner &t0 = corners[i];
803 Corner &n1 = corners[(i + 1) % numCorners];
804 Corner &t1 = corners[(i + 2) % numCorners];
805
806 if (t0.cornerType == 1 && n1.cornerType == -1 && t1.cornerType == 1) {
807 /* Construct a hand model: */
808 Point2 tp0(float(t0.x) + 0.5f, float(t0.y) + 0.5f);
809 Point2 np1(float(n1.x) + 0.5f, float(n1.y) + 0.5f);
810 Point2 tp1(float(t1.x) + 0.5f, float(t1.y) + 0.5f);
811
812 /* Calculate the lengths of three sides of the triangle. */
813 // nook to the two tips
814 Interval sideLengths(Geometry::dist(tp0, np1));
815 sideLengths.addValue(Geometry::dist(np1, tp1));
816 // imaginary line connecting two finger tips
817 sideLengths.addValue(Geometry::dist(tp0, tp1));
818
819 /* Calculate the probability that this is a drain hand. */
820 /* Look for a nearly (> 0.70 ratio between sides) equilateral triangle. */
821 float prob = sideLengths.getMin() / (sideLengths.getMax() * 4.66f);
822
823 if (maxProb < prob) {
824 /* Center drain tool on the nook. */
825 center = np1;
826 /* Enlarge radius by 1/3 to approximate a full hand (rain) circle size. */
827 radius = 1.33 * sideLengths.getMax();
828
829 /* Calculate the hand's average depth in depth-corrected depth image space: */
830 depth = 0.0f;
831 if (pixelDepthCorrection != 0) {
832 ptrdiff_t t0Off = t0.y * depthFrameSize[0] + t0.x;
833 depth += pixelDepthCorrection[t0Off].correct(float(depthFrame[t0Off]));
834
835 ptrdiff_t n1Off = n1.y * depthFrameSize[0] + n1.x;
836 depth += pixelDepthCorrection[n1Off].correct(float(depthFrame[n1Off]));
837
838 ptrdiff_t t1Off = t1.y * depthFrameSize[0] + t1.x;
839 depth += pixelDepthCorrection[t1Off].correct(float(depthFrame[t1Off]));
840 } else {
841 depth += float(depthFrame[t0.y * depthFrameSize[0] + t0.x]);
842 depth += float(depthFrame[n1.y * depthFrameSize[0] + n1.x]);
843 depth += float(depthFrame[t1.y * depthFrameSize[0] + t1.x]);
844 }
845 depth /= 3.0f;
846 maxProb = prob;
847 if (imgPtr != 0) {
848 /* Draw the hand: */
849 drawLine(*blobImage, tp0, np1, Images::RGBImage::Color(255, 255, 255));
850 drawLine(*blobImage, tp1, np1, Images::RGBImage::Color(255, 255, 255));
851 drawCircle(*blobImage, center, radius, Images::RGBImage::Color(255, 255, 255));
852 }
853 }
854 /* Evaporate at twice the rainfall rate for fast clean up of water. */
855 handType = -2;
856 }
857 }
858 }
859 /* Check if the blob matches a hand: */
860 if(maxProb > minHandProbability) {
861 // DEBUGGING
862 // std::cout<<"Hand in depth space: "<<center[0]<<", "<<center[1]<<", "<<depth<<", "<<radius<<std::endl;
863
864 /* Store the hand in camera space: */
865 Hand newHand;
866 newHand.center = depthProjection.transform(Point(center[0], center[1], depth));
867 newHand.radius = Geometry::dist(newHand.center, depthProjection.transform(Point(center[0] + radius,
868 center[1], depth)));
869 newHand.direction = handType;
870 hands.push_back(newHand);
871
872 // DEBUGGING
873 // std::cout<<"Hand in camera space: "<<newHand.center[0]<<", "<<newHand.center[1]<<", "<<newHand.center[2]<<", "<<newHand.radius<<", "<<newHand.direction<<std::endl;
874 }
875
876 /* Clean up: */
877 corners.clear();
878 }
879
880 /* Clean up: */
881 delete[] blobOrigins;
882 }
883
884 void HandExtractor::setHandsExtractedFunction(HandExtractor::HandsExtractedFunction*
885 newHandsExtractedFunction) {
886 delete handsExtractedFunction;
887 handsExtractedFunction = newHandsExtractedFunction;
888 }
889
890 void HandExtractor::receiveRawFrame(const Kinect::FrameBuffer& newFrame) {
891 Threads::MutexCond::Lock inputLock(inputCond);
892
893 /* Store the new buffer in the input buffer: */
894 inputFrame = newFrame;
895 ++inputFrameVersion;
896
897 /* Signal the background thread: */
898 inputCond.signal();
899 }
AR Sandbox