FieldImageProcessor.cpp 7.37 KB
Newer Older
Thiago Santini's avatar
Thiago Santini committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22
#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)),
      forceSanitize(false),
      QObject(parent)
{
    settings = new QSettings(gCfgDir + "/" + id + " ImageProcessor", QSettings::IniFormat);
    updateConfig();

    dict = getPredefinedDictionary(DICT_4X4_250);
    detectorParameters = new DetectorParameters();
    detectorParameters->markerBorderBits = 2;
23
	detectorParameters->minMarkerPerimeterRate = 0.05; // TODO: determine a good value for these based on the fov and maximum detection distance
Thiago Santini's avatar
Thiago Santini committed
24
    //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 38 39 40 41 42 43 44
}

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
45
    if ( gPerformanceMonitor.shouldDrop(pmIdx, gTimer.elapsed() - timestamp, 100) )
Thiago Santini's avatar
Thiago Santini committed
46 47 48 49 50 51 52 53 54 55
        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
56 57 58 59
	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
60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75

    if (cfg.flip != CV_FLIP_NONE)
        flip(data.input, data.input, cfg.flip);

    sanitizeCameraParameters( Size(data.input.cols, data.input.rows) );

    data.undistorted = cfg.undistort;
    if (data.undistorted)
        remap(data.input, data.input, map1, map2, CV_INTER_AREA);

    data.width = data.input.cols;
    data.height = data.input.rows;

    // Marker detection and pose estimation
    vector<int> ids;
    vector<vector<Point2f> > corners;
Thiago Santini's avatar
Thiago Santini committed
76 77
	Mat downscaled;
	if (cfg.processingDownscalingFactor > 1) {
Thiago Santini's avatar
Thiago Santini committed
78 79 80 81
        resize(data.input, downscaled, Size(),
               1/cfg.processingDownscalingFactor,
               1/cfg.processingDownscalingFactor,
               INTER_AREA);
Thiago Santini's avatar
Thiago Santini committed
82 83 84 85
	} else {
		downscaled = data.input;
	}

86
	if (cfg.markerDetectionMethod == "aruco" || gCalibrating) {
Thiago Santini's avatar
Thiago Santini committed
87 88 89 90 91 92 93 94 95

		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
96 97 98 99 100

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

101 102 103 104 105
	// 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.
	 */
Thiago Santini's avatar
Thiago Santini committed
106 107 108 109 110 111 112 113 114
    if (ids.size() > 0) {
        if (data.undistorted) {
            Mat emptyCameraMatrix = (Mat_<double>(3,3) <<
                           data.width,          0, data.width/2,
                           0         , data.width, data.height/2,
                           0         ,0          , 1);
            Mat emptyDistCoeffs = (Mat_<double>(1,4) << 0, 0, 0, 0);
            estimatePoseSingleMarkers(corners, cfg.collectionMarkerSizeMeters, emptyCameraMatrix, emptyDistCoeffs, rvecs, tvecs);
        } else
Thiago Santini's avatar
Thiago Santini committed
115
			estimatePoseSingleMarkers(corners, cfg.collectionMarkerSizeMeters, cameraMatrix, distCoeffs, rvecs, tvecs);
Thiago Santini's avatar
Thiago Santini committed
116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137
    }

    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
138
	data.processingTimestamp = gTimer.elapsed() - data.timestamp;
Thiago Santini's avatar
Thiago Santini committed
139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162

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

void FieldImageProcessor::sanitizeCameraParameters(Size size)
{
    if (!forceSanitize) {
        if ( (size == imageSize) && !map1.empty() && !map2.empty() )
            return;
    } else
        forceSanitize = false;

163 164 165 166 167
	/* TODO:
	 *
	 * 1) Move camera parameters logic to the Camera instead so it's
	 * considered for the eye cameras as well.
	 *
168
	 * 2) Drop the logic for resizing the intrinsic parameters since it's not
169 170 171 172
	 * throughly tested.
	 *
	 */

Thiago Santini's avatar
Thiago Santini committed
173 174
    FileStorage fs( QString(gCfgDir + "/" + id + "Calibration.xml").toStdString(), FileStorage::READ);
    fs["cameraMatrix"] >> cameraMatrix;
Thiago Santini's avatar
Thiago Santini committed
175 176 177
	fs["distCoeffs"] >> distCoeffs;
	fs["imageSize"] >> imageSize;
	bool haveCameraParameters = false;
Thiago Santini's avatar
Thiago Santini committed
178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196
    if (!cameraMatrix.empty() && !distCoeffs.empty())
        haveCameraParameters = true;

    double currentAspectRatio = size.width / (float) size.height;
    double originalAspectRatio = imageSize.width / (float) imageSize.height;

    if (haveCameraParameters && (currentAspectRatio == originalAspectRatio) ) {
        qInfo() << "Found intrinsic parameters.";
        double rx = imageSize.width  / (double) size.width;
        double ry = imageSize.height / (double) size.height;
        cameraMatrix.at<double>(0,2) = cameraMatrix.at<double>(0,2) / rx;
        cameraMatrix.at<double>(1,2) = cameraMatrix.at<double>(1,2) / ry;
        cameraMatrix.at<double>(0,0) = cameraMatrix.at<double>(0,0) / rx;
        cameraMatrix.at<double>(1,1) = cameraMatrix.at<double>(1,1) / rx;
        imageSize = size;
    } else {
        qInfo() << "No valid intrinsic parameters available. Using dummy values";
        // Dummy estimation
        cameraMatrix = (Mat_<double>(3,3) <<
Thiago Santini's avatar
Thiago Santini committed
197 198
		                size.width, 0, 0.5*size.width,
		                0, size.width, 0.5*size.height,
Thiago Santini's avatar
Thiago Santini committed
199 200 201 202 203 204 205 206 207
                        0, 0 ,1 );
        distCoeffs = (Mat_<double>(1,4) << 0, 0, 0, 0);
        imageSize = size;
    }

    initUndistortRectifyMap(
                            cameraMatrix,
                            distCoeffs,
                            Mat(),
Thiago Santini's avatar
Thiago Santini committed
208
							getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize),
Thiago Santini's avatar
Thiago Santini committed
209 210 211 212 213 214
                            imageSize,
                            CV_32FC1,
                            map1,
                            map2
                            );
}