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

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

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

Thiago Santini's avatar
Thiago Santini committed
10 11 12
static int markerDetectionCounter = 0;
static int redetectMarkerCounter = 1;

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

    dict = getPredefinedDictionary(DICT_4X4_250);
    detectorParameters = new DetectorParameters();
    detectorParameters->markerBorderBits = 2;
27 28 29
	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
30 31

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

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
50
	if ( gPerformanceMonitor.shouldDrop(pmIdx, gTimer.elapsed() - timestamp, 100) )
Thiago Santini's avatar
Thiago Santini committed
51 52 53 54 55 56 57 58 59 60
        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
61 62 63 64
	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
65 66

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

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

71 72 73 74 75 76 77 78
	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
79 80

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

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

96
	if (cfg.markerDetectionMethod == "aruco" || gCalibrating) {
Thiago Santini's avatar
Thiago Santini committed
97 98 99 100 101 102 103 104 105 106 107 108 109 110
#define LIMIT_MARKER_DETECTION
#ifdef LIMIT_MARKER_DETECTION
		if ( markerDetectionCounter == redetectMarkerCounter ) {
			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];
			}
			markerDetectionCounter = 0;
		} else
			markerDetectionCounter++;
#else
Thiago Santini's avatar
Thiago Santini committed
111 112 113 114 115 116 117
		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
118
#endif
Thiago Santini's avatar
Thiago Santini committed
119
	}
Thiago Santini's avatar
Thiago Santini committed
120 121 122 123 124

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

125 126 127 128 129
	// 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.
	 */
130 131 132 133 134 135
	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
136
			estimatePoseSingleMarkers(corners, cfg.collectionMarkerSizeMeters, cameraMatrix, distCoeffs, rvecs, tvecs);
Thiago Santini's avatar
Thiago Santini committed
137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158
    }

    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
159
	data.processingTimestamp = gTimer.elapsed() - data.timestamp;
Thiago Santini's avatar
Thiago Santini committed
160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175

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

176
void FieldImageProcessor::sanitizeCameraParameters(cv::Size size)
Thiago Santini's avatar
Thiago Santini committed
177
{
178 179 180 181 182 183
	if (!forceSanitize && size == expectedSize)
		return;
	forceSanitize = false;
	expectedSize = size;
	cameraMatrix = cameraCalibration->getCameraMatrix(size);
	distCoeffs = cameraCalibration->getDistortionCoefficients(size);
Thiago Santini's avatar
Thiago Santini committed
184
}