OpenNI2/Source/Drivers/Libfreenect2/LF2DepthStream.cpp at 0fd447718efbb16e3188877bf1814f74f5970fee · structureio/OpenNI2 · GitHub

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
#include "LF2DepthStream.h"
#include <XnOS.h>
#include "LF2Common.h"
#include <XnMath.h>

using namespace LF2;

int
LF2DepthStream::BuildFrame(libfreenect2::Frame* frame_in,OniFrame* frame_out)
{
  // 1 DepthPixel is 2 byte
  frame_out->dataSize = frame_in->width * frame_in->height * 2;

  frame_out->data = xnOSMalloc (frame_out->dataSize);

  const float* const in_array = (float*) frame_in->data;
  uint16_t* const out_array = (uint16_t*) frame_out->data;
  unsigned int t = 0;
  for (unsigned int y = 0; y < frame_in->height; y++)
	{
      for (unsigned int x = 0; x < frame_in->width; x++)
		{
          out_array[t] = in_array[t];
          ++t;
        }
    }


  frame_out->frameIndex = frame_in->sequence;
  frame_out->sensorType = ONI_SENSOR_DEPTH;

  frame_out->videoMode.pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM;
  frame_out->videoMode.resolutionX = frame_in->width;
  frame_out->videoMode.resolutionY = frame_in->height;
  frame_out->videoMode.fps = 30;

  frame_out->width = frame_in->width;
  frame_out->height = frame_in->height;

  frame_out->cropOriginX = frame_out->cropOriginY = 0;
  frame_out->croppingEnabled = FALSE;

  frame_out->sensorType = ONI_SENSOR_DEPTH;
  frame_out->stride = frame_in->width*sizeof(OniDepthPixel);
  frame_out->timestamp = frame_in->sequence*33369;

  return 1;
}

OniStatus
LF2DepthStream::getProperty(int propertyId, void* data, int* pDataSize)
{
  OniStatus status = ONI_STATUS_NOT_SUPPORTED;

  switch (propertyId)
    {
    case ONI_STREAM_PROPERTY_HORIZONTAL_FOV:
      {
        float* val = (float*)data;
        XnDouble tmp;
        tmp =  LF2_DEPTH_HORIZONTAL_FOV * xnl::Math::DTR;
        *val = (float)tmp;
        status = ONI_STATUS_OK;
        break;
      }
    case ONI_STREAM_PROPERTY_VERTICAL_FOV:
      {
        float* val = (float*)data;
        XnDouble tmp;
        tmp =  LF2_DEPTH_VERTICAL_FOV * xnl::Math::DTR;
        *val = (float)tmp;
        status = ONI_STATUS_OK;
        break;
      }
	case ONI_STREAM_PROPERTY_MAX_VALUE:
      if (*pDataSize != sizeof(int))
		{
          return ONI_STATUS_BAD_PARAMETER;
		}

      *(int*)data = 10000;
      return ONI_STATUS_OK;
	case ONI_STREAM_PROPERTY_MIN_VALUE:
      if (*pDataSize != sizeof(int))
		{
          return ONI_STATUS_BAD_PARAMETER;
		}

      *(int*)data = 0;
      return ONI_STATUS_OK;
    default:
      return LF2Stream::getProperty(propertyId, data, pDataSize);
    }

  return status;
}