ContourLineExtractor.cpp

   1 /***********************************************************************
   2 ContourLineExtractor - Class to identify contour lines from a depth image.
   3 Copyright (c) 2015-2016 Oliver Kreylos
   4 Copyright (c) 2020 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 "ContourLineExtractor.h"
  24
  25 #include <Misc/Utility.h>
  26 #include <Misc/FunctionCalls.h>
  27 #include <Math/Math.h>
  28
  29 // DEBUGGING
  30 #include <iostream>
  31
  32 /*********************************************
  33 Static elements of class ContourLineExtractor:
  34 *********************************************/
  35
  36 const unsigned short ContourLineExtractor::invalidLineId = 0xffffU;
  37 const int ContourLineExtractor::walkDx[8] = { 1, 1, 0, -1, -1, -1, 0, 1};
  38 const int ContourLineExtractor::walkDy[8] = { 0, 1, 1, 1, 0, -1, -1, -1};
  39
  40 int once = 0;
  41
  42 /*************************************
  43 Methods of class ContourLineExtractor:
  44 *************************************/
  45
  46 void* ContourLineExtractor::extractorThreadMethod(void) {
  47     unsigned int lastInputFrameVersion = 0;
  48
  49     while(true) {
  50         Kinect::FrameBuffer frame;
  51         {
  52             Threads::MutexCond::Lock inputLock(inputCond);
  53
  54             /* Wait until a new frame arrives or the program shuts down: */
  55             while(runExtractorThread && lastInputFrameVersion == inputFrameVersion)
  56                 inputCond.wait(inputLock);
  57
  58             /* Bail out if the program is shutting down: */
  59             if(!runExtractorThread)
  60                 break;
  61
  62             /* Work on the new frame: */
  63             frame = inputFrame;
  64             lastInputFrameVersion = inputFrameVersion;
  65         }
  66
  67         /* Prepare a new output contour line list: */
  68         ContourLineList& newContourLineList = extractedContourLines.startNewValue();
  69
  70         /* Extract contour lines from the new input frame: */
  71         extractContourLines(frame.getData<DepthPixel>(), newContourLineList);
  72
  73         /* Finalize the new extracted contour lines list in the output buffer: */
  74         extractedContourLines.postNewValue();
  75
  76         /* Pass the new output frame to the registered receiver: */
  77         if(contourLinesExtractedFunction != 0)
  78             (*contourLinesExtractedFunction)(newContourLineList);
  79     }
  80
  81     return 0;
  82 }
  83
  84 ContourLineExtractor::ContourLineExtractor(const unsigned int sDepthFrameSize[2],
  85                              const ContourLineExtractor::PixelDepthCorrection* sPixelDepthCorrection,
  86                              const PTransform& sDepthProjection)
  87     : pixelDepthCorrection(sPixelDepthCorrection), depthProjection(sDepthProjection),
  88       inputFrameVersion(0), runExtractorThread(false),
  89       maxFgDepth(0x07ffU - 1U),
  90       contourLinesExtractedFunction(0) {
  91
  92     /* Copy the depth frame size: */
  93     for(int i = 0; i < 2; ++i)
  94         depthFrameSize[i] = sDepthFrameSize[i];
  95
  96     /* Start the hand extraction thread: */
  97     runExtractorThread = true;
  98     extractorThread.start(this, &ContourLineExtractor::extractorThreadMethod);
  99 }
 100
 101 ContourLineExtractor::~ContourLineExtractor(void) {
 102     /* Shut down the extraction thread: */
 103     {
 104         Threads::MutexCond::Lock inputLock(inputCond);
 105         runExtractorThread = false;
 106         inputCond.signal();
 107     }
 108     extractorThread.join();
 109 }
 110
 111 void ContourLineExtractor::setMaxFgDepth(DepthPixel newMaxFgDepth) {
 112     maxFgDepth = newMaxFgDepth;
 113 }
 114
 115 void ContourLineExtractor::extractContourLines(const ContourLineExtractor::DepthPixel* depthFrame,
 116                                                ContourLineExtractor::ContourLineList& lines) {
 117
 118 //     std::cerr << "scanning depth frame for contour lines." << std::endl;
 119
 120     /* Scan the depth image, setting all non-contour interval elevations to zero: */
 121     std::vector<unsigned int> correctedDepths;
 122     const DepthPixel* dfPtr = depthFrame;
 123     float depth = 0;
 124
 125     if (once > 90 && once < 92) {
 126         std::cout << std::endl;
 127         std::cout << "basePlane: (" << basePlane.getNormal()[0] << ", " << basePlane.getNormal()[1] << ", " << basePlane.getNormal()[2] << ") = " << basePlane.getOffset() << std::endl;
 128         std::cout << "basePlaneDicEq: (" << basePlaneDicEq[0] << ", " << basePlaneDicEq[1] << ", " << basePlaneDicEq[2] << ") = " << basePlaneDicEq[3] << std::endl;
 129         const int x = depthFrameSize[0];
 130         const int y = depthFrameSize[1];
 131
 132         unsigned int sealevel = floor(0x03ffU * basePlaneDicEq[3]);
 133         std::cout << "sealevel: " << sealevel << std::endl;
 134
 135 //         for (unsigned int i = 0; i < x * y; ++i, ++dfPtr) {
 136 //             if (*dfPtr > maxDepth)
 137 //                 continue;
 138 //
 139 //             if (*dfPtr > sealevel && *dfPtr < sealevel + 500) {
 140 //                 std::cout << i << ", " << float(depthFrame[i]) << ", " << depth << std::endl;
 141 //             }
 142
 143
 144 //             depth = float(0x03ffU - *dfPtr) * basePlaneDicEq[3];
 145
 146 //             depth = pixelDepthCorrection[i].correct(float(depthFrame[i])) * basePlaneDicEq[3] - sealevel;
 147 //             depth = basePlane.getOffset() * (pixelDepthCorrection[i].correct(float(depthFrame[i])) / 2047.0) - basePlane.getOffset();
 148
 149 //             std::cout << depth;
 150 //             if (i % x > 638)
 151 //                 std::cout << std::endl;
 152 //             else
 153 //                 std::cout << ", ";
 154
 155 //         if (*dfPtr < -10) {
 156 //                 correctedDepths.push_back(0);
 157 //             } else {
 158 //                 correctedDepths.push_back(*dfPtr);
 159 //             }
 160 //         }
 161
 162     }
 163     once += 1;
 164
 165 //     for(unsigned int y = 0; y < depthFrameSize[1]; ++y, dfRowPtr += depthFrameSize[0]) {
 166 //         const DepthPixel* dfPtr = dfRowPtr;
 167 //         unsigned int x = 0;
 168 //         while(true) {
 169 //
 170 //             DepthPixel lastDepth = *dfPtr;
 171 //             ++x;
 172 //             ++dfPtr;
 173 //             for(; x < depthFrameSize[0] && *dfPtr <= maxFgDepth; ++x, ++dfPtr)
 174 //                 lastDepth = *dfPtr;
 175 //
 176 //         }
 177 //     }
 178
 179     /* Initialize the result list: */
 180     lines.clear();
 181
 182     /* Trace contiguous, non-zero values to form lines: */
 183
 184
 185 //     // DEBUGGING
 186 //     // std::cout<<"Hand in depth space: "<<center[0]<<", "<<center[1]<<", "<<depth<<", "<<radius<<std::endl;
 187 //
 188 //     /* Store the hand in camera space: */
 189 //     ContourLine newLine;
 190 //
 191 //     newLine.origin = depthProjection.transform(Point(center[0], center[1], center[2]));
 192 //     newLine.points = Geometry::dist(newHand.center,
 193 //                                     depthProjection.transform(Point(center[0], center[1], center[2])));
 194 //
 195 //     lines.push_back(newLine);
 196 //
 197 //     // DEBUGGING
 198 //     // std::cout<<"Hand in camera space: "<<newHand.center[0]<<", "<<newHand.center[1]<<", "<<newHand.center[2]<<", "<<newHand.radius<<", "<<newHand.direction<<std::endl;
 199
 200 }
 201
 202 void ContourLineExtractor::setBasePlane(const Plane& newBasePlane) {
 203     /* Set the base plane: */
 204     basePlane = newBasePlane;
 205
 206     /* Transform the base plane to depth image space and into a GLSL-compatible format: */
 207     const PTransform::Matrix& dpm = depthProjection.getMatrix();
 208     const Plane::Vector& bpn = basePlane.getNormal();
 209     Scalar bpo = basePlane.getOffset();
 210     for(int i = 0; i < 4; ++i)
 211         basePlaneDicEq[i] = GLfloat(dpm(0, i) * bpn[0] + dpm(1, i) * bpn[1] + dpm(2, i) * bpn[2] - dpm(3, i) * bpo);
 212 }
 213
 214 void ContourLineExtractor::setContourLinesExtractedFunction(ContourLineExtractor::ContourLinesExtractedFunction* newContourLinesExtractedFunction) {
 215     delete contourLinesExtractedFunction;
 216     contourLinesExtractedFunction = newContourLinesExtractedFunction;
 217 }
 218
 219 void ContourLineExtractor::receiveRawFrame(const Kinect::FrameBuffer& newFrame) {
 220     Threads::MutexCond::Lock inputLock(inputCond);
 221
 222     /* Store the new buffer in the input buffer: */
 223     inputFrame = newFrame;
 224     ++inputFrameVersion;
 225
 226     /* Signal the background thread: */
 227     inputCond.signal();
 228 }