Commit 20542325 authored by Thiago Santini's avatar Thiago Santini

Starts parametrisable camera calibration

parent 9435fc5f
...@@ -39,7 +39,8 @@ SOURCES +=\ ...@@ -39,7 +39,8 @@ SOURCES +=\
$${TOP}/src/Reference.cpp \ $${TOP}/src/Reference.cpp \
$${TOP}/src/LogWidget.cpp \ $${TOP}/src/LogWidget.cpp \
$${TOP}/src/PerformanceMonitor.cpp \ $${TOP}/src/PerformanceMonitor.cpp \
$${TOP}/src/PerformanceMonitorWidget.cpp $${TOP}/src/PerformanceMonitorWidget.cpp \
src/CameraCalibration.cpp
HEADERS += \ HEADERS += \
$${TOP}/src/MainWindow.h\ $${TOP}/src/MainWindow.h\
...@@ -65,7 +66,8 @@ HEADERS += \ ...@@ -65,7 +66,8 @@ HEADERS += \
$${TOP}/src/Reference.h \ $${TOP}/src/Reference.h \
$${TOP}/src/LogWidget.h \ $${TOP}/src/LogWidget.h \
$${TOP}/src/PerformanceMonitor.h \ $${TOP}/src/PerformanceMonitor.h \
$${TOP}/src/PerformanceMonitorWidget.h $${TOP}/src/PerformanceMonitorWidget.h \
src/CameraCalibration.h
FORMS += \ FORMS += \
$${TOP}/src/MainWindow.ui \ $${TOP}/src/MainWindow.ui \
......
#include "CameraCalibration.h"
using namespace cv;
using namespace std;
#define DBG_WINDOW_NAME "Camera Calibration Sample"
// TODO: handle user closing the calibration window during calibration
void CameraCalibration::toggleCalibration(bool status)
{
Q_UNUSED(status);
if (calibrationTogglePB->isChecked()) {
QMetaObject::invokeMethod(this, "startCalibration", Qt::QueuedConnection);
} else {
// Set temporary GUI state during calculations since it might take some time
calibrationTogglePB->setText("Calibrating...");
calibrationTogglePB->setEnabled(false);
collectPB->setEnabled(false);
QMetaObject::invokeMethod(this, "finishCalibration", Qt::QueuedConnection);
}
}
void CameraCalibration::startCalibration()
{
calibrationTogglePB->setText("Finish");
settingsGB->setEnabled(false);
collectPB->setEnabled(true);
imagePoints.clear();
sampleCountQL->setText(QString::number(imagePoints.size()));
if (dbgCB->isChecked())
namedWindow(DBG_WINDOW_NAME);
}
void CameraCalibration::finishCalibration()
{
if (dbgCB->isChecked())
destroyWindow(DBG_WINDOW_NAME);
calibrate();
updateCalibrationStatus( calibrationSuccessful );
calibrationTogglePB->setText("Start");
calibrationTogglePB->setEnabled(true);
settingsGB->setEnabled(true);
emit calibrationFinished( calibrationSuccessful );
}
void CameraCalibration::updateCalibrationStatus(bool success)
{
if (success) {
qInfo() << "Camera calibration done. RMS Error =" << rms;
undistortPB->setEnabled(true);
if (rms < 1)
rmsQL->setStyleSheet("QLabel { font : bold; color : green }");
else {
rmsQL->setStyleSheet("QLabel { font : bold; color : red }");
qInfo() << "RMS Error is above the expected value. It's recommended to recalibrate.";
}
rmsQL->setText( QString::number(rms) );
} else {
qInfo() << "Camera calibration failed.";
undistortPB->setEnabled(false);
rmsQL->setStyleSheet("QLabel { font : bold; color : black }");
rmsQL->setText( "N/A" );
}
}
void CameraCalibration::collectCalibrationSample()
{
collectPB->setEnabled(false);
emit requestSample();
}
void CameraCalibration::collectUndistortionSample()
{
undistortPB->setEnabled(false);
emit requestSample();
}
void CameraCalibration::calculateBoardCorners(vector<Point3f> &corners)
{
Size boardSize(hPatternSizeSB->value(), vPatternSizeSB->value());
float squareSize = 1e-3*squareSizeMMDSB->value();
corners.clear();
switch(patternCB->currentData().toInt()) {
case CHESSBOARD:
case CIRCLES_GRID:
for( int i = 0; i < boardSize.height; ++i )
for( int j = 0; j < boardSize.width; ++j )
corners.push_back(Point3f(j*squareSize, i*squareSize, 0));
break;
case ASYMMETRIC_CIRCLES_GRID:
for( int i = 0; i < boardSize.height; i++ )
for( int j = 0; j < boardSize.width; j++ )
corners.push_back(Point3f((2*j + i % 2)*squareSize, i*squareSize, 0));
break;
default:
break;
}
}
void CameraCalibration::newSample(const Mat &frame)
{
if (calibrationTogglePB->isChecked())
processSample(frame);
else
undistortSample(frame);
}
void CameraCalibration::undistortSample(const Mat &frame)
{
if (undistortPB->isEnabled())
return;
Mat tmp;
if (fishEyeCB->isChecked())
cv::fisheye::undistortImage(frame, tmp, newCameraMatrix, distCoeffs);
else
cv::undistort(frame, tmp, newCameraMatrix, distCoeffs);
imshow("Undistorted Image", tmp);
undistortPB->setEnabled(true);
}
void CameraCalibration::processSample(const Mat &frame)
{
if (collectPB->isEnabled())
return;
vector<Point2f> pointBuf;
bool found;
Mat gray = frame;
if (frame.channels() == 3)
cvtColor(frame, gray, CV_BGR2GRAY);
imageSize = { gray.cols, gray.rows };
Size patternSize( hPatternSizeSB->value(), vPatternSizeSB->value() );
int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
if( !fishEyeCB->isChecked())
chessBoardFlags |= CALIB_CB_FAST_CHECK;
switch( patternCB->currentData().toInt() )
{
case CHESSBOARD:
found = findChessboardCorners( gray, patternSize, pointBuf, chessBoardFlags);
if (found)
cv::cornerSubPix( gray, pointBuf, Size(11,11), Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 ) );
break;
case CIRCLES_GRID:
found = findCirclesGrid( gray, patternSize, pointBuf );
break;
case ASYMMETRIC_CIRCLES_GRID:
found = findCirclesGrid( gray, patternSize, pointBuf, CALIB_CB_ASYMMETRIC_GRID );
break;
default:
found = false;
break;
}
if (dbgCB->isChecked()) {
Mat tmp;
if (frame.channels() == 1)
cvtColor(frame, tmp, CV_GRAY2BGR);
else
tmp = frame.clone();
if (found)
drawChessboardCorners( tmp, patternSize, Mat(pointBuf), found);
else
putText(tmp, "Pattern not found", Point(0, 0.5*frame.rows), CV_FONT_HERSHEY_PLAIN, 2, Scalar(0,0,255));
imshow(DBG_WINDOW_NAME, tmp);
}
if (found) {
imagePoints.push_back(pointBuf);
sampleCountQL->setText(QString::number(imagePoints.size()));
}
collectPB->setEnabled(true);
}
void CameraCalibration::calibrate()
{
qInfo() << "Calibrating, this might take some time; please wait.";
cameraMatrix = Mat::eye(3, 3, CV_64F);
if (fishEyeCB->isChecked())
distCoeffs = Mat::zeros(4, 1, CV_64F);
else
distCoeffs = Mat::zeros(8, 1, CV_64F);
if (imagePoints.size() < 5) {
rms = 0;
calibrationSuccessful = false;
return;
}
vector<vector<Point3f> > objectPoints(1);
calculateBoardCorners(objectPoints[0]);
objectPoints.resize(imagePoints.size(),objectPoints[0]);
Mat rv, tv;
if (fishEyeCB->isChecked()) {
rms = fisheye::calibrate(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rv, tv, 0);
fisheye::estimateNewCameraMatrixForUndistortRectify(cameraMatrix, distCoeffs, imageSize, Matx33d::eye(), newCameraMatrix, 1);
} else {
rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rv, tv);
newCameraMatrix = getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 0, imageSize);
}
calibrationSuccessful = true;
}
void CameraCalibration::store(const QString &fileName)
{
FileStorage fs( fileName.toStdString(), FileStorage::WRITE);
fs << "cameraMatrix" << cameraMatrix;
fs << "distCoeffs" << distCoeffs;
fs << "imageSize" << imageSize;
fs << "newCameraMatrix" << newCameraMatrix;
fs << "rms" << rms;
}
void CameraCalibration::load(const QString &fileName)
{
QFileInfo info(fileName);
if ( !info.exists() ) {
calibrationSuccessful = false;
} else {
FileStorage fs( QString(fileName).toStdString(), FileStorage::READ);
fs["cameraMatrix"] >> cameraMatrix;
fs["distCoeffs"] >> distCoeffs;
fs["imageSize"] >> imageSize;
fs["newCameraMatrix"] >> newCameraMatrix;
fs["rms"] >> rms;
calibrationSuccessful = true;
}
updateCalibrationStatus(calibrationSuccessful);
}
#ifndef CAMERACALIBRATION_H
#define CAMERACALIBRATION_H
#include <vector>
#include <QObject>
#include <QDialog>
#include <QGridLayout>
#include <QHBoxLayout>
#include <QVBoxLayout>
#include <QFormLayout>
#include <QGroupBox>
#include <QSpinBox>
#include <QComboBox>
#include <QPushButton>
#include <QCheckBox>
#include <QLabel>
#include <QFileInfo>
#include "opencv2/imgproc.hpp"
#include "opencv2/calib3d.hpp"
#include "opencv2/highgui.hpp"
#include "utils.h"
class CameraCalibration : public QDialog
{
Q_OBJECT
public:
CameraCalibration(QWidget *parent=0)
: QDialog(parent),
calibrationSuccessful(false)
{
this->setWindowModality(Qt::ApplicationModal);
this->setWindowTitle("Camera Calibration");
QFormLayout *formLayout;
QHBoxLayout *hl;
hl = new QHBoxLayout();
settingsGB = new QGroupBox("Settings");
formLayout = new QFormLayout();
hPatternSizeSB = new QSpinBox();
hPatternSizeSB->setValue(4);
hPatternSizeSB->setWhatsThis("Horizontal pattern size");
hPatternSizeSB->setToolTip(hPatternSizeSB->whatsThis());
formLayout->addRow( new QLabel("Horizontal:"), hPatternSizeSB );
vPatternSizeSB = new QSpinBox();
vPatternSizeSB->setValue(11);
vPatternSizeSB->setWhatsThis("Vertical pattern size");
vPatternSizeSB->setToolTip(vPatternSizeSB->whatsThis());
formLayout->addRow( new QLabel("Vertical:"), vPatternSizeSB );
squareSizeMMDSB = new QDoubleSpinBox();
squareSizeMMDSB->setValue(20);
squareSizeMMDSB->setSuffix(" mm");
squareSizeMMDSB->setWhatsThis("Square size");
squareSizeMMDSB->setToolTip(squareSizeMMDSB->whatsThis());
formLayout->addRow( new QLabel("Square Size:"), squareSizeMMDSB );
fishEyeCB = new QCheckBox();
fishEyeCB->setChecked(false);
fishEyeCB->setWhatsThis("Use fish eye camera model instead of pinhole. Untested.");
fishEyeCB->setToolTip(fishEyeCB->whatsThis());
// TODO: test fish eye. Until then, disable it
fishEyeCB->setEnabled(false);
formLayout->addRow( new QLabel("Fish Eye"), fishEyeCB );
dbgCB = new QCheckBox();
dbgCB->setChecked(false);
dbgCB->setWhatsThis("Display results from the pattern detection.");
dbgCB->setToolTip(dbgCB->whatsThis());
formLayout->addRow( new QLabel("Show Debug"), dbgCB );
patternCB = new QComboBox();
patternCB->addItem("Assymetric Circles", ASYMMETRIC_CIRCLES_GRID);
patternCB->addItem("Chessboard", CHESSBOARD);
patternCB->addItem("Circles", CIRCLES_GRID);
patternCB->setWhatsThis("Calibration pattern");
patternCB->setToolTip(patternCB->whatsThis());
formLayout->addRow(patternCB);
settingsGB->setLayout(formLayout);
hl->addWidget(settingsGB);
controlGB = new QGroupBox("Control");
formLayout = new QFormLayout();
calibrationTogglePB = new QPushButton();
calibrationTogglePB->setText("Start");
calibrationTogglePB->setCheckable(true);
connect(calibrationTogglePB, SIGNAL(toggled(bool)),
this, SLOT(toggleCalibration(bool)) );
formLayout->addRow(calibrationTogglePB);
collectPB = new QPushButton();
collectPB->setText("Collect");
connect(collectPB, SIGNAL(pressed()),
this, SLOT(collectCalibrationSample()) );
collectPB->setEnabled(false);
formLayout->addRow(collectPB);
undistortPB = new QPushButton();
undistortPB->setText("Undistort");
connect(undistortPB, SIGNAL(pressed()),
this, SLOT(collectUndistortionSample()) );
undistortPB->setEnabled(false);
formLayout->addRow(undistortPB);
sampleCountQL = new QLabel();
sampleCountQL->setText(QString::number(0));
formLayout->addRow( new QLabel("Samples:"), sampleCountQL);
rmsQL = new QLabel();
rmsQL->setStyleSheet("QLabel { font : bold; color : black }");
rmsQL->setText("N/A");
formLayout->addRow( new QLabel("RMS Error:"), rmsQL);
controlGB->setLayout(formLayout);
hl->addWidget(controlGB);
setLayout(hl);
}
enum CALIBRATION_PATTERN {
ASYMMETRIC_CIRCLES_GRID = 0,
CIRCLES_GRID = 1,
CHESSBOARD = 2,
};
cv::Mat cameraMatrix;
cv::Mat newCameraMatrix;
cv::Mat distCoeffs;
bool calibrationSuccessful;
signals:
void requestSample();
void calibrationFinished(bool success);
public slots:
void showOptions(QPoint pos) {
move(pos);
show();
}
void toggleCalibration(bool status);
void collectCalibrationSample();
void collectUndistortionSample();
void newSample(const cv::Mat &frame);
void store(const QString &fileName);
void load(const QString &fileName);
private:
QGroupBox *settingsGB;
QSpinBox *hPatternSizeSB;
QSpinBox *vPatternSizeSB;
QDoubleSpinBox *squareSizeMMDSB;
QComboBox *patternCB;
QCheckBox *fishEyeCB;
QCheckBox *dbgCB;
QGroupBox *controlGB;
QPushButton *calibrationTogglePB;
QPushButton *collectPB;
QPushButton *undistortPB;
QLabel *sampleCountQL;
QLabel *rmsQL;
std::vector<std::vector<cv::Point2f> > imagePoints;
cv::Size imageSize;
void processSample(const cv::Mat &frame);
void undistortSample(const cv::Mat &frame);
void calculateBoardCorners(std::vector<cv::Point3f> &corners);
void calibrate();
void updateCalibrationStatus(bool success);
double rms;
private slots:
void startCalibration();
void finishCalibration();
};
#endif // CAMERACALIBRATION_H
...@@ -14,11 +14,8 @@ CameraWidget::CameraWidget(QString id, ImageProcessor::Type type, QWidget *paren ...@@ -14,11 +14,8 @@ CameraWidget::CameraWidget(QString id, ImageProcessor::Type type, QWidget *paren
settingROI(false), settingROI(false),
lastUpdate(0), lastUpdate(0),
updateIntervalMs(50), updateIntervalMs(50),
maxAgeMs(300), maxAgeMs(300),
collectionIntervalMs(2000), cameraCalibrationSampleRequested(false),
collectionCount(20),
patternSize(9,6),
squareSizeMM(25),
ui(new Ui::CameraWidget) ui(new Ui::CameraWidget)
{ {
ui->setupUi(this); ui->setupUi(this);
...@@ -93,7 +90,14 @@ CameraWidget::CameraWidget(QString id, ImageProcessor::Type type, QWidget *paren ...@@ -93,7 +90,14 @@ CameraWidget::CameraWidget(QString id, ImageProcessor::Type type, QWidget *paren
recorderThread->setPriority(QThread::NormalPriority); recorderThread->setPriority(QThread::NormalPriority);
recorder = new DataRecorderThread(id, type == ImageProcessor::Eye ? EyeData().header() : FieldData().header()); recorder = new DataRecorderThread(id, type == ImageProcessor::Eye ? EyeData().header() : FieldData().header());
recorder->moveToThread(recorderThread); recorder->moveToThread(recorderThread);
QMetaObject::invokeMethod(recorder, "create"); QMetaObject::invokeMethod(recorder, "create");
cameraCalibration = new CameraCalibration();
connect(cameraCalibration, SIGNAL(requestSample()),
this, SLOT(requestCameraCalibrationSample()) );
connect(cameraCalibration, SIGNAL(calibrationFinished(bool)),
this, SLOT(onCameraCalibrationFinished(bool)) );
cameraCalibration->load(gCfgDir + "/" + id + "Calibration.xml");
// GUI // GUI
optionsGroup = new QActionGroup(this); optionsGroup = new QActionGroup(this);
...@@ -139,7 +143,12 @@ CameraWidget::~CameraWidget() ...@@ -139,7 +143,12 @@ CameraWidget::~CameraWidget()
if (recorder) { if (recorder) {
recorder->deleteLater(); recorder->deleteLater();
recorder = NULL; recorder = NULL;
} }
if (cameraCalibration) {
cameraCalibration->deleteLater();
cameraCalibration = NULL;
}
cameraThread->quit(); cameraThread->quit();
cameraThread->wait(); cameraThread->wait();
...@@ -148,7 +157,7 @@ CameraWidget::~CameraWidget() ...@@ -148,7 +157,7 @@ CameraWidget::~CameraWidget()
processorThread->wait(); processorThread->wait();
recorderThread->quit(); recorderThread->quit();
recorderThread->wait(); recorderThread->wait();
} }
void CameraWidget::preview(Timestamp t, const cv::Mat &frame) void CameraWidget::preview(Timestamp t, const cv::Mat &frame)
...@@ -182,7 +191,9 @@ void CameraWidget::preview(EyeData data) ...@@ -182,7 +191,9 @@ void CameraWidget::preview(EyeData data)
drawROI(painter); drawROI(painter);
if (data.validPupil) if (data.validPupil)
drawPupil(data.pupil, painter); drawPupil(data.pupil, painter);
ui->viewFinder->setPixmap(QPixmap::fromImage(scaled)); ui->viewFinder->setPixmap(QPixmap::fromImage(scaled));
sendCameraCalibrationSample(data.input);
} }
void CameraWidget::preview(FieldData data) void CameraWidget::preview(FieldData data)
...@@ -218,7 +229,9 @@ void CameraWidget::preview(const DataTuple &data) ...@@ -218,7 +229,9 @@ void CameraWidget::preview(const DataTuple &data)
drawMarker(data.field.collectionMarker, painter, Qt::green); drawMarker(data.field.collectionMarker, painter, Qt::green);
if (data.field.validGazeEstimate) if (data.field.validGazeEstimate)
drawGaze(data.field, painter); drawGaze(data.field, painter);
ui->viewFinder->setPixmap(QPixmap::fromImage(scaled)); ui->viewFinder->setPixmap(QPixmap::fromImage(scaled));
sendCameraCalibrationSample(input);
} }
void CameraWidget::updateFrameRate(Timestamp t) void CameraWidget::updateFrameRate(Timestamp t)
...@@ -270,89 +283,9 @@ void CameraWidget::options(QAction* action) ...@@ -270,89 +283,9 @@ void CameraWidget::options(QAction* action)
if (imageProcessor) if (imageProcessor)
QMetaObject::invokeMethod(imageProcessor, "showOptions", Qt::QueuedConnection, Q_ARG(QPoint, this->pos())); QMetaObject::invokeMethod(imageProcessor, "showOptions", Qt::QueuedConnection, Q_ARG(QPoint, this->pos()));
if (option == "calibrate camera") { if (option == "calibrate camera")
QMessageBox::StandardButton reply = QMessageBox::question(this, "Camera Calibration", if (cameraCalibration)
QString("Show the chess pattern in multiple positions to calibrate the camera.\n") QMetaObject::invokeMethod(cameraCalibration, "showOptions", Qt::QueuedConnection, Q_ARG(QPoint, this->pos()));
+QString("Current configuration is:\n\n")
+QString("Samples: %1\n").arg(collectionCount)
+QString("Inter-sample interval: %1 s\n").arg(1.0e-3*collectionIntervalMs)
+QString("Pattern size: %1 x %2\n").arg(patternSize.width).arg(patternSize.height)
+QString("Square size: %1 mm\n").arg(squareSizeMM)
+QString("\nOther widget options will be inaccessible during calibration.\n")
+QString("Continue?"),
QMessageBox::Yes|QMessageBox::No);
if (reply != QMessageBox::Yes)
return;
ui->menubar->setEnabled(false);
imagePoints.clear();
connect(camera, SIGNAL(newFrame(Timestamp,cv::Mat)),
this, SLOT(collectCameraCalibration(Timestamp,cv::Mat)) );
cameraCalTimer.start();
calibratingCamera = true;
}
}
void CameraWidget::collectCameraCalibration(Timestamp t, cv::Mat frame)
{
Q_UNUSED(t)
// TODO: parametrize me
if (!calibratingCamera || cameraCalTimer.elapsed() < collectionIntervalMs)
return;
else
cameraCalTimer.restart();
Mat view = frame.clone();
vector<Point2f> pointBuf;
bool found;
int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
found = findChessboardCorners( view, patternSize, pointBuf, chessBoardFlags);
int count = collectionCount;
if (found) {
imagePoints.push_back(pointBuf);
cameraCalTimer.restart();
drawChessboardCorners(view, patternSize, pointBuf, found);
QString tmp = QString::number(imagePoints.size()) + "/" + QString::number(count);
putText(view, tmp.toStdString(), Point(0.05*view.cols, 0.9*view.rows), CV_FONT_NORMAL, 1, Scalar(0,255,255),2);
imshow("Camera calibration", view);
}
if (imagePoints.size() >= count) {
putText(view, "Calibrating, please wait...", Point(0.05*view.cols, 0.5*view.rows), CV_FONT_NORMAL, 1, Scalar(0,255,255),2);
imshow("Camera calibration", view);
waitKey(500);
calibratingCamera = false;
cameraCalTimer.invalidate();
double squareSize = squareSizeMM * 1.0e-3;
Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
Mat distCoeffs = Mat::zeros(8, 1, CV_64F);
Size imageSize = frame.size();