FieldImageProcessor.cpp 5.42 KB
Newer Older
Thiago Santini's avatar
Thiago Santini committed
1 2 3 4 5 6 7
#include "FieldImageProcessor.h"
#include <opencv2/highgui.hpp>

using namespace std;
using namespace cv;
using namespace aruco;

8 9 10 11 12 13 14
FieldImageProcessor::FieldImageProcessor(QString id, QObject* parent)
    : id(id)
    , sROI(QPointF(0, 0))
    , eROI(QPointF(1, 1))
    , forceSanitize(false)
    , cameraCalibration(nullptr)
    , QObject(parent)
Thiago Santini's avatar
Thiago Santini committed
15
{
16
    settings = new QSettings(gCfgDir + "/" + id + " ImageProcessor.ini", QSettings::IniFormat);
Thiago Santini's avatar
Thiago Santini committed
17 18 19 20 21
    updateConfig();

    dict = getPredefinedDictionary(DICT_4X4_250);
    detectorParameters = new DetectorParameters();
    detectorParameters->markerBorderBits = 2;
22 23 24
    detectorParameters->minMarkerPerimeterRate = 0.05; // TODO: determine a good value for these based on the fov and maximum detection distance
    detectorParameters->doCornerRefinement = false;
    //printMarkers(); // TODO: parametrize me
25 26

    pmIdx = gPerformanceMonitor.enrol(id, "Image Processor");
Thiago Santini's avatar
Thiago Santini committed
27 28 29 30 31 32 33 34 35 36 37
}

void FieldImageProcessor::updateConfig()
{
    QMutexLocker locker(&cfgMutex);
    cfg.load(settings);
    forceSanitize = true;
}

FieldImageProcessor::~FieldImageProcessor()
{
38
    if (settings)
Thiago Santini's avatar
Thiago Santini committed
39 40 41
        settings->deleteLater();
}

42
void FieldImageProcessor::process(Timestamp timestamp, const Mat& frame)
Thiago Santini's avatar
Thiago Santini committed
43 44
{
    // TODO: parametrize frame drop due to lack of processing power
45
    if (gPerformanceMonitor.shouldDrop(pmIdx, timestamp, 100))
Thiago Santini's avatar
Thiago Santini committed
46 47 48
        return;

    QMutexLocker locker(&cfgMutex);
49
    Timestamp processingStartNs = gTimer.nsecsElapsed();
Thiago Santini's avatar
Thiago Santini committed
50 51 52 53

    data.timestamp = timestamp;

    if (cfg.inputSize.width > 0 && cfg.inputSize.height > 0) {
54
        data.input = Mat(cfg.inputSize, frame.type());
Thiago Santini's avatar
Thiago Santini committed
55
        resize(frame, data.input, cfg.inputSize);
56 57 58
    } else {
        Q_ASSERT_X(frame.data != data.input.data, "Field Image Processing", "Previous and current input image matches!");
        data.input = frame;
Thiago Santini's avatar
Thiago Santini committed
59 60 61
    }

    if (cfg.flip != CV_FLIP_NONE)
62
        flip(data.input, data.input, cfg.flip);
Thiago Santini's avatar
Thiago Santini committed
63

64 65
    Size curSize = { data.input.cols, data.input.rows };
    sanitizeCameraParameters(curSize);
Thiago Santini's avatar
Thiago Santini committed
66

67 68 69 70 71 72 73
    if (cfg.undistort && cameraCalibration) {
        Mat tmp;
        cameraCalibration->undistort(data.input, tmp);
        data.input = tmp;
        data.undistorted = true;
    } else
        data.undistorted = false;
Thiago Santini's avatar
Thiago Santini committed
74 75

    data.width = data.input.cols;
76
    data.height = data.input.rows;
Thiago Santini's avatar
Thiago Santini committed
77 78 79

    // Marker detection and pose estimation
    vector<int> ids;
80 81 82
    vector<vector<Point2f>> corners;
    Mat downscaled;
    if (cfg.processingDownscalingFactor > 1) {
Thiago Santini's avatar
Thiago Santini committed
83
        resize(data.input, downscaled, Size(),
84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99
            1 / cfg.processingDownscalingFactor,
            1 / cfg.processingDownscalingFactor,
            INTER_AREA);
    } else {
        downscaled = data.input;
    }

    if (cfg.markerDetectionMethod == "aruco" || gCalibrating) {
        detectMarkers(downscaled, dict, corners, ids, detectorParameters);

        if (cfg.processingDownscalingFactor > 1) { // Upscale if necessary
            for (unsigned int i = 0; i < ids.size(); i++)
                for (unsigned int j = 0; j < corners[i].size(); j++)
                    corners[i][j] = cfg.processingDownscalingFactor * corners[i][j];
        }
    }
Thiago Santini's avatar
Thiago Santini committed
100 101 102 103 104

    // Filling the marker data
    data.collectionMarker = Marker();
    data.markers.clear();

105 106
    // Note that the following is based on the COLLECTION MARKER size.
    /* TODO: check whether the pose estimation works with fisheye intrinsic parameters
107 108 109
	 * An initial (and short) test with a pupil labs wide angle camera at 720p seeemed
	 * to match the distance measured with a laser distance meter.
	 */
110 111 112 113 114 115 116 117
    if (ids.size() > 0) {
        if (cameraCalibration)
            cameraCalibration->estimatePoseSingleMarkers(curSize, corners, cfg.collectionMarkerSizeMeters, data.undistorted, rvecs, tvecs);
        else
            estimatePoseSingleMarkers(corners, cfg.collectionMarkerSizeMeters, CameraCalibration::optimalCameraMatrix(curSize), CameraCalibration::optimalDistortionCoefficients(), rvecs, tvecs);
    }

    for (unsigned int i = 0; i < ids.size(); i++) {
Thiago Santini's avatar
Thiago Santini committed
118 119 120
        Marker marker(corners[i], ids[i]);

        marker.center = estimateMarkerCenter(marker.corners);
121 122 123
        marker.center.z = tvecs.at<double>(i, 2);
        marker.tv = (Mat_<double>(1, 3) << tvecs.at<double>(i, 0), tvecs.at<double>(i, 1), tvecs.at<double>(i, 2));
        marker.rv = (Mat_<double>(1, 3) << rvecs.at<double>(i, 0), rvecs.at<double>(i, 1), rvecs.at<double>(i, 2));
Thiago Santini's avatar
Thiago Santini committed
124

125
        data.markers.push_back(marker);
Thiago Santini's avatar
Thiago Santini committed
126 127 128 129 130 131 132 133 134 135

        // use closest calibration marker -- to try and avoid detecting the one viewed in the field camera when testing :-)
        if (marker.id == cfg.collectionMarkerId) {
            if (data.collectionMarker.id == -1)
                data.collectionMarker = data.markers.back();
            else if (data.collectionMarker.center.z > marker.center.z)
                data.collectionMarker = data.markers.back();
        }
    }

136 137 138
    data.validGazeEstimate = false;
    data.cameraCalibration = cameraCalibration;
    data.processingTimestamp = ns2ms(gTimer.nsecsElapsed() - processingStartNs);
Thiago Santini's avatar
Thiago Santini committed
139 140 141 142 143 144 145 146

    emit newData(data);
}

void FieldImageProcessor::newROI(QPointF sROI, QPointF eROI)
{
    QMutexLocker locker(&cfgMutex);
    if (sROI.isNull() || eROI.isNull()) {
147 148
        this->sROI = QPointF(0, 0);
        this->eROI = QPointF(1, 1);
Thiago Santini's avatar
Thiago Santini committed
149 150 151 152 153 154
    } else {
        this->sROI = sROI;
        this->eROI = eROI;
    }
}

155
void FieldImageProcessor::sanitizeCameraParameters(cv::Size size)
Thiago Santini's avatar
Thiago Santini committed
156
{
157 158 159 160
    if (!forceSanitize && size == expectedSize)
        return;
    forceSanitize = false;
    expectedSize = size;
Thiago Santini's avatar
Thiago Santini committed
161
}