libfreenect2/examples/kinect_webcam/Kinect_Webcam.cpp at b29b1c73191c41d4873a48cc463c6dba9cca537e · OpenKinect/libfreenect2 · 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
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
#include <iostream>
#include <cstdlib>
#include <signal.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/videodev2.h>
#include <errno.h>
#include <string.h>
#include <opencv2/opencv.hpp>
#include <libfreenect2/libfreenect2.hpp>
#include <libfreenect2/frame_listener_impl.h>
#include <libfreenect2/registration.h>
#include <libfreenect2/packet_pipeline.h>
#include <libfreenect2/logger.h>

bool protonect_shutdown = false;
int v4l2_fd = -1;

void sigint_handler(int s)
{
  protonect_shutdown = true;
}

bool init_v4l2_device(const char *device, int width, int height)
{
  v4l2_fd = open(device, O_WRONLY);
  if (v4l2_fd < 0)
  {
    std::cerr << "Error opening v4l2loopback device: " << strerror(errno) << std::endl;
    return false;
  }

  struct v4l2_format v4l2_fmt;
  memset(&v4l2_fmt, 0, sizeof(v4l2_fmt));
  v4l2_fmt.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
  v4l2_fmt.fmt.pix.width = width;
  v4l2_fmt.fmt.pix.height = height;
  v4l2_fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_BGR24;
  v4l2_fmt.fmt.pix.sizeimage = width * height * 3;
  v4l2_fmt.fmt.pix.field = V4L2_FIELD_NONE;

  if (ioctl(v4l2_fd, VIDIOC_S_FMT, &v4l2_fmt) < 0)
  {
    std::cerr << "Error setting v4l2 format: " << strerror(errno) << std::endl;
    close(v4l2_fd);
    v4l2_fd = -1;
    return false;
  }

  return true;
}

void write_frame_to_v4l2(unsigned char *rgb_data, int width, int height)
{
  if (v4l2_fd < 0)
    return;

  cv::Mat frame(height, width, CV_8UC4, rgb_data);
  cv::Mat bgr_frame;
  cv::cvtColor(frame, bgr_frame, cv::COLOR_BGRA2BGR);

  write(v4l2_fd, bgr_frame.data, width * height * 3);
}

int main(int argc, char *argv[])
{
  libfreenect2::Freenect2 freenect2;
  libfreenect2::Freenect2Device *dev = nullptr;
  libfreenect2::PacketPipeline *pipeline = nullptr;

  if (!init_v4l2_device("/dev/video10", 1920, 1080))
  {
    std::cerr << "Failed to initialize v4l2loopback device" << std::endl;
    return -1;
  }

  if (freenect2.enumerateDevices() == 0)
  {
    std::cout << "No device connected!" << std::endl;
    return -1;
  }

  std::string serial = freenect2.getDefaultDeviceSerialNumber();

  dev = freenect2.openDevice(serial);

  if (dev == nullptr)
  {
    std::cout << "Failure opening device!" << std::endl;
    return -1;
  }

  signal(SIGINT, sigint_handler);

  libfreenect2::SyncMultiFrameListener listener(libfreenect2::Frame::Color);
  libfreenect2::FrameMap frames;

  dev->setColorFrameListener(&listener);

  if (!dev->start())
    return -1;

  std::cout << "Device serial: " << dev->getSerialNumber() << std::endl;
  std::cout << "Device firmware: " << dev->getFirmwareVersion() << std::endl;

  while (!protonect_shutdown)
  {
    if (!listener.waitForNewFrame(frames, 10 * 1000)) // 10 seconds
    {
      std::cout << "Timeout!" << std::endl;
      return -1;
    }
    libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];

    write_frame_to_v4l2(rgb->data, rgb->width, rgb->height);

    listener.release(frames);
  }

  dev->stop();
  dev->close();
  close(v4l2_fd);
  return 0;
}