v4l2_loopback_output.cpp

#include <opencv2/opencv.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <sstream>
#include <string>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <linux/videodev2.h>

int NumDisparities = 7; /**< Range of disparity */
int BlockSize = 3; /**< Size of the block window. Must be odd */
cv::Ptr<cv::StereoBM> sbm = cv::StereoBM::create(NumDisparities, BlockSize);

void on_trackbar_ndisparities(int value, void*) {
    sbm->setNumDisparities((value + 1)*16);
}
void on_trackbar_blockSize(int value, void*) {
    sbm->setBlockSize(value*2 + 5);
}

void createGUI() {
    char windowGUI[] = "gui";
    cv::namedWindow(windowGUI, CV_WINDOW_AUTOSIZE);
    cv::createTrackbar("NumDisparities", windowGUI, &NumDisparities, 16, on_trackbar_ndisparities);
    cv::createTrackbar("BlockSize", windowGUI, &BlockSize, 125, on_trackbar_blockSize);
}

int main(int, char** argv) {
    
    sbm->setNumDisparities(NumDisparities*16);
    sbm->setBlockSize(BlockSize*2 + 5);
    sbm->setPreFilterCap(61);
    sbm->setPreFilterSize(5);
    sbm->setTextureThreshold(500);
    sbm->setSpeckleWindowSize(0);
    sbm->setSpeckleRange(8);
    sbm->setMinDisparity(0);
    sbm->setUniquenessRatio(0);
    sbm->setDisp12MaxDiff(1);

    cv::VideoCapture cap1(0);
    cv::VideoCapture cap2(1);
    if (!cap1.isOpened() || !cap2.isOpened())
    {
        std::cout << "ERROR INITIALIZING VIDEO CAPTURE" << std::endl;
        return -1;
    }
    char windowName1[] = "Webcam Feed 1";
    cv::namedWindow(windowName1, CV_WINDOW_AUTOSIZE);
    char windowName2[] = "Webcam Feed 2";
    cv::namedWindow(windowName2, CV_WINDOW_AUTOSIZE); 
    char windowDisparity[] = "Disparity";
    cv::namedWindow(windowDisparity, CV_WINDOW_AUTOSIZE);
    
    createGUI();
    
    int width = 640;
    int height = 480;
    int vidsendsiz = 0;

    int v4l2lo = open("/dev/video22", O_WRONLY); 
    if(v4l2lo < 0) {
        std::cout << "Error opening v4l2l device: " << strerror(errno);
        exit(-2);
    }
    {
        struct v4l2_format v;
        int t;
        v.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
        t = ioctl(v4l2lo, VIDIOC_G_FMT, &v);
        if( t < 0 ) {
            exit(t);
        }
        v.fmt.pix.width = width;
        v.fmt.pix.height = height;
        v.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB24;
        vidsendsiz = width * height * 3;
        v.fmt.pix.sizeimage = vidsendsiz;
        t = ioctl(v4l2lo, VIDIOC_S_FMT, &v);
        if( t < 0 ) {
            exit(t);
        }
    }

    while (1) {
        cv::Mat frame1;
        cv::Mat frame2;
        
        if (!cap1.grab() || !cap2.grab())
        {
            std::cout << "ERROR READING FRAME FROM CAMERA FEED" << std::endl;
            break;
        }
        
        cap1.retrieve(frame1);
        cap2.retrieve(frame2);
        
        cv::Mat f1 = cv::Mat(frame1.rows, frame1.cols, CV_8UC1);
        cv::Mat f2 = cv::Mat(frame2.rows, frame2.cols, CV_8UC1);
        
        cv::cvtColor(frame1, f1, cv::COLOR_BGR2GRAY);
        cv::cvtColor(frame2, f2, cv::COLOR_BGR2GRAY);

        cv::Mat imgDisparity(frame1.rows, frame1.cols, CV_16S);
        
        sbm->compute(f1, f2, imgDisparity);
        
        cv::Mat imgDisparity2 = cv::Mat(imgDisparity.rows, imgDisparity.cols, CV_8UC1);
        cv::normalize(imgDisparity, imgDisparity2, 0, 255, CV_MINMAX, CV_8UC1);
        

        cv::imshow(windowName1, frame1);
        cv::imshow(windowName2, frame2);
        cv::imshow(windowDisparity, imgDisparity2);
        
        
        cv::Mat imgStream(frame1.rows, frame1.cols, CV_8UC3);
        cv::cvtColor(imgDisparity2, imgStream, CV_GRAY2RGB);
        std::cout << "frame: " << imgStream.rows << "x" << imgStream.cols << std::endl;
        
        int size = imgStream.total() * imgStream.elemSize();
        
        if (size != vidsendsiz) {
            std::cout << "size != vidsendsiz " << size << " / " << vidsendsiz << std::endl;
        }
        
        size_t written = write(v4l2lo, imgStream.data, size);
        if (written < 0) {
            std::cout << "Error writing v4l2l device";
            close(v4l2lo);
            return 1;
        }

        switch (cv::waitKey(10)) {
            case 27:
                return 0;
        }
    }

    return 0;
}