depth's player

play_depth.cpp

#include <OpenNI.h>
#include <opencv2/opencv.hpp>
#include <vector>
#include <stdexcept>

int main(int argc,char* argv[])
{
  try {
    openni::Status ret = openni::OpenNI::initialize();
    if ( ret != openni::STATUS_OK ) {
      throw std::runtime_error( "" );
    }

    openni::Device device;
    if (argc == 1)
      ret = device.open( "./depth.oni" );
    else
      ret = device.open( argv[1] );
    if ( ret != openni::STATUS_OK ) {
      throw std::runtime_error( "" );
    }

    openni::VideoStream depthStream;
    ret = depthStream.create( device, openni::SensorType::SENSOR_DEPTH );
    if ( ret != openni::STATUS_OK ) {
      throw std::runtime_error( "" );
    }
    depthStream.start();


    std::vector<openni::VideoStream*> streams;
    streams.push_back( &depthStream );

    cv::Mat depthImage;

    while ( 1 ) {
      int changedIndex;
      openni::OpenNI::waitForAnyStream( &streams[0], streams.size(), &changedIndex );
      if ( changedIndex == 0 ) {
        openni::VideoFrameRef depthFrame;
        depthStream.readFrame( &depthFrame );
        if ( depthFrame.isValid() ) {

          depthImage = cv::Mat( depthStream.getVideoMode().getResolutionY(),
                                depthStream.getVideoMode().getResolutionX(),
                                CV_16U, (char*)depthFrame.getData() );

          // 中心点の距離を表示する
          auto videoMode = depthStream.getVideoMode();

          // 0-10000mmまでのデータを0-255にする
          depthImage.convertTo( depthImage, CV_8U, 255.0 / 10000 );

          cv::imshow( "Depth Camera", depthImage);
        }
      }

      int key = cv::waitKey( 10 );
      if ( key == 'q' ) {
        break;
      }
    }
  }
  catch ( std::exception& ) {
    std::cout << openni::OpenNI::getExtendedError() << std::endl;
  }
}