FieldImageProcessor.cpp 5.31 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 15 16
	  forceSanitize(false),
	  cameraCalibration(NULL),
	  QObject(parent)
Thiago Santini's avatar
Thiago Santini committed
17 18 19 20 21 22 23
{
    settings = new QSettings(gCfgDir + "/" + id + " ImageProcessor", QSettings::IniFormat);
    updateConfig();

    dict = getPredefinedDictionary(DICT_4X4_250);
    detectorParameters = new DetectorParameters();
    detectorParameters->markerBorderBits = 2;
24 25 26
	detectorParameters->minMarkerPerimeterRate = 0.1; // TODO: determine a good value for these based on the fov and maximum detection distance
	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, gTimer.elapsed() - timestamp, 100) )
Thiago Santini's avatar
Thiago Santini committed
48 49 50 51 52 53 54 55 56 57
        return;

    QMutexLocker locker(&cfgMutex);

    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
58 59 60 61
	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
62 63

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

66
	sanitizeCameraParameters( Size(data.input.cols, data.input.rows) );
Thiago Santini's avatar
Thiago Santini committed
67

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

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

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

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

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

107 108 109 110 111
	// 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.
	 */
112 113 114 115 116 117
	if (ids.size() > 0) {
		if (data.undistorted) {
			qWarning() << "Marker pose estimation using undistorted image is not implemented yet.";
			// TODO: undistort the corners, then apply the estimation
			estimatePoseSingleMarkers(corners, cfg.collectionMarkerSizeMeters, cameraMatrix, distCoeffs, rvecs, tvecs);
		} else
Thiago Santini's avatar
Thiago Santini committed
118
			estimatePoseSingleMarkers(corners, cfg.collectionMarkerSizeMeters, cameraMatrix, distCoeffs, rvecs, tvecs);
Thiago Santini's avatar
Thiago Santini committed
119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140
    }

    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);
        marker.tv = ( Mat_<float>(1,3) << tvecs.at<double>(i,0), tvecs.at<double>(i,1), tvecs.at<double>(i,2) );
        marker.rv = ( Mat_<float>(1,3) << rvecs.at<double>(i,0), rvecs.at<double>(i,1), rvecs.at<double>(i,2) );

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

    data.validGazeEstimate = false;
Thiago Santini's avatar
Thiago Santini committed
141
	data.processingTimestamp = gTimer.elapsed() - data.timestamp;
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 164 165
	if (!forceSanitize && size == expectedSize)
		return;
	forceSanitize = false;
	expectedSize = size;
	cameraMatrix = cameraCalibration->getCameraMatrix(size);
	distCoeffs = cameraCalibration->getDistortionCoefficients(size);
Thiago Santini's avatar
Thiago Santini committed
166
}