/*
* Class —— StereoCamera
* 方法：
* 初始化双目相机参数
* 创建视频读取到队列容器线程
* 获取队头图像 —— 如果为空自动等待
* 获取队头时间帧 —— 如果为空自动等待
* pop, empty
* 特点：
* 线程安全，具有锁
*
**/
StereoCamera.hpp
#ifndef STEREOCAMERA_HPP
#define STEREOCAMERA_HPP
#include <opencv2/opencv.hpp>
#include <queue>
#include <thread>
/*
* 定义一些输入量
* FramInputPerSecond 输入源视频获取帧数
* FramInputInterval 转化成时间间隔
* TimeoutThreshold 帧过期丢弃阈值
*/
#define FramInputPerSecond 15
#define FramInputInterval 1000/FramInputPerSecond
#define FramWaitThreshold 40
#define TimeoutThreshold 50
class _fram;
class StereoCamera
{
public:
StereoCamera(int videoDeviceID);
~StereoCamera();
//init and calculate parameters;
void getCameraReady();
//begin get fram from camera and save to FramQueue
void getFram();
//work in a new thread
void getFramStart();
//get the first piece of fram (by reference) from FramQueue
std::pair<cv::Mat, cv::Mat>& firstFrams();
//get the time point of the first piece of fram (by reference) from FramQueue
std::chrono::system_clock::time_point& firstTp();
//pop the first piece of fram from FramQueue
void pop();
//reture 1 if FramQueue is empty
bool empty();
private:
cv::VideoCapture v;
cv::Mat inMatrixL;
cv::Mat inMatrixR;
cv::Mat distCoeffsL;
cv::Mat distCoeffsR;
cv::Mat rotation;
cv::Mat translation;
cv::Mat rectifyRotationL;
cv::Mat rectifyRotationR;
cv::Mat rectifyProjectionL;
cv::Mat rectifyProjectionR;
cv::Mat Q; //视差深度映射矩阵
cv::Mat rectifyStereoCameraMapR2;
cv::Mat rectifyMapL1;
cv::Mat rectifyMapL2;
cv::Mat rectifyMapR1;
cv::Mat rectifyMapR2;
cv::Size imageSize;
std::queue<_fram> FramBuffer;
bool WorkStatus;
std::mutex lock;
std::thread* T_getFram;
};
class _fram
{
public:
_fram(cv::Mat matL, cv::Mat matR, std::chrono::system_clock::time_point tp) : framContent(matL, matR), tp(tp){};
~_fram(){};
std::pair<cv::Mat, cv::Mat> framContent;
std::chrono::system_clock::time_point tp;
};
#endif
StereoCamera.cc
#include "./include/StereoCamera.hpp"
#include <opencv2/opencv.hpp>
#include <thread>
#include <chrono>
//init Camera parameters
StereoCamera::StereoCamera(int videoDeviceID)
{
//open camera through gstreamer
std::string gstString = "v4l2src device=/dev/video"+ std::to_string(videoDeviceID) + " ! image/jpeg,width=1280,height=480 ! jpegdec ! appsink";
v.open(gstString ,cv::CAP_GSTREAMER);
if (!v.isOpened()) {
std::cerr << "Error: Failed to open camera" << std::endl;
exit(-1);
}
inMatrixL = (cv::Mat_<double>(3, 3) <<
508.025383095559, -4.76577646636038, 298.758408695013,
0, 516.367682321281, 269.943747097106,
0.0, 0.0, 1.0);
inMatrixR = (cv::Mat_<double>(3, 3) <<
508.010513516867, -4.25470144125126, 316.923841134557,
0, 514.076781977890, 268.517991005795,
0.0, 0.0, 1.0);
distCoeffsL = (cv::Mat_<double>(1, 5) <<
-0.0467613211617264, 0.00456959237953496, 0.00599468139779429, 0.00562291627888502, 0);
distCoeffsR = (cv::Mat_<double>(1, 5) <<
-0.0348340455225505, 0.0319726382145138, 0.00730631911282040, 0.00880828926688701, 0);;
rotation = (cv::Mat_<double>(3, 3) <<
0.999385581703916, -0.000462918384616362, -0.0350463234718745,
0.000595261372085020, 0.999992731840344, 0.00376589038394706,
0.0350443254497070, -0.00378443827459052, 0.999378593567383);
translation = (cv::Mat_<double>(3, 1) <<
-61.4675664088485, 0.340648089329872, -3.97468175020695);
imageSize = cv::Size(640, 480);
getCameraReady();
}
void StereoCamera::getCameraReady()
{
//立体校正——获取每个摄像头的映射矩阵
stereoRectify(
inMatrixL, distCoeffsL,
inMatrixR, distCoeffsR,
imageSize, rotation, translation,
rectifyRotationL, rectifyRotationR,
rectifyProjectionL, rectifyProjectionR,
Q);
//计算立体校正映射表
initUndistortRectifyMap(
inMatrixL, distCoeffsL,
rectifyRotationL, rectifyProjectionL,
imageSize, CV_16SC2,
rectifyMapL1, rectifyMapL2);
initUndistortRectifyMap(
inMatrixR, distCoeffsR,
rectifyRotationR, rectifyProjectionR,
imageSize, CV_16SC2,
rectifyMapR1, rectifyMapR2);
}
void StereoCamera::getFram()
{
WorkStatus = 1;
while(WorkStatus){
std::chrono::system_clock::time_point t1 = std::chrono::system_clock::now();
cv::Mat framL;
cv::Mat framR;
cv::Mat framTmp;
v.read(framTmp);
framL = framTmp(cv::Rect(0, 0, 640, 480));
framR = framTmp(cv::Rect(640, 0, 640, 480));
cvtColor(framL, framL, cv::COLOR_BGR2GRAY);
cvtColor(framR, framR, cv::COLOR_BGR2GRAY);
remap(framL, framL, rectifyMapL1, rectifyMapL2, cv::INTER_LINEAR);
remap(framR, framR, rectifyMapR1, rectifyMapR2, cv::INTER_LINEAR);
lock.lock();
FramBuffer.emplace(framL, framR, std::chrono::system_clock::now());
lock.unlock();
std::chrono::system_clock::time_point t2 = std::chrono::system_clock::now();
double imageTransCost = std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count();
//std::cout<<"Image Transform Cost Time: "<< imageTransCost <<std::endl;
//精打细算 细节！
if (imageTransCost > FramInputInterval)
std::cout<<" WARNING: Image Transform Cost Too Much Time! "<< imageTransCost <<std::endl;
else
std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(std::round(FramInputInterval - imageTransCost))));
}
}
void StereoCamera::getFramStart()
{
T_getFram = new std::thread(&StereoCamera::getFram, this);
}
std::pair<cv::Mat, cv::Mat>& StereoCamera::firstFrams()
{
while (empty())
{
std::this_thread::sleep_for(std::chrono::milliseconds(FramWaitThreshold));
std::cout<<"wait for image"<<std::endl;
}
std::lock_guard<std::mutex> lg(lock);
return FramBuffer.front().framContent;
}
std::chrono::system_clock::time_point& StereoCamera::firstTp()
{
while (empty())
{
std::this_thread::sleep_for(std::chrono::milliseconds(FramWaitThreshold));
std::cout<<"wait for image"<<std::endl;
}
std::lock_guard<std::mutex> lg(lock);
return FramBuffer.front().tp;
}
void StereoCamera::pop()
{
std::lock_guard<std::mutex> lg(lock);
if (!FramBuffer.empty())
FramBuffer.pop();
}
bool StereoCamera::empty()
{
std::lock_guard<std::mutex> lg(lock);
return FramBuffer.empty();
}
StereoCamera::~StereoCamera()
{
WorkStatus = 0;
if (!lock.try_lock())
lock.unlock();
T_getFram->join();
v.release();
}
