Jump to content
Compvision.ru
Vitalii

Расчёт дистанции до объекта

Recommended Posts

Добрый день!

У меня возникли вопросы при расчёте дистанции до объекта по двум камерам в OpenCV.

Калибровку камер выполнил в Matlab Stereo Camera Calibrator App (в OpenCV возникли проблемы) и затем сохранил параметры в формате OpenCV с помощью:

https://github.com/JinyongJeong/StereoCalibration_matlab2opencv

Получил матрицы: R1, R2, P1, P2, Q, R, T, M1, M2, D1, D2.

При использовании алгоритма:

https://github.com/mixr26/stereo_vision

cv::Rect RectLeft=cv::Rect(1190,960,550,450); //Объект на левом кадре
cv::Rect RectRight=cv::Rect(790,960,550,450); //Объект на правом кадре
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);

//X, Y and Z coordinates of a real-world object
cv::perspectiveTransform(input, output, Q);
std::cout << output[0] <<std::endl;

получаю значение X, Y, Z

[-2.04382, 0.548109, 9.06563] – Не совсем понятно, как перевести эти значения в метры.

При использовании другого алгоритма, в котором рассчитывается карта глубины, получаю некорректные значения дистанции (в то числе, отрицательные).

cv::Mat img1 = cv::imread("left_12.bmp", 1);
cv::Mat img2 = cv::imread("right_12.bmp", 1);
Size img_size = img1.size();
Ptr<StereoSGBM> sgbm = StereoSGBM::create(0,16,3);
int numberOfDisparities = 64;

//известно
cv::Mat M1, D1, M2 D2 Q R1, R2, P1, P2, R, T;

//устраняем дисторсию
Mat img1r, img2r;
cv::undistort(img1, img1r, M1, D1);
cv::undistort(img2, img2r, M2, D2);

img1 = img1r;
img2 = img2r;

numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : ((img_size.width/8) + 15) & -16;

sgbm->setPreFilterCap(63);
int sgbmWinSize = 3;
sgbm->setBlockSize(sgbmWinSize);

int cn = img1.channels();

sgbm->setP1(8*cn*sgbmWinSize*sgbmWinSize);
sgbm->setP2(32*cn*sgbmWinSize*sgbmWinSize);
sgbm->setMinDisparity(0);
sgbm->setNumDisparities(numberOfDisparities);
sgbm->setUniquenessRatio(10);
sgbm->setSpeckleWindowSize(100);
sgbm->setSpeckleRange(32);
sgbm->setDisp12MaxDiff(1);

Mat disp, disp8, disp32;

sgbm->compute(img1, img2, disp);

// compute the real-world distance [mm]
//distance = (baseline * focal length) / disparity
//Q (output from stereoRectify). Q[2][3] = focal length, 1/Q[3][2] = baseline.
   
float fMaxDistance = static_cast<float>((1. / Q.at<double>(3, 2)) * Q.at<double>(2, 3));
disp.convertTo(disp32, CV_32F);
auto outputDisparityValue =  disp32.at<float>(1200,1200);
// DISP_SCALE = 16
float fDisparity = outputDisparityValue / cv::StereoMatcher::DISP_SCALE;
float fDistance = fMaxDistance / fDisparity;

std::cout << "Distance = " <<fDistance <<std::endl;

В чём может быть проблема?

 

Share this post


Link to post
Share on other sites

А можете привести схему эксперимента? Как установлены камеры, расположение и размеры объекта. А то вы привели результат работы алгоритма, а что должно быть мы не знаем. Может быть еще минимальный запускаемый кусок кода с тестовым изображением для пощупать проблему.

  • Thanks 1

Share this post


Link to post
Share on other sites

Добрый день!

 

Сейчас пытаюсь определить расстояние до калибровочной доски (расстояние примерно 1.6 метра, размер клетки 28 мм).

Левая камера

left_3.thumb.png.a91dfcf6bc988bb7a67f8394e106d941.png 

Правая камера

right_3.thumb.png.f4d182dd8906c218d25ebaa4a8c38e38.png

 

Примерное расположение доски при калибровке. Расстояние рассчитано автоматически в Matlab Stereo Camera Calibrator App.

image.png.b242d1f156edd4023077c56df07f3bee.png

Первый алгоритм

#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:

 

filtered_disp_vis.thumb.png.12b06d4a4661da0bc5fc34c0983758d8.png

Результат: 57.55

Расположение точки:

 

filtered_disp_vis_.thumb.png.b8dab0ec7b0d999300c50ff823a921d4.png

 

  • Like 1

Share this post


Link to post
Share on other sites

Да, disparity как то не сошлось. Видимо картинка не подходящая, квадратики гладкие и регулярные текстуры. Не то что любят алгоритмы стерео матчинга. 

Посмотрите примеры здесь: https://answers.opencv.org/question/117141/triangulate-3d-points-from-a-stereo-camera-and-chessboard/  может наведет на мысли. 

А почему калибруете через матлаб ? OpenCv - шная тулза не подходит ? Можно попробовать откалибровать opencv-шным и сравнить коэффициенты.

  • Thanks 1

Share this post


Link to post
Share on other sites

Вы хотите по 2 снимкам с такой малой базой получить премлимую 3-d карту местности? Ничего не получится, возьмите какой-нибудь  софт типа Agisoft или 3DF Zephyr Free (он бесплатен) и поэксперементируте там. Кстати 3DF Zephyr Free может делать калибровку по набору произвольных фоток (штук 30) и по опыту лучше чем с шахматной доской.

Если просто нужно измерять расстояния можно использовать Aruco с 4 точками, которое выдаст координаты и направление камер. Еще Aruco нельзя в лоб снимать, точно резко падает, нужно чтоб квадрат на снимках был типа трапеции

  • Like 1
  • Thanks 1

Share this post


Link to post
Share on other sites
20 часов назад, Smorodov сказал:

Посмотрите примеры здесь: https://answers.opencv.org/question/117141/triangulate-3d-points-from-a-stereo-camera-and-chessboard/  может наведет на мысли. 

Благодарю!

20 часов назад, Smorodov сказал:

А почему калибруете через матлаб ? OpenCv - шная тулза не подходит ? Можно попробовать откалибровать opencv-шным и сравнить коэффициенты.

Пробовал. Получаются разные результаты.

Значения матрицы M1, полученные в Matlab:

M1 = 7986.2559620260, 0.0000000000, 524.7794558939,
     0.0000000000, 7603.3099586941, 765.6466654278,
     0.0000000000, 0.0000000000, 1.0000000000);

Значения матрицы M1, полученные в OpenCV:

M1 = 3.4296806178638740e+03, 0., 1.2727552031201496e+03, 
     0., 3.3866798437426000e+03, 6.9065925391676274e+02, 
     0., 0., 1.

Фокусные расстояния, полученные в OpenCV в 2,3 раза меньше, чем в Matlab.

Результат расчёта disparity (для устранения дисторсии использовал матрицы полученные при калибровке с помощью OpenCV):

filtered_disp_vis.thumb.png.aff012ea3a27493e0970588589122363.png

 

3 часа назад, fotomer сказал:

Вы хотите по 2 снимкам с такой малой базой получить премлимую 3-d карту местности? Ничего не получится, возьмите какой-нибудь  софт типа Agisoft или 3DF Zephyr Free (он бесплатен) и поэксперементируте там. Кстати 3DF Zephyr Free может делать калибровку по набору произвольных фоток (штук 30) и по опыту лучше чем с шахматной доской.

Если просто нужно измерять расстояния можно использовать Aruco с 4 точками, которое выдаст координаты и направление камер. Еще Aruco нельзя в лоб снимать, точно резко падает, нужно чтоб квадрат на снимках был типа трапеции

Благодарю за рекомендации!

 

 

 

Share this post


Link to post
Share on other sites
26 минут назад, Vitalii сказал:

Благодарю!

Пробовал. Получаются разные результаты.

Значения матрицы M1, полученные в Matlab:


M1 = 7986.2559620260, 0.0000000000, 524.7794558939,
     0.0000000000, 7603.3099586941, 765.6466654278,
     0.0000000000, 0.0000000000, 1.0000000000);

 


M1 = 3.4296806178638740e+03, 0., 1.2727552031201496e+03, 
     0., 3.3866798437426000e+03, 6.9065925391676274e+02, 
     0., 0., 1.

Фокусные расстояния, полученные в OpenCV в 2,3 раза меньше, чем в Matlab.

 

Вообще именно фокус никогда точно не угадывается, его я на контрольном отрезке точно выставлял. Если интересно в свое время писал программку http://xn--e1anfamim.xn--p1ai/index.php/novosti/iphotomeasure

здесь процесс калибровки http://xn--e1anfamim.xn--p1ai/index.php/novosti/kalibrovka-kamery

Только снимки должны быть пестрые и не однообразные, шахматные доски не подходят. Даже дешевые смартфоны потом точно измеряют

  • Thanks 1

Share this post


Link to post
Share on other sites
1 час назад, fotomer сказал:

Вообще именно фокус никогда точно не угадывается, его я на контрольном отрезке точно выставлял. Если интересно в свое время писал программку http://xn--e1anfamim.xn--p1ai/index.php/novosti/iphotomeasure

здесь процесс калибровки http://xn--e1anfamim.xn--p1ai/index.php/novosti/kalibrovka-kamery

Только снимки должны быть пестрые и не однообразные, шахматные доски не подходят. Даже дешевые смартфоны потом точно измеряют

Интересно, благодарю!

Share this post


Link to post
Share on other sites

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now


  • Recently Browsing   0 members

    No registered users viewing this page.

×