双目测距实践

相机标定

推荐：Camera Calibration Toolbox for Matlab

注意：如果使用 OpenCV stereoCalibrate 标定，有时不准确。

相机参数

%YAML:1.0
---
M1: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [3.6459219777739514e+02, 0.0000000000000000e+00, 3.9185768866120537e+02, 0.0000000000000000e+00, 3.6554705148119706e+02, 2.2141071388057932e+02, 0.0000000000000000e+00, 0.0000000000000000e+00, 1.0000000000000000e+00]
D1: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [-2.9171567968247419e-01, 6.9443677458318795e-02, 1.2997395294960031e-03, -6.1020297049036993e-04, 0.0000000000000000e+00]
M2: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [3.6223851073380979e+02, 0.0000000000000000e+00, 3.5691196239969628e+02, 0.0000000000000000e+00, 3.6444462126929182e+02, 2.6063185759617613e+02, 0.0000000000000000e+00, 0.0000000000000000e+00, 1.0000000000000000e+00]
D2: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [-2.5411455824424356e-01, 4.8203032751684569e-02, -1.2395568054111433e-03, 3.4717812567463969e-03, 0.0000000000000000e+00]
R: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [9.9736462975222850e-01, 3.6018960645630145e-03, 7.2462553528979914e-02, -2.7604532363296190e-03, 9.9992763812704788e-01, -1.1708902920155377e-02, -7.2499484254236121e-02, 1.1478016135363778e-02, 9.9730240144525184e-01]
T: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [-1.1506411960375723e+02, 5.4795604549239174e-01, 5.8816978191100553e+00]

原理实现

C++源代码（MYNTEYE）

#include <iomanip>
#include <iostream>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "camera.h"
#include "utility.h"

using namespace std;
using namespace mynteye;

int main(int argc, char const *argv[]) {
    std::string name;
    if (argc >= 2) {
        name = argv[1];
    } else {
        bool found = false;
        name = FindDeviceName(&found);
    }
    cout << "Open Camera: " << name << endl;

    Camera cam;
    InitParameters params(name);
    cam.Open(params);

    if (!cam.IsOpened()) {
        std::cerr << "Error: Open camera failed" << std::endl;
        return 1;
    }
    std::cout << "\033[1;32mPress ESC/Q on Windows to terminate\033[0m\n";

    double t, fps = 0;
    ErrorCode code;
    cv::Mat img_left, img_right;

    // Add codes to get (X,Y,Z)
    bool found_left = false;
    bool found_right  = false;
    vector<cv::Point2f> corners_left;
    vector<cv::Point2f> corners_right;
    cv::Size board_sz = cv::Size(7, 5); // chess5x7x0.03

    // Some parameters used in calculation
    double fx = 3.6459219777739514e+02;
    double fy = 3.6554705148119706e+02;
    double b  = 1.1506411960375723e+02;
    double cx = 3.9185768866120537e+02;
    double cy = 2.2141071388057932e+02;

    for (;;) {
        t = (double)cv::getTickCount();

        code = cam.Grab();

        if (code != ErrorCode::SUCCESS) continue;

        if (cam.RetrieveImage(img_left, View::VIEW_LEFT) == ErrorCode::SUCCESS &&
            cam.RetrieveImage(img_right, View::VIEW_RIGHT) == ErrorCode::SUCCESS) {

            // top left: width x height
            stringstream ss;
            ss << img_left.cols << "x" << img_left.rows;
            DrawInfo(img_left, ss.str(), Gravity::TOP_LEFT);
            DrawInfo(img_right, ss.str(), Gravity::TOP_LEFT);
            // top right: fps
            ss.str(""); ss.clear();
            ss << fixed << setw(7) << setprecision(2) << setfill(' ') << fps;
            DrawInfo(img_left, ss.str(), Gravity::TOP_RIGHT);
            DrawInfo(img_right, ss.str(), Gravity::TOP_RIGHT);

            // Add codes to get (X,Y,Z)
            found_left = cv::findChessboardCorners(img_left, board_sz, corners_left);
            found_right = cv::findChessboardCorners(img_right, board_sz, corners_right);
            double disparity = corners_left[0].x - corners_right[0].x;

            if (found_left && found_right && abs(corners_left[0].y - corners_right[0].y) <= 2.0)
            {
                // cout << "corners_left [0]: " << corners_left[0] << endl;
                // cout << "corners_right[0]: " << corners_right[0] << endl;
                double z = fx * b / disparity;
                double x = (corners_left[0].x - cx) * z / fx;
                double y = (corners_left[0].y - cy) * z / fy;
                cout << "(" << x << ", " << y << ", " << z << ")" << endl;
                // Show the corner
                circle(img_left, corners_left[0], 10, cv::Scalar(255,255,255), 3);
                circle(img_right, corners_right[0], 10, cv::Scalar(255,255,255), 3);
            }
            cv::imshow("left", img_left);
            cv::imshow("right", img_right);
        }

        char key = (char) cv::waitKey(1);
        if (key == 27 || key == 'q' || key == 'Q') {  // ESC/Q
            // Save the images
            // cv::imwrite("left.jpg",img_left);
            // cv::imwrite("right.jpg", img_right);
            // cout << "Saved Images!" << endl;
            break;
        }

        t = (double)cv::getTickCount() - t;
        fps = cv::getTickFrequency() / t;
    }

    cam.Close();
    cv::destroyAllWindows();
    return 0;
}

注意：VIEW_LEFT/VIEW_RIGHT(无畸变) 需要输入相机参数。