Skip to content

Instantly share code, notes, and snippets.

@thearchitect
Last active June 20, 2022 13:45
Show Gist options
  • Save thearchitect/96ab846a2dae98329d1617e538fbca3c to your computer and use it in GitHub Desktop.
Save thearchitect/96ab846a2dae98329d1617e538fbca3c to your computer and use it in GitHub Desktop.
#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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment