Добрый день!
Сейчас пытаюсь определить расстояние до калибровочной доски (расстояние примерно 1.6 метра, размер клетки 28 мм).
Левая камера
Правая камера
Примерное расположение доски при калибровке. Расстояние рассчитано автоматически в Matlab Stereo Camera Calibrator App.
Первый алгоритм
#include <iostream>
#include <vector>
#include <stdint.h>
#include <string>
#include <fstream>
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/core/utility.hpp"
#include <opencv2/calib3d.hpp>
#include "opencv2/ximgproc.hpp"
#include <stdio.h>
using namespace cv;
using namespace cv::ximgproc;
using namespace std;
int main(int argc, char *argv[])
{
cv::Mat Image_Left = cv::imread("left_3.png");
cv::Mat Image_Right = cv::imread("right_3.png");
cv::Mat M1 = (cv::Mat_<double>(3, 3) << 7986.2559620260, 0.0000000000, 524.7794558939,
0.0000000000, 7603.3099586941, 765.6466654278,
0.0000000000, 0.0000000000, 1.0000000000);
cv::Mat D1 = (cv::Mat_<double>(1, 5) << -0.0626244338, -23.0384673454, 0.0000000000, 0.0000000000, 0.0000000000);
cv::Mat M2 = (cv::Mat_<double>(3, 3) << 7789.0835738047, 0.0000000000, 2085.4988788672,
0.0000000000, 7523.3700278846, 271.1883527428,
0.0000000000, 0.0000000000, 1.0000000000);
cv::Mat D2 = (cv::Mat_<double>(1, 5) << 0.2407018952, -7.8745465803, 0.0000000000, 0.0000000000, 0.0000000000);
cv::Mat Q = (cv::Mat_<double>(4, 4) << 1, 0, 0, -2.8951307125091553e+03,
0, 1, 0, -5.0271924400329590e+02,
0, 0, 0, 7.5633399932893499e+03,
0, 0, 2.0857193103842788e+00, 0);
cv::Rect RectLeft=cv::Rect(1080,890, 300,290); //Доска на левом фото
cv::Rect RectRight=cv::Rect(880, 890,300,290); //Доска на правом фото
cv::Point3f P3f(RectLeft.x, RectRight.y, abs(RectLeft.x - RectRight.x));
std::vector<cv::Point3f> input;
std::vector<cv::Point3f> output;
input.push_back(P3f);
cv::perspectiveTransform(input, output, Q);
std::cout << output[0] <<std::endl;
std::cin.get();
return 0;
}
Результат:
XYZ: [-4.35133, 0.928411, 18.1313]
Второй алгоритм:
Расстояние рассчитываю по формуле:
Расстояние = (Расстояние между камерами (мм) × фокусное расстояние) / карта несоответствия (disparity)
Фокусное расстояние = Q[2][3]
Расстояние между камерами (мм) = 1/Q[3][2] =0,5 м
Для расчёта disparity (в том числе отфильтрованной) использую пример:
https://docs.opencv.org/master/d3/d14/tutorial_ximgproc_disparity_filtering.html
Код:
#include <iostream>
#include <vector>
#include <stdint.h>
#include <string>
#include <fstream>
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/core/utility.hpp"
#include <opencv2/calib3d.hpp>
#include "opencv2/ximgproc.hpp"
#include <stdio.h>
using namespace cv;
using namespace cv::ximgproc;
using namespace std;
//https://docs.opencv.org/master/d3/d14/tutorial_ximgproc_disparity_filtering.html
int main(int argc, char** argv)
{
cv::Mat M1 = (cv::Mat_<double>(3, 3) << 7986.2559620260, 0.0000000000, 524.7794558939,
0.0000000000, 7603.3099586941, 765.6466654278,
0.0000000000, 0.0000000000, 1.0000000000);
cv::Mat D1 = (cv::Mat_<double>(1, 5) << -0.0626244338, -23.0384673454, 0.0000000000, 0.0000000000, 0.0000000000);
cv::Mat M2 = (cv::Mat_<double>(3, 3) << 7789.0835738047, 0.0000000000, 2085.4988788672,
0.0000000000, 7523.3700278846, 271.1883527428,
0.0000000000, 0.0000000000, 1.0000000000);
cv::Mat D2 = (cv::Mat_<double>(1, 5) << 0.2407018952, -7.8745465803, 0.0000000000, 0.0000000000, 0.0000000000);
cv::Mat Q = (cv::Mat_<double>(4, 4) << 1, 0, 0, -2.8951307125091553e+03,
0, 1, 0, -5.0271924400329590e+02,
0, 0, 0, 7.5633399932893499e+03,
0, 0, 2.0857193103842788e+00, 0);
cv::Mat left = cv::imread("left_3.bmp");
cv::Mat right = cv::imread("right_3.bmp");
//Устраняем дисторсию
cv::Mat tmp1, tmp2;
cv::undistort(left, tmp1, M1, D1);
cv::undistort(right, tmp2, M2, D2);
left = tmp1;
right = tmp2;
cv::Mat left_for_matcher, right_for_matcher;
//int numberOfDisparities = 160;
int max_disp= 160;
max_disp/=2;
if(max_disp%16!=0)
max_disp += 16-(max_disp%16);
float scale = 2;
resize(left ,left_for_matcher ,Size(),scale,scale, INTER_LINEAR_EXACT);
resize(right,right_for_matcher,Size(),scale,scale, INTER_LINEAR_EXACT);
Ptr<StereoBM> left_matcher = StereoBM::create(max_disp,5);
/* left_matcher->setPreFilterCap(31);
left_matcher->setBlockSize(31);
left_matcher->setMinDisparity(0);
left_matcher->setNumDisparities(numberOfDisparities);
left_matcher->setTextureThreshold(10);
left_matcher->setUniquenessRatio(15);
left_matcher->setSpeckleWindowSize(100);
left_matcher->setSpeckleRange(32);
left_matcher->setDisp12MaxDiff(1);*/
auto wls_filter = createDisparityWLSFilter(left_matcher);
Ptr<StereoMatcher> right_matcher = createRightMatcher(left_matcher);
cvtColor(left_for_matcher, left_for_matcher, COLOR_BGR2GRAY);
cvtColor(right_for_matcher, right_for_matcher, COLOR_BGR2GRAY);
cv::Mat left_disp, right_disp, filtered_disp;
left_matcher->compute(left_for_matcher, right_for_matcher,left_disp);
right_matcher->compute(right_for_matcher,left_for_matcher, right_disp);
wls_filter->setLambda(8000);
wls_filter->setSigmaColor(1.5);
wls_filter->filter(left_disp,left_for_matcher,filtered_disp,right_disp);
Mat raw_disp_vis;
double vis_mult = 1;
cv::ximgproc::getDisparityVis(left_disp,raw_disp_vis,vis_mult);
namedWindow("raw disparity", WINDOW_NORMAL);
imshow("raw disparity", raw_disp_vis);
Mat filtered_disp_vis;
getDisparityVis(filtered_disp,filtered_disp_vis,vis_mult);
namedWindow("filtered disparity", WINDOW_NORMAL);
imshow("filtered disparity", filtered_disp_vis);
std::cout << filtered_disp_vis.size() <<std::endl;
resize(filtered_disp_vis ,filtered_disp_vis ,Size(),1/scale,1/scale, INTER_LINEAR_EXACT);
std::cout << filtered_disp_vis.size() <<std::endl;
cv::Point p(1200,900); //Берём произвольную точку на доске
filtered_disp_vis.convertTo(filtered_disp_vis, CV_32F);
float fDisp = filtered_disp_vis.at<float>(p.y, p.x);
//Расчёт расстояния
float distance = Q.at<double>(2, 3)*(1./ Q.at<double>(3, 2))/fDisp;
std::cout << "Distance = " << distance << std::endl;
cv::imwrite("filtered_disp_vis.png", filtered_disp_vis);
waitKey();
return 0;
}
Disparity:
Результат: 57.55
Расположение точки:
