OpenCV 2.4 を用いた USB カメラの取り込み2

前回のエントリで OpenCV2.4 を使って USB カメラの取り込みを実施したけど, Pixel Format がうまくコントロールできなかったのでこのエントリでは V4L2 の API からカメラの設定を変更するコードを作る.

参考にしたのは下記のホームページ
Capture images using V4L2 on Linux — Jay Rambhia

完成形のコードは最後に乗せておくとして, V4L2 でビデオからデータをキャプチャするまでには大きく分けて下記の 10 個くらいのステップをふまないといけなかった.

1. Video デバイスをオープンする.

 当然のことながら,まずはデバイスファイルをオープンする必要がある.

コード該当部

  // 1. Open Video Device.
  int fd;
  fd = open("/dev/videoLeftImgSrc", O_RDWR, 0);
  if (fd == -1)
  {
    std::cout << "Failed to open video device." << std::endl;
    return 1;
  }
2. デバイス情報をデバイスから抜き出す.

 次に,どうやらデバイス情報をクエリするのがお作法となっているみたい.確かに,素性の分からないカメラを使うこともあるだろうから,クエリして事前情報を掴んで置くことは必要なのかも.ただ,クエリ情報をその後のコードで使わないなら, v4l2-ctl コマンドで調べるほうが早いかも.

コード該当部

  // 2. Querying video capabilities.
  struct v4l2_capability caps;
  memset(&caps, 0, sizeof(caps));
  if (-1 == xioctl(fd, VIDIOC_QUERYCAP, &caps))
  {
    std::cout << "Failed to query capabilities." << std::endl;
      return 1;
  }
  std::cout << "bus_info  : " << caps.bus_info << std::endl;
  std::cout << "card    : " << caps.card << std::endl;
  std::cout << "driver  : " << caps.driver << std::endl;
  std::cout << "version  : " << caps.version << std::endl;
3. Video に対して,フォーマットのリクエストをする.

 つぎに,やり取りしたいビデオのフォーマットをデバイスに対してリクエストする.今回使ったカメラだと,例えばピクセルフォーマットは下記の 3 つが対応可能であった.

  1. V4L2_PIX_FMT_SGRBG8
  2. V4L2_PIX_FMT_GREY
  3. V4L2_PIX_FMT_Y16

 今回はカラー画像を使いたかったので,ベイヤーパターンの V4L2_PIX_FMT_SGRBG8 を指定した.また,最大サイズ 1280 x 960 を指定.

コード該当部

  // 3. Format Specification.
  {
    struct v4l2_format fmt;
    memset(&(fmt), 0, sizeof(fmt));

    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width = 1280;
    fmt.fmt.pix.height = 960;
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_SGRBG8;
    //fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_GREY;
    fmt.fmt.pix.field = V4L2_FIELD_NONE;

    if (-1 == xioctl(fd, VIDIOC_S_FMT, &fmt))
    {
      std::cout << "Failed to set pixel format." << std::endl;
      return 1;
    }
  }
4. Video に対してバッファのリクエストをする.

 デバイス内部で保持されるバッファのリクエストをする.memory では,バッファの I/O を指定.count ではバッファ数, type は Stream/Buffer の種類を指定する...と書いてみたものの,このあたりの詳しい話は自分もちゃんとは理解できず...たぶん下記のままでそんなに問題無いかと.詳しい説明は下記をどーぞ!
ioctl VIDIOC_REQBUFS

コード該当部

  // 4. Request Buffer
  {
    struct v4l2_requestbuffers req;
    memset(&(req), 0, sizeof(req));
    req.count = 1;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;

    if (-1 == xioctl(fd, VIDIOC_REQBUFS, &req))
    {
      std::cout << "Failed to request buffer." << std::endl;
      return 1;
    }
  }
5. Video のバッファ情報を抜き出す.

 4 のステップでバッファリクエストをしたが,ここではそのリクエストに基づいて作成されたバッファの情報を抜き出す.今回のケースでは, 4 のステップで一つのバッファしかリクエストしなかったので index = 0 としているが,複数個のバッファをリクエストした場合,複数個のバッファに対して複数回クエリをする形になる.最後の mmap という関数がバッファのアドレスを返すので,そのアドレスを用意したアドレス変数 "buffer" に渡してやって,アプリの側ではこのポインタの先を見に行くことでキャプチャが見れるようになる.

コード該当部

  // 5. Query Buffer
  struct   v4l2_buffer buf;
  memset(&(buf), 0, sizeof(buf));
  {
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    //buf.index = bufferindex;
    buf.index = 0;
    if(-1 == xioctl(fd, VIDIOC_QUERYBUF, &buf))
    {
      std::cout << "Failed to query buffer." << std::endl;
      return 1;
    }

    std::cout << "buf.length : " << buf.length << std::endl;
    std::cout << "buf.m.offset : " << buf.m.offset << std::endl;

    buffer = (unsigned char*)mmap (NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
  }
6. Streaming を開始する.

 ついに Streaming を開始できるとこまで来た!やった!

コード該当部

  // 6. Start Streaming
  {
    if(-1 == xioctl(fd, VIDIOC_STREAMON, &buf.type))
    {
      std::cout << "Start Capture" << std::endl;
      return 1;
    }
  }
7. Buffer をキューにつなぐ, 8. Buffer をキューから外す

 下記, while ループが閉じてない中途半端なコードの一部分になってしまったが,,, 6 のステップで Streaming を開始したが,実際にはバッファを渡してあげないとカメラからデータを取ることはできない.流れとしては下記のようになると思う.

  1. VIDIOC_QBUF で使用可能なバッファをカメラに伝えてあげる.(ストリームを開始した直後はカメラが使用可能なバッファがひとつもない状態なので, while 文の上で index=0 を指定してその後の VIDIOC_QBUF でこのバッファが使用可能であることを伝えている.複数個のバッファをリクエストした場合は,最初のステップとして利用可能なすべてのバッファをカメラに伝える必要がある.)
  2. VIDIOC_DQBUF でカメラがデータを詰めたバッファを取得する.この時,buf.index にはカメラによって該当の index 番号が渡されているので,これを辿ってデータを取得する.
  3. アプリ側でデータを吸い上げ完了したら,不要になったバッファ (index = buf.index) をカメラに返してあげる.これには再び VIDIOC_QBUF を用いる.

コード該当部

struct 	v4l2_buffer buf;
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = 0;
while (true) {
  // 7. Capture Image
  {
    // Connect buffer to queue for next capture.
    if (-1 == xioctl(fd, VIDIOC_QBUF, &buf)) {
      std::cout << "VIDIOC_QBUF" << std::endl;
    }

    fd_set fds;
    FD_ZERO(&fds);
    FD_SET(fd, &fds);
    struct timeval tv = {0};
    tv.tv_sec = 2;
    int r = select(fd+1, &fds, NULL, NULL, &tv);

    if(-1 == r)
    {
      std::cout << "Waiting for Frame" << std::endl;
      return 1;
    }

    memset(&(buf), 0, sizeof(buf));
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;

    if(-1 == xioctl(fd, VIDIOC_DQBUF, &buf))
    {
      std::cout << "Retrieving Frame" << std::endl;
      return 1;
    }
  }

....
8. 適切なイメージフォーマットに変換する.

 デバイスからのデータは 8bit Bayer Pattern なので,OpenCV 提供の cvColor 関数を使って RGB 情報に変換する.

コード該当部

// 8. Store Image in OpenCV Data Type
{
  memcpy(bayerRaw.data, buffer, 1280 * 960);
  cv::cvtColor(bayerRaw, color, CV_BayerGB2BGR);
}
9. 表示!

 ここは説明不要かと.

コード該当部

// 9. Display Image
{
  cv::imshow("edges", color);
  if (-1 != cv::waitKey(1000)) {
    break;
  }
}
10. Streaming を終了する.

 最後にカメラにストリーミングの終了を宣言して,終了!

コード該当部

// 10. Turn off streaming.
if (-1 == xioctl(fd, VIDIOC_STREAMOFF, &buf.type)) {
  std::cout << "VIDIOC_STREAMOFF" << std::endl;
}


とりあえず作成したコードは下記の通り.実行するところまではできたので,雛形としては使ってもらえるかも.

#include <fcntl.h>
#include <errno.h>
#include <sys/mman.h>
#include <sys/ioctl.h>

#include <linux/videodev2.h>
#include "opencv2/opencv.hpp"

static int xioctl(int fd, int request, void *arg)
{
    int r;
	do {
		r = ioctl (fd, request, arg);
		if (request == VIDIOC_DQBUF) {
			std::cout << "r : " << r << std::endl;
		}
	} while (-1 == r && EINTR == errno);
	return r;
}

int main() {

	unsigned char *buffer;

	// 1. Open Video Device.
	int fd;
	fd = open("/dev/videoLeftImgSrc", O_RDWR, 0);
	if (fd == -1)
	{
	    std::cout << "Failed to open video device." << std::endl;
	    return 1;
	}

	// 2. Querying video capabilities.
	struct v4l2_capability caps;
	memset(&caps, 0, sizeof(caps));
	if (-1 == xioctl(fd, VIDIOC_QUERYCAP, &caps))
	{
		std::cout << "Failed to query capabilities." << std::endl;
	    return 1;
	}
	std::cout << "bus_info	: " << caps.bus_info << std::endl;
	std::cout << "card		: " << caps.card << std::endl;
	std::cout << "driver	: " << caps.driver << std::endl;
	std::cout << "version	: " << caps.version << std::endl;

	// 3. Format Specification.
	{
		struct v4l2_format fmt;
		memset(&(fmt), 0, sizeof(fmt));

		fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		fmt.fmt.pix.width = 1280;
		fmt.fmt.pix.height = 960;
		fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_SGRBG8;
		//fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_GREY;
		fmt.fmt.pix.field = V4L2_FIELD_NONE;

		if (-1 == xioctl(fd, VIDIOC_S_FMT, &fmt))
		{
			std::cout << "Failed to set pixel format." << std::endl;
			return 1;
		}
	}

	// 4. Request Buffer
	{
		struct v4l2_requestbuffers req;
		memset(&(req), 0, sizeof(req));
		req.count = 1;
		req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		req.memory = V4L2_MEMORY_MMAP;

		if (-1 == xioctl(fd, VIDIOC_REQBUFS, &req))
		{
			std::cout << "Failed to request buffer." << std::endl;
			return 1;
		}
	}

	// 5. Query Buffer
	{
		struct 	v4l2_buffer buf;
		memset(&(buf), 0, sizeof(buf));
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory = V4L2_MEMORY_MMAP;
		//buf.index = bufferindex;
		buf.index = 0;
		if(-1 == xioctl(fd, VIDIOC_QUERYBUF, &buf))
		{
			std::cout << "Failed to query buffer." << std::endl;
			return 1;
		}

		std::cout << "buf.length : " << buf.length << std::endl;
		std::cout << "buf.m.offset : " << buf.m.offset << std::endl;

		buffer = (unsigned char*)mmap (NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
	}

	// 6. Start Streaming
	{
		struct 	v4l2_buffer buf;
		memset(&(buf), 0, sizeof(buf));
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		if(-1 == xioctl(fd, VIDIOC_STREAMON, &buf.type))
		{
			std::cout << "Start Capture" << std::endl;
			return 1;
		}
	}

	cv::namedWindow("edges",1);
	cv::Mat bayerRaw(960, 1280, CV_8UC1);
	cv::Mat color(960, 1, CV_8UC3);
	struct 	v4l2_buffer buf;
	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	buf.memory = V4L2_MEMORY_MMAP;
	buf.index = 0;
	while (true) {
		// 7. Capture Image
		{
			// Connect buffer to queue for next capture.
			if (-1 == xioctl(fd, VIDIOC_QBUF, &buf)) {
				std::cout << "VIDIOC_QBUF" << std::endl;
			}

			fd_set fds;
			FD_ZERO(&fds);
			FD_SET(fd, &fds);
			struct timeval tv = {0};
			tv.tv_sec = 2;
			int r = select(fd+1, &fds, NULL, NULL, &tv);

			if(-1 == r)
			{
				std::cout << "Waiting for Frame" << std::endl;
				return 1;
			}

			memset(&(buf), 0, sizeof(buf));
			buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
			buf.memory = V4L2_MEMORY_MMAP;

			if(-1 == xioctl(fd, VIDIOC_DQBUF, &buf))
			{
				std::cout << "Retrieving Frame" << std::endl;
				return 1;
			}

		}

		// 8. Store Image in OpenCV Data Type
		{
			memcpy(bayerRaw.data, buffer, 1280 * 960);
			cv::cvtColor(bayerRaw, color, CV_BayerGB2BGR);
		}

		// 9. Display Image
		{
			cv::imshow("edges", color);
			if (-1 != cv::waitKey(1000)) {
				break;
			}
		}

	}

	// 10. Turn off streaming.
	if (-1 == xioctl(fd, VIDIOC_STREAMOFF, &buf.type)) {
		std::cout << "VIDIOC_STREAMOFF" << std::endl;
	}

	return 0;
}

下記の写真が実際に上記プログラムを実行してとった写真.(洗濯物とか写ってて,生活感出ててすみません(笑))
とりあえず,ようやくカメラとOpenCVがつながったということで,バンザイ!
f:id:rkoichi2001:20160525025235p:plain