FieldImageProcessor.cpp 5.25 KB
Newer Older
Thiago Santini's avatar
Thiago Santini committed
1 2 3 4 5 6 7 8 9 10 11 12 13
#include "FieldImageProcessor.h"
#include <opencv2/highgui.hpp>

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

static int gFieldDataId = qRegisterMetaType<FieldData>("FieldData");

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

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

    pmIdx = gPerformanceMonitor.enrol(id, "Image Processor");
Thiago Santini's avatar
Thiago Santini committed
29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46
}

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

FieldImageProcessor::~FieldImageProcessor()
{
    if(settings)
        settings->deleteLater();
}

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

    QMutexLocker locker(&cfgMutex);
51
	Timestamp processingStartNs = gTimer.nsecsElapsed();
Thiago Santini's avatar
Thiago Santini committed
52 53 54 55 56 57 58

    data.timestamp = timestamp;

    if (cfg.inputSize.width > 0 && cfg.inputSize.height > 0) {
        data.input = Mat(cfg.inputSize, frame.type() );
        resize(frame, data.input, cfg.inputSize);
    }
Thiago Santini's avatar
Thiago Santini committed
59 60 61 62
	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
63 64

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

67 68
	Size curSize = {data.input.cols, data.input.rows};
	sanitizeCameraParameters( curSize );
Thiago Santini's avatar
Thiago Santini committed
69

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

    data.width = data.input.cols;
79
	data.height = data.input.rows;
Thiago Santini's avatar
Thiago Santini committed
80 81 82 83

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

94
	if (cfg.markerDetectionMethod == "aruco" || gCalibrating) {
Thiago Santini's avatar
Thiago Santini committed
95 96 97 98 99 100 101 102
		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
103 104 105 106 107

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

108 109 110 111 112
	// Note that the following is based on the COLLECTION MARKER size.
	/* TODO: check whether the pose estimation works with fisheye intrinsic parameters
	 * 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.
	 */
113
	if (ids.size() > 0) {
114
		if (cameraCalibration)
115
			cameraCalibration->estimatePoseSingleMarkers(curSize, corners, cfg.collectionMarkerSizeMeters, data.undistorted, rvecs, tvecs);
116 117 118
		else
			estimatePoseSingleMarkers(corners, cfg.collectionMarkerSizeMeters, CameraCalibration::optimalCameraMatrix(curSize), CameraCalibration::optimalDistortionCoefficients(), rvecs, tvecs);
	}
Thiago Santini's avatar
Thiago Santini committed
119 120 121 122 123 124

    for (unsigned int i=0; i<ids.size(); i++) {
        Marker marker(corners[i], ids[i]);

        marker.center = estimateMarkerCenter(marker.corners);
        marker.center.z = tvecs.at<double>(i,2);
125 126
		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
127 128 129 130 131 132 133 134 135 136 137 138

        data.markers.push_back( marker );

        // 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();
        }
    }

139 140
	data.validGazeEstimate = false;
	data.cameraCalibration = cameraCalibration;
141
	data.processingTimestamp = ns2ms(gTimer.nsecsElapsed() - processingStartNs);
Thiago Santini's avatar
Thiago Santini committed
142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157

    emit newData(data);
}

void FieldImageProcessor::newROI(QPointF sROI, QPointF eROI)
{
    QMutexLocker locker(&cfgMutex);
    if (sROI.isNull() || eROI.isNull()) {
        this->sROI = QPointF(0,0);
        this->eROI = QPointF(1,1);
    } else {
        this->sROI = sROI;
        this->eROI = eROI;
    }
}

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