OpenNIUtils.h

Go to the documentation of this file.

/********************************************************************
**                Image Component Library (ICL)                    **
**                                                                 **
** Copyright (C) 2006-2013 CITEC, University of Bielefeld          **
**                         Neuroinformatics Group                  **
** Website: www.iclcv.org and                                      **
**          http://opensource.cit-ec.de/projects/icl               **
**                                                                 **
** File   : ICLIO/src/ICLIO/OpenNIUtils.h                          **
** Module : ICLIO                                                  **
** Authors: Viktor Richter                                         **
**                                                                 **
**                                                                 **
** GNU LESSER GENERAL PUBLIC LICENSE                               **
** This file may be used under the terms of the GNU Lesser General **
** Public License version 3.0 as published by the                  **
**                                                                 **
** Free Software Foundation and appearing in the file LICENSE.LGPL **
** included in the packaging of this file.  Please review the      **
** following information to ensure the license requirements will   **
** be met: http://www.gnu.org/licenses/lgpl-3.0.txt                **
**                                                                 **
** The development of this software was supported by the           **
** Excellence Cluster EXC 277 Cognitive Interaction Technology.    **
** The Excellence Cluster EXC 277 is a grant of the Deutsche       **
** Forschungsgemeinschaft (DFG) in the context of the German       **
** Excellence Initiative.                                          **
**                                                                 **
********************************************************************/

#pragma once

#include <ICLCore/ImgBase.h>
#include <ICLUtils/Mutex.h>
#include <ICLUtils/Thread.h>
#include <ICLUtils/Configurable.h>
#include <ICLCore/CCFunctions.h>

#include <ICLIO/OpenNIIncludes.h>

#include <map>
#include <limits>
#include <set>

namespace icl {
  namespace io{

    namespace icl_openni {

      template<class T>
      core::Img<T>* convertDepthImg(xn::DepthMetaData* src, core::Img<T>* dst){
        float max = 0;
        if (std::numeric_limits<T>::max() < src -> ZRes()){
          max = ((float) std::numeric_limits<T>::max()) / ((float) src -> ZRes());
        }

        dst -> setSize(utils::Size(src -> XRes(), src -> YRes()));
        T* data = dst -> getData(0);
        // draw DEPTH image
        const XnDepthPixel* pDepthRow = src -> Data();
        if(!max){
          for (unsigned int y = 0; y < src -> YRes(); ++y){
            for (unsigned int x = 0; x < src -> XRes(); ++x, ++pDepthRow, ++data){
              *data = *pDepthRow;
            }
          }
        } else {
          for (unsigned int y = 0; y < src -> YRes(); ++y){
            for (unsigned int x = 0; x < src -> XRes(); ++x, ++pDepthRow, ++data){
              *data = *pDepthRow * max;
            }
          }
        }
        return dst;
      }

      inline core::Img16s* convertIRImg(xn::IRMetaData* src, core::Img16s* dst){
        dst -> setSize(utils::Size(src -> XRes(), src -> YRes()));
        icl16s* data = dst -> getData(0);
        const XnIRPixel* pIRRow = src -> Data();
        // draw grayscale image
        for (unsigned int y = 0; y < src -> YRes(); ++y){
          for (unsigned int x = 0; x < src -> XRes(); ++x, ++pIRRow, ++data){
            *data = *pIRRow;
          }
        }
        return dst;
      }

      inline core::Img8u* convertRGBImg(xn::ImageMetaData* src, core::Img8u* dst){
        dst -> setSize(utils::Size(src -> XRes(), src -> YRes()));
        dst -> setFormat(core::formatRGB);
        // draw RGB image
        icl8u* rChannel = dst -> getData(0);
        icl8u* gChannel = dst -> getData(1);
        icl8u* bChannel = dst -> getData(2);
        const XnRGB24Pixel* rgbPixel = src -> RGB24Data();
        for (unsigned int y = 0; y < src -> YRes(); ++y){
          for (unsigned int x = 0; x < src -> XRes(); ++x, ++rgbPixel, ++rChannel,
               ++gChannel, ++bChannel)
          {
            *rChannel = rgbPixel -> nRed;
            *gChannel = rgbPixel -> nGreen;
            *bChannel = rgbPixel -> nBlue;
          }
        }
        return dst;
      }


      inline core::Img8u* convertYuv422Img(xn::ImageMetaData* src, core::Img8u* dst){
        dst -> setSize(utils::Size(src -> XRes(), src -> YRes()));
        dst -> setFormat(core::formatRGB);
        // draw RGB image
        icl8u* rChannel = dst -> getData(0);
        icl8u* gChannel = dst -> getData(1);
        icl8u* bChannel = dst -> getData(2);
        const XnYUV422DoublePixel* yuvPixel = src -> YUV422Data();
        icl32s r,g,b;
        for (int i = 0; i < dst-> getDim()/2 ; ++i){
          icl::core::cc_util_yuv_to_rgb(yuvPixel->nY1,yuvPixel->nU,yuvPixel->nV,r,g,b);
          *rChannel = r; ++rChannel;
          *gChannel = g; ++gChannel;
          *bChannel = b; ++bChannel;
          icl::core::cc_util_yuv_to_rgb(yuvPixel->nY2,yuvPixel->nU,yuvPixel->nV,r,g,b);
          *rChannel = r; ++rChannel;
          *gChannel = g; ++gChannel;
          *bChannel = b; ++bChannel;
          ++yuvPixel;
        }
        return dst;
      }

      inline core::Img8u* convertGrayScale8Img(xn::ImageMetaData* src, core::Img8u* dst){
        dst -> setSize(utils::Size(src -> XRes(), src -> YRes()));
        dst -> setFormat(core::formatGray);
        // draw RGB image
        icl8u* gChannel = dst -> getData(0);
        const XnGrayscale8Pixel* grayPixel = src -> Grayscale8Data();
        for (int i = 0; i < dst-> getDim() ; ++i){
          *gChannel = *grayPixel;
          ++gChannel; ++grayPixel;
        }
        return dst;
      }

      template<typename T>
      class ReadWriteBufferHandler {
        public:
          virtual T* initBuffer() = 0;
      };


      template<typename T>
      class ReadWriteBuffer {
        public:
          ReadWriteBuffer(ReadWriteBufferHandler<T>* buffer_handler)
            : m_Mutex(), m_Write(0), m_Next(1), m_Read(2)
          {
            utils::Mutex::Locker l(m_Mutex);
            m_BufferHandler = buffer_handler;
            m_Buffers[0] = m_BufferHandler -> initBuffer();
            m_Buffers[1] = m_BufferHandler -> initBuffer();
            m_Buffers[2] = m_BufferHandler -> initBuffer();
            m_ResetBuffers[0] = false;
            m_ResetBuffers[1] = false;
            m_ResetBuffers[2] = false;
          }

          ~ReadWriteBuffer(){
            utils::Mutex::Locker l(m_Mutex);
            ICL_DELETE(m_Buffers[0]);
            ICL_DELETE(m_Buffers[1]);
            ICL_DELETE(m_Buffers[2]);
          }


          T* getNextReadBuffer(){
            utils::Mutex::Locker l(m_Mutex);
            if(m_Avail){
              // new buffer is available.
              std::swap(m_Next, m_Read);
              m_Avail = false;
            }
            return m_Buffers[m_Read];
          }


          T* getNextReadBuffer(bool omit_double_frames=false,
                               int omit_max_wait_millis=1000,
                               int omit_sleep_micros=1000){
            T* tmp = NULL;
            utils::Time t = utils::Time::now();
            while (true){
              m_Mutex.lock();
              if(m_Avail){
                // new buffer is available.
                std::swap(m_Next, m_Read);
                m_Avail = false;
                tmp = m_Buffers[m_Read];
                m_Mutex.unlock();
                break;
              } else if(!omit_double_frames){
                tmp = m_Buffers[m_Read];
                m_Mutex.unlock();
                break;
              }
              m_Mutex.unlock();
              if(t.age().toMilliSeconds() > omit_max_wait_millis){
                break;
              }
              utils::Thread::usleep(omit_sleep_micros);
            }
            return tmp;
          }


          T* getNextWriteBuffer(){
            utils::Mutex::Locker l(m_Mutex);
            // swap write buffer and next buffer.
            std::swap(m_Next, m_Write);
            // new buffer is available for reading.
            m_Avail = true;
            // reset buffer when needed
            if(m_ResetBuffers[m_Write]){
              ICL_DELETE(m_Buffers[m_Write]);
              m_Buffers[m_Write] = m_BufferHandler -> initBuffer();
              m_ResetBuffers[m_Write] = false;
            }
            // return new write buffer.
            return m_Buffers[m_Write];
          }

          void setReset(){
            utils::Mutex::Locker l(m_Mutex);
            m_ResetBuffers[0] = true;
            m_ResetBuffers[1] = true;
            m_ResetBuffers[2] = true;
          }

          void switchHandler(ReadWriteBufferHandler<T>* new_handler){
            utils::Mutex::Locker l(m_Mutex);
            m_BufferHandler = new_handler;
            m_ResetBuffers[0] = true;
            m_ResetBuffers[1] = true;
            m_ResetBuffers[2] = true;
          }

          bool newAvailable(){
            utils::Mutex::Locker l(m_Mutex);
            return m_Avail;
          }

        private:
          ReadWriteBufferHandler<T>* m_BufferHandler;
          T*  m_Buffers[3];
          bool  m_ResetBuffers[3];
          utils::Mutex m_Mutex;
          int m_Write;
          int m_Next;
          int m_Read;
          bool m_Avail;
      };

      class OpenNIContext : public utils::Uncopyable {
        private:
          OpenNIContext();

          ~OpenNIContext();

          static OpenNIContext* getInst();

        public:

          static XnStatus waitAndUpdate();

          static XnStatus CreateProductionTree(xn::NodeInfo& Tree, xn::ProductionNode& node);

          static XnStatus EnumerateProductionTrees(XnProductionNodeType type,
                                                   const xn::Query* pQuery,
                                                   xn::NodeInfoList& TreesList,
                                                   xn::EnumerationErrors* pErrors = NULL);

          static XnStatus Create(xn::DepthGenerator* generator);

        private:
          utils::Mutex m_Lock;
          bool m_Initialized;
          xn::Context m_Context;
      };

      class MapGeneratorOptions : public utils::Configurable {
        public:
          MapGeneratorOptions(xn::MapGenerator* generator);

          void processPropertyChange(const utils::Configurable::Property &prop);

          void addGeneralIntProperty(const std::string name);

        private:
          xn::MapGenerator* m_Generator;
          std::vector<std::string> m_Capabilities;
          std::map<std::string, xn::ProductionNode> m_ProductionNodeMap;
      };

      class DepthGeneratorOptions : public MapGeneratorOptions {
        public:
          DepthGeneratorOptions(xn::DepthGenerator* generator);

          void processPropertyChange(const utils::Configurable::Property &prop);

        private:
          xn::DepthGenerator* m_DepthGenerator;
      };

      class ImageGeneratorOptions : public MapGeneratorOptions {
        public:
          ImageGeneratorOptions(xn::ImageGenerator* generator);

          void processPropertyChange(const utils::Configurable::Property &prop);

        private:
          xn::ImageGenerator* m_ImageGenerator;
      };

      class OpenNIMapGenerator : public ReadWriteBufferHandler<core::ImgBase> {
        public:

          enum Generators {
            RGB,
            DEPTH,
            IR,
            NOT_SPECIFIED = -1
          };

          virtual bool acquireImage(core::ImgBase* dest) = 0;
          virtual bool newFrameAvailable() = 0;
          virtual Generators getGeneratorType() = 0;
          virtual xn::MapGenerator* getMapGenerator() = 0;
          virtual core::ImgBase* initBuffer() = 0;
          virtual MapGeneratorOptions* getMapGeneratorOptions() = 0;


          static OpenNIMapGenerator* createGenerator(std::string id);
          static std::string getMapOutputModeInfo(xn::MapGenerator* gen);
          static std::string getCurrentMapOutputMode(xn::MapGenerator* gen);
      };

      class OpenNIDepthGenerator : public OpenNIMapGenerator {
        public:
          OpenNIDepthGenerator(int num);
          ~OpenNIDepthGenerator();

          bool acquireImage(core::ImgBase* dest);
          bool newFrameAvailable();
          Generators getGeneratorType();
          xn::MapGenerator* getMapGenerator();
          core::ImgBase* initBuffer();
          MapGeneratorOptions* getMapGeneratorOptions();

        private:
          xn::DepthGenerator* m_DepthGenerator;
          xn::DepthMetaData m_DepthMD;
          MapGeneratorOptions* m_Options;
          unsigned int m_FrameId;
      };

      class OpenNIRgbGenerator : public OpenNIMapGenerator {
        public:
          OpenNIRgbGenerator(int num);
          ~OpenNIRgbGenerator();

          bool acquireImage(core::ImgBase* dest);
          bool newFrameAvailable();
          Generators getGeneratorType();
          xn::MapGenerator* getMapGenerator();
          core::Img8u* initBuffer();
          MapGeneratorOptions* getMapGeneratorOptions();

        private:
          xn::NodeInfo* m_DeviceInfo = NULL;
          xn::ImageGenerator* m_RgbGenerator = NULL;
          unsigned int m_FrameId;

          xn::DepthGenerator* m_DepthGenerator = NULL;
          xn::IRGenerator* m_IrGenerator = NULL;
          xn::ImageMetaData m_RgbMD;
          MapGeneratorOptions* m_Options = NULL;
      };

      class OpenNIIRGenerator : public OpenNIMapGenerator {
        public:
          OpenNIIRGenerator(int num);
          ~OpenNIIRGenerator();

          bool acquireImage(core::ImgBase* dest);
          bool newFrameAvailable();
          Generators getGeneratorType();
          xn::MapGenerator* getMapGenerator();
          core::Img16s* initBuffer();
          MapGeneratorOptions* getMapGeneratorOptions();

        private:
          xn::NodeInfo* m_DeviceInfo;
          xn::IRGenerator* m_IrGenerator;
          xn::IRMetaData m_IrMD;
          MapGeneratorOptions* m_Options;
          unsigned int m_FrameId;
      };

    } // namespace icl_openni

  } // namespace io
} // namespace icl