v4l2を使いWebカメラから取り込み処理した画像がおかしい

Question

カメラパラメータや焦点距離を任意で調整したかったので,v4l2を介してWebカメラから画像を取り込み,OpenCVで処理を行いたいと思っています.

しかし,YUYVからBGRに変換した後の画像が下の様に下半分がG一色になります.
全て画像として表示されることが理想なのですが.

原因と対策を教示頂けると幸いです.

環境

OS	Ubuntu16.04LTS
カメラ	logicool C615
OpenCV	4.0.0
C++	C++14

画像

ソースコード

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

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

static void change_format(cv::Mat& src, cv::Mat& img){

	  unsigned char *yuyv = src.data;
    unsigned char *bgr = img.data;
    int z = 0;
 
    for (int yline=0; yline < src.rows; yline++){

        for (int x=0; x < src.cols; x++){
            int r, g, b;
            int y, u, v;
 
            if (!z)
              y = yuyv[0] << 8;
            else
              y = yuyv[2] << 8;
            u = yuyv[1] - 128;
            v = yuyv[3] - 128;
 
            r = (y + (359 * v)) >> 8;
            g = (y - (88 * u) - (183 * v)) >> 8;
            b = (y + (454 * u)) >> 8;
 
            *(bgr++) = (b > 255) ? 255 : ((b < 0) ? 0 : b);
            *(bgr++) = (g > 255) ? 255 : ((g < 0) ? 0 : g);
            *(bgr++) = (r > 255) ? 255 : ((r < 0) ? 0 : r);
 
            if (z++) {
                z = 0;
                yuyv += 4;
            }
        }
    }

}

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/video0", 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 = 640;
		fmt.fmt.pix.height = 480;
		//fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_SGRBG8;
		fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
		//fmt.fmt.pix.field = V4L2_FIELD_NONE;
		fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;

		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::Mat bayerRaw(480, 640, CV_8UC1);
	cv::Mat color(480, 640, 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, 640 * 480);
			//cv::cvtColor(bayerRaw, color, cv::COLOR_YUV2BGR_YUYV);
      change_format(bayerRaw, color);
		}

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

	}

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

	return 0;
}

Answer 1

よくわかってないけど回答します。

変換元データのYUYVというのは、「4バイトで2画素を表現している」ということで合ってますか？
だとすれば、

cv::Mat bayerRaw(480, 640, CV_8UC1);

memcpy(bayerRaw.data, buffer, 640 * 480);

上記2行は両方とも「1画素あたり1バイトで、(640×480)画素ぶんの大きさ」の扱いになっていて正しくないような気がします。