Commit 9a49047c authored by Thiago Santini's avatar Thiago Santini

Field image processor now uses the camera calibration class

parent 862ae24b
......@@ -47,6 +47,7 @@ void CameraCalibration::startCalibration()
void CameraCalibration::finishCalibration()
{
calibrated = false;
QFuture<void> future = QtConcurrent::run(this, &CameraCalibration::calibrate);
watcher.setFuture(future);
}
......@@ -54,12 +55,12 @@ void CameraCalibration::finishCalibration()
void CameraCalibration::onCalibrated()
{
this->layout()->setEnabled(true);
updateCalibrationStatus( calibrationSuccessful );
updateCalibrationStatus( calibrated );
calibrationTogglePB->setText("Start");
calibrationTogglePB->setEnabled(true);
undistortPB->setEnabled(true);
settingsGB->setEnabled(true);
emit calibrationFinished( calibrationSuccessful );
emit calibrationFinished( calibrated );
}
void CameraCalibration::updateCalibrationStatus(bool success)
......@@ -121,15 +122,13 @@ void CameraCalibration::newSample(const Mat &frame)
undistortSample(frame);
}
void CameraCalibration::undistortSample(const Mat &frame)
{
if (undistortPB->isEnabled())
return;
Mat tmp;
if (fishEyeCB->isChecked())
remap(frame, tmp, map1, map2, CV_INTER_AREA);
else
remap(frame, tmp, map1, map2, CV_INTER_AREA);
undistort(frame, tmp);
imshow("Undistorted Image", tmp);
this->activateWindow();
this->setFocus();
......@@ -153,7 +152,7 @@ void CameraCalibration::processSample(const Mat &frame)
Size patternSize( hPatternSizeSB->value(), vPatternSizeSB->value() );
int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
if( !fishEyeCB->isChecked())
if( !fishEye)
chessBoardFlags |= CALIB_CB_FAST_CHECK;
switch( patternCB->currentData().toInt() )
......@@ -204,16 +203,36 @@ void CameraCalibration::processSample(const Mat &frame)
collectPB->setEnabled(true);
}
void CameraCalibration::internalInitUndistortRectifyMap()
{
float alpha = 1;
if (fishEye) {
fisheye::estimateNewCameraMatrixForUndistortRectify(cameraMatrix, distCoeffs, imageSize, Matx33d::eye(), newCameraMatrix, alpha, imageSize);
fisheye::initUndistortRectifyMap(cameraMatrix, distCoeffs, Matx33d::eye(),
newCameraMatrix, imageSize, CV_16SC2,
map1, map2
);
} else {
newCameraMatrix = getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, alpha, imageSize);
initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
newCameraMatrix, imageSize, CV_16SC2,
map1, map2
);
}
}
void CameraCalibration::calibrate()
{
calibrated = false;
qInfo() << "Calibrating, this might take some time; please wait.";
cameraMatrix = Mat::eye(3, 3, CV_64F);
if (fishEyeCB->isChecked())
fishEye = fishEyeCB->isChecked();
if (fishEye)
distCoeffs = Mat::zeros(4, 1, CV_64F);
else
distCoeffs = Mat::zeros(8, 1, CV_64F);
calibrationSuccessful = false;
rms = 0;
if (imagePoints.size() < 5)
......@@ -225,28 +244,20 @@ void CameraCalibration::calibrate()
try {
Mat rv, tv;
if (fishEyeCB->isChecked()) {
if (fishEye) {
int fisheyeFlags = 0;
fisheyeFlags |= fisheye::CALIB_RECOMPUTE_EXTRINSIC;
fisheyeFlags |= fisheye::CALIB_CHECK_COND;
fisheyeFlags |= fisheye::CALIB_FIX_SKEW;
rms = fisheye::calibrate(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rv, tv, fisheyeFlags);
fisheye::estimateNewCameraMatrixForUndistortRectify(cameraMatrix, distCoeffs, imageSize, Matx33d::eye(), newCameraMatrix, 1, imageSize);
fisheye::initUndistortRectifyMap(cameraMatrix, distCoeffs, Matx33d::eye(),
newCameraMatrix, imageSize, CV_16SC2,
map1, map2);
} else {
rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rv, tv);
newCameraMatrix = getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize);
initUndistortRectifyMap( cameraMatrix, distCoeffs, Mat(),
newCameraMatrix, imageSize, CV_16SC2,
map1, map2
);
}
calibrationSuccessful = true;
internalInitUndistortRectifyMap();
calibrated = true;
} catch (cv::Exception &e) {
qWarning() << "Calibration failed:" << e.what();
calibrationSuccessful = false;
calibrated = false;
}
}
......@@ -260,14 +271,14 @@ void CameraCalibration::store(const QString &fileName)
fs << "sampleCount" << sampleCount;
fs << "coverage" << coverage;
fs << "rms" << rms;
fs << "fisheye" << fishEyeCB->isChecked();
fs << "fishEye" << fishEye;
}
void CameraCalibration::load(const QString &fileName)
{
QFileInfo info(fileName);
if ( !info.exists() ) {
calibrationSuccessful = false;
calibrated = false;
} else {
FileStorage fs( QString(fileName).toStdString(), FileStorage::READ);
fs["cameraMatrix"] >> cameraMatrix;
......@@ -277,19 +288,10 @@ void CameraCalibration::load(const QString &fileName)
fs["sampleCount"] >> sampleCount;
fs["coverage"] >> coverage;
fs["rms"] >> rms;
bool isFisheye;
fs["fisheye"] >> isFisheye;
calibrationSuccessful = true;
if (isFisheye) {
fisheye::initUndistortRectifyMap(cameraMatrix, distCoeffs, Matx33d::eye(),
newCameraMatrix, imageSize, CV_16SC2,
map1, map2);
} else {
initUndistortRectifyMap( cameraMatrix, distCoeffs, Mat(),
newCameraMatrix, imageSize, CV_32FC1,
map1, map2
);
}
fs["fishEye"] >> fishEye;
fishEyeCB->setChecked(fishEye);
internalInitUndistortRectifyMap();
calibrated = true;
}
updateCalibrationStatus(calibrationSuccessful);
updateCalibrationStatus(calibrated);
}
......@@ -32,7 +32,7 @@ class CameraCalibration : public QDialog
public:
CameraCalibration(QString id, QWidget *parent=0)
: QDialog(parent),
calibrationSuccessful(false),
calibrated(false),
sampleCount(0),
coverage(0),
id(id)
......@@ -129,15 +129,39 @@ public:
CHESSBOARD = 2,
};
cv::Mat cameraMatrix;
cv::Mat newCameraMatrix;
cv::Mat distCoeffs;
cv::Size imageSize;
cv::Rect covered;
int sampleCount;
double coverage;
double rms;
bool calibrationSuccessful;
void undistort(const cv::Mat &in, cv::Mat &out) {
if ( !calibrated || in.cols != imageSize.width || in.rows != imageSize.height ) {
out = in.clone();
return;
}
if (fishEye)
remap(in, out, map1, map2, CV_INTER_AREA);
else
remap(in, out, map1, map2, CV_INTER_AREA);
}
cv::Mat getCameraMatrix(cv::Size size) {
if ( !calibrated || size != imageSize ) {
return (cv::Mat_<double>(3,3) <<
size.width, 0, 0.5*size.width,
0, size.width, 0.5*size.height,
0, 0 ,1 );
}
return cameraMatrix;
}
cv::Mat getDistortionCoefficients(cv::Size size) {
if ( !calibrated || size != imageSize )
return (cv::Mat_<double>(4,1) << 0, 0, 0, 0);
// our coefficients are k1,k2,k3,k4, but estimatePoseSingleMarkers wants (k1,k2,p1,p2[,k3[,k4,k5,k6],[s1,s2,s3,s4]])
if (fishEye)
return (cv::Mat_<double>(4,1) << distCoeffs.at<double>(0,0), distCoeffs.at<double>(1,0), 0, 0);
else
return distCoeffs;
}
signals:
void requestSample();
......@@ -176,6 +200,16 @@ private:
QFutureWatcher<void> watcher;
std::vector<std::vector<cv::Point2f> > imagePoints;
cv::Mat cameraMatrix;
cv::Mat newCameraMatrix;
cv::Mat distCoeffs;
cv::Size imageSize;
cv::Rect covered;
int sampleCount;
double coverage;
double rms;
bool fishEye;
std::atomic<bool> calibrated;
cv::Mat map1, map2;
void processSample(const cv::Mat &frame);
......@@ -183,6 +217,7 @@ private:
void calculateBoardCorners(std::vector<cv::Point3f> &corners);
void calibrate();
void updateCalibrationStatus(bool success);
void internalInitUndistortRectifyMap();
void setLabelText(QLabel* label, QString val, QString color) {
label->setStyleSheet("QLabel { font : bold; color : " + color + " }");
label->setText( val );
......
......@@ -53,15 +53,23 @@ CameraWidget::CameraWidget(QString id, ImageProcessor::Type type, QWidget *paren
this, SLOT(noCamera(QString)));
// debugging preview
//connect(camera, SIGNAL(newFrame(Timestamp, const cv::Mat&)),
// this, SLOT(preview(Timestamp, const cv::Mat&)) );
// this, SLOT(preview(Timestamp, const cv::Mat&)) );
cameraCalibration = new CameraCalibration(id);
connect(cameraCalibration, SIGNAL(requestSample()),
this, SLOT(requestCameraCalibrationSample()) );
connect(cameraCalibration, SIGNAL(calibrationFinished(bool)),
this, SLOT(onCameraCalibrationFinished(bool)) );
cameraCalibration->load(gCfgDir + "/" + id + "Calibration.xml");
// Image Processor
processorThread = new QThread();
processorThread->setObjectName(id + " Processor");
processorThread->start();
processorThread->setPriority(QThread::TimeCriticalPriority);
imageProcessor = new ImageProcessor(id, type);
imageProcessor->moveToThread(processorThread);
imageProcessor = new ImageProcessor(id, type);
imageProcessor->cameraCalibration = cameraCalibration;
imageProcessor->moveToThread(processorThread);
switch (type) {
case ImageProcessor::Eye:
imageProcessor->eyeProcessorUI = new EyeImageProcessorUI;
......@@ -70,8 +78,8 @@ CameraWidget::CameraWidget(QString id, ImageProcessor::Type type, QWidget *paren
connect(imageProcessor, SIGNAL(newData(EyeData)),
this, SIGNAL(newData(EyeData)) );
break;
case ImageProcessor::Field:
imageProcessor->fieldProcessorUI = new FieldImageProcessorUI;
case ImageProcessor::Field:
imageProcessor->fieldProcessorUI = new FieldImageProcessorUI;
connect(imageProcessor, SIGNAL(newData(FieldData)),
this, SLOT(preview(FieldData)) );
connect(imageProcessor, SIGNAL(newData(FieldData)),
......@@ -79,7 +87,7 @@ CameraWidget::CameraWidget(QString id, ImageProcessor::Type type, QWidget *paren
break;
}
QMetaObject::invokeMethod(imageProcessor, "create");
connect(camera, SIGNAL(newFrame(Timestamp, const cv::Mat&)),
connect(camera, SIGNAL(newFrame(Timestamp, const cv::Mat&)),
imageProcessor, SIGNAL(process(Timestamp, const cv::Mat&)) );
// Data Recorder
......@@ -91,13 +99,6 @@ CameraWidget::CameraWidget(QString id, ImageProcessor::Type type, QWidget *paren
recorder->moveToThread(recorderThread);
QMetaObject::invokeMethod(recorder, "create");
cameraCalibration = new CameraCalibration(id);
connect(cameraCalibration, SIGNAL(requestSample()),
this, SLOT(requestCameraCalibrationSample()) );
connect(cameraCalibration, SIGNAL(calibrationFinished(bool)),
this, SLOT(onCameraCalibrationFinished(bool)) );
cameraCalibration->load(gCfgDir + "/" + id + "Calibration.xml");
// GUI
optionsGroup = new QActionGroup(this);
connect(optionsGroup, SIGNAL(triggered(QAction*)),
......
......@@ -11,8 +11,9 @@ FieldImageProcessor::FieldImageProcessor(QString id, QObject *parent)
: id(id),
sROI(QPointF(0,0)),
eROI(QPointF(1,1)),
forceSanitize(false),
QObject(parent)
forceSanitize(false),
cameraCalibration(NULL),
QObject(parent)
{
settings = new QSettings(gCfgDir + "/" + id + " ImageProcessor", QSettings::IniFormat);
updateConfig();
......@@ -59,16 +60,21 @@ void FieldImageProcessor::process(Timestamp timestamp, const Mat &frame)
}
if (cfg.flip != CV_FLIP_NONE)
flip(data.input, data.input, cfg.flip);
flip(data.input, data.input, cfg.flip);
sanitizeCameraParameters( Size(data.input.cols, data.input.rows) );
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.undistorted = cfg.undistort;
if (data.undistorted) {
if (cameraCalibration) {
Mat tmp;
cameraCalibration->undistort(data.input, tmp);
data.input = tmp;
}
}
data.width = data.input.cols;
data.height = data.input.rows;
data.height = data.input.rows;
// Marker detection and pose estimation
vector<int> ids;
......@@ -84,7 +90,6 @@ void FieldImageProcessor::process(Timestamp timestamp, const Mat &frame)
}
if (cfg.markerDetectionMethod == "aruco" || gCalibrating) {
detectMarkers(downscaled, dict, corners, ids, detectorParameters);
if (cfg.processingDownscalingFactor > 1) { // Upscale if necessary
......@@ -103,15 +108,12 @@ void FieldImageProcessor::process(Timestamp timestamp, const Mat &frame)
* 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.
*/
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
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
estimatePoseSingleMarkers(corners, cfg.collectionMarkerSizeMeters, cameraMatrix, distCoeffs, rvecs, tvecs);
}
......@@ -152,63 +154,12 @@ void FieldImageProcessor::newROI(QPointF sROI, QPointF eROI)
}
}
void FieldImageProcessor::sanitizeCameraParameters(Size size)
void FieldImageProcessor::sanitizeCameraParameters(cv::Size size)
{
if (!forceSanitize) {
if ( (size == imageSize) && !map1.empty() && !map2.empty() )
return;
} else
forceSanitize = false;
/* TODO:
*
* 1) Move camera parameters logic to the Camera instead so it's
* considered for the eye cameras as well.
*
* 2) Drop the logic for resizing the intrinsic parameters since it's not
* throughly tested.
*
*/
FileStorage fs( QString(gCfgDir + "/" + id + "Calibration.xml").toStdString(), FileStorage::READ);
fs["cameraMatrix"] >> cameraMatrix;
fs["distCoeffs"] >> distCoeffs;
fs["imageSize"] >> imageSize;
bool haveCameraParameters = false;
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) <<
size.width, 0, 0.5*size.width,
0, size.width, 0.5*size.height,
0, 0 ,1 );
distCoeffs = (Mat_<double>(1,4) << 0, 0, 0, 0);
imageSize = size;
}
initUndistortRectifyMap(
cameraMatrix,
distCoeffs,
Mat(),
getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize),
imageSize,
CV_32FC1,
map1,
map2
);
if (!forceSanitize && size == expectedSize)
return;
forceSanitize = false;
expectedSize = size;
cameraMatrix = cameraCalibration->getCameraMatrix(size);
distCoeffs = cameraCalibration->getDistortionCoefficients(size);
}
......@@ -20,6 +20,8 @@
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/charuco.hpp>
#include "CameraCalibration.h"
#include "InputWidget.h"
#include "utils.h"
......@@ -56,8 +58,6 @@ public:
extrapolatedGazeEstimate = 0;
markers = std::vector<Marker>();
collectionMarker = Marker();
cameraMatrix = cv::Mat();
distCoeffs = cv::Mat();
undistorted = false;
width = 0;
height = 0;
......@@ -69,9 +69,8 @@ public:
int extrapolatedGazeEstimate;
Marker collectionMarker;
std::vector<Marker> markers;
cv::Mat cameraMatrix, distCoeffs;
bool undistorted;
unsigned int width;
bool undistorted;
unsigned int width;
unsigned int height;
QString header(QString prefix = "") const {
......@@ -355,6 +354,7 @@ public:
explicit FieldImageProcessor(QString id, QObject *parent = 0);
~FieldImageProcessor();
QSettings *settings;
CameraCalibration *cameraCalibration;
signals:
void newData(FieldData data);
......@@ -374,12 +374,12 @@ private:
cv::Ptr<cv::aruco::Dictionary> dict;
cv::Ptr<cv::aruco::DetectorParameters> detectorParameters;
// Undistortion
cv::Mat cameraMatrix, newCameraMatrix, distCoeffs;
cv::Mat rvecs, tvecs;
cv::Mat map1, map2;
cv::Size imageSize, newImageSize;
bool forceSanitize;
// Undistortion
cv::Mat cameraMatrix, newCameraMatrix, distCoeffs;
cv::Mat rvecs, tvecs;
bool forceSanitize;
cv::Size expectedSize;
void sanitizeCameraParameters(cv::Size size);
unsigned int pmIdx;
......
......@@ -2,7 +2,8 @@
ImageProcessor::ImageProcessor(QString id, Type type, QObject *parent)
: id(id),
type(type),
type(type),
cameraCalibration(NULL),
eyeProcessor(NULL),
fieldProcessor(NULL),
eyeProcessorUI(NULL),
......@@ -36,7 +37,7 @@ void ImageProcessor::create()
this->type = type;
switch (type) {
case Eye:
eyeProcessor = new EyeImageProcessor(id);
eyeProcessor = new EyeImageProcessor(id);
connect(this, SIGNAL(process(Timestamp,const cv::Mat&)),
eyeProcessor, SLOT(process(Timestamp,const cv::Mat&)) );
connect(this, SIGNAL(newROI(QPointF,QPointF)),
......@@ -60,7 +61,8 @@ void ImageProcessor::create()
eyeProcessor, SLOT(updateConfig()) );
break;
case Field:
fieldProcessor = new FieldImageProcessor(id);
fieldProcessor = new FieldImageProcessor(id);
fieldProcessor->cameraCalibration = cameraCalibration;
connect(this, SIGNAL(process(Timestamp,const cv::Mat&)),
fieldProcessor, SLOT(process(Timestamp,const cv::Mat&)) );
connect(this, SIGNAL(newROI(QPointF,QPointF)),
......
......@@ -13,11 +13,12 @@ class ImageProcessor : public QObject
Q_OBJECT
public:
enum Type { Eye, Field };
explicit ImageProcessor(QString id, Type type, QObject *parent = 0);
explicit ImageProcessor(QString id, Type type, QObject *parent = 0);
~ImageProcessor();
EyeImageProcessorUI* eyeProcessorUI;
FieldImageProcessorUI* fieldProcessorUI;
CameraCalibration *cameraCalibration;
EyeImageProcessorUI* eyeProcessorUI;
FieldImageProcessorUI* fieldProcessorUI;
signals:
void process(Timestamp t, cv::Mat frame);
......@@ -33,8 +34,8 @@ public slots:
private:
QString id;
Type type;
EyeImageProcessor* eyeProcessor;
FieldImageProcessor* fieldProcessor;
EyeImageProcessor* eyeProcessor;
FieldImageProcessor* fieldProcessor;
};
......
......@@ -18,7 +18,7 @@ MainWindow::MainWindow(QWidget *parent) :
ui(new Ui::MainWindow)
{
ui->setupUi(this);
#define EYEREC
//#define EYEREC
#ifdef EYEREC
createExtraMenus();
......
......@@ -90,7 +90,7 @@ void FieldOverlay::drawMarker(const Marker &marker)
for (unsigned int i=0; i<4; i++)
contour[i] = QPointF(marker.corners[i].x, marker.corners[i].y);
painter.drawConvexPolygon(contour, 4);
int delta = 15*refPx;
int delta = 30*refPx;
painter.drawText(
QRectF(marker.center.x-delta/2, marker.center.y-delta/2, delta, delta),
Qt::AlignCenter|Qt::TextWordWrap,
......
......@@ -64,7 +64,8 @@ void PupilTrackingMethod::run(const Timestamp &ts, const cv::Mat &frame, const c
predictMaxPupilDiameter();
if ( previousPupil.confidence == NO_CONFIDENCE ) {
pupil = pupilDetectionMethod.runWithConfidence(frame, roi, -1, predictedMaxPupilDiameter);
//pupil = pupilDetectionMethod.runWithConfidence(frame, roi, -1, predictedMaxPupilDiameter);
pupil = pupilDetectionMethod.runWithConfidence(frame, roi, -1, -1);
} else {
run(frame, roi, previousPupil, pupil);
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment