Доска почета

Добрый день! Сейчас пытаюсь определить расстояние до калибровочной доски (расстояние примерно 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 Расположение точки: