added CalibrationDialog class and Calibration tool app

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1587 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-07-22 22:22:59 +00:00
parent 3328a7c9a0
commit c1497d9e92
5 changed files with 757 additions and 3 deletions

View File

@@ -0,0 +1,67 @@
/*
* CalibrationDialog.h
*
* Created on: 2014-07-22
* Author: Mathieu
*/
#ifndef CALIBRATIONDIALOG_H_
#define CALIBRATIONDIALOG_H_
#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
#include <QtGui/QDialog>
#include <opencv2/opencv.hpp>
#include <rtabmap/utilite/UEventsHandler.h>
class Ui_calibrationDialog;
namespace rtabmap {
class RTABMAPGUI_EXP CalibrationDialog : public QDialog, public UEventsHandler
{
Q_OBJECT;
public:
CalibrationDialog(QWidget * parent = 0);
virtual ~CalibrationDialog();
private slots:
void processImage(const cv::Mat & image);
void restart();
void calibrate();
void exit();
protected:
virtual void closeEvent(QCloseEvent* event);
virtual void handleEvent(UEvent * event);
private:
float getArea(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize);
float getSkew(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize);
// x -> [0, 1] (left, right)
// y -> [0, 1] (top, bottom)
// size -> [0, 1] (small -> big)
// skew -> [0, 1] (low, high)
void getParams(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize, const cv::Size & imageSize,
float & x, float & y, float & size, float & skew);
private:
// parameters
cv::Size boardSize_; // innner squares
float squareSize_; // m
std::vector<std::vector<cv::Point2f> > imagePoints_;
std::vector<std::vector<float> > imageParams_;
cv::Size imageSize_;
bool calibrated_;
cv::Mat cameraMatrix_;
cv::Mat distCoeffs_;
Ui_calibrationDialog * ui_;
};
} /* namespace rtabmap */
#endif /* CALIBRATIONDIALOG_H_ */

View File

@@ -12,10 +12,12 @@ SET(headers_ui
./StatsToolBox.h
./DetailedProgressDialog.h
./utilite/UPlot.h
./utilite/UImageView.h
../include/${PROJECT_PREFIX}/gui/CloudViewer.h
../include/${PROJECT_PREFIX}/gui/OdometryViewer.h
../include/${PROJECT_PREFIX}/gui/LoopClosureViewer.h
../include/${PROJECT_PREFIX}/gui/DataRecorder.h
../include/${PROJECT_PREFIX}/gui/CalibrationDialog.h
./ExportDialog.h
./MapVisibilityWidget.h
)
@@ -28,6 +30,7 @@ SET(uis
./ui/DatabaseViewer.ui
./ui/loopClosureViewer.ui
./ui/exportDialog.ui
./ui/calibrationDialog.ui
)
SET(qrc
@@ -63,6 +66,7 @@ SET(SRC_FILES
./OdometryViewer.cpp
./LoopClosureViewer.cpp
./DataRecorder.cpp
./CalibrationDialog.cpp
./ExportDialog.cpp
./MapVisibilityWidget.cpp
./GraphViewer.cpp

View File

@@ -0,0 +1,358 @@
/*
* CalibrationDialog.cpp
*
* Created on: 2014-07-22
* Author: Mathieu
*/
#include "rtabmap/gui/CalibrationDialog.h"
#include "ui_calibrationDialog.h"
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <rtabmap/core/CameraEvent.h>
#include <rtabmap/gui/UCv2Qt.h>
#include <rtabmap/utilite/ULogger.h>
namespace rtabmap {
#define COUNT_MIN 12
CalibrationDialog::CalibrationDialog(QWidget * parent) :
QDialog(parent),
boardSize_(8,6),
squareSize_(0.033),
calibrated_(false),
cameraMatrix_(cv::Mat::eye(3, 3, CV_64F)),
distCoeffs_(cv::Mat::zeros(8, 1, CV_64F))
{
qRegisterMetaType<cv::Mat>("cv::Mat");
ui_ = new Ui_calibrationDialog();
ui_->setupUi(this);
connect(ui_->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
connect(ui_->pushButton_restart, SIGNAL(clicked()), this, SLOT(restart()));
connect(ui_->pushButton_exit, SIGNAL(clicked()), this, SLOT(exit()));
ui_->progressBar_count->setMaximum(COUNT_MIN);
ui_->progressBar_count->setFormat("%v");
this->restart();
}
CalibrationDialog::~CalibrationDialog()
{
this->unregisterFromEventsManager();
delete ui_;
}
void CalibrationDialog::closeEvent(QCloseEvent* event)
{
this->unregisterFromEventsManager();
}
void CalibrationDialog::handleEvent(UEvent * event)
{
if(event->getClassName().compare("CameraEvent") == 0)
{
rtabmap::CameraEvent * e = (rtabmap::CameraEvent *)event;
if(e->getCode() == rtabmap::CameraEvent::kCodeImage ||
e->getCode() == rtabmap::CameraEvent::kCodeImageDepth)
{
QMetaObject::invokeMethod(this, "processImage", Q_ARG(cv::Mat, e->image().image()));
}
}
}
void CalibrationDialog::processImage(const cv::Mat & image)
{
imageSize_ = image.size();
std::vector<cv::Point2f> pointBuf;
bool found = cv::findChessboardCorners( image, boardSize_, pointBuf,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
if ( found) // If done with success,
{
// improve the found corners' coordinate accuracy for chessboard
cv::Mat viewGray;
cvtColor(image, viewGray, cv::COLOR_BGR2GRAY);
int border = 8; // minimum distance from border
bool reject = false;
for(unsigned int i=0; i<pointBuf.size(); ++i)
{
if(pointBuf[i].x < border || pointBuf[i].x > image.cols-border ||
pointBuf[i].y < border || pointBuf[i].y > image.rows-border)
{
reject = false;
break;
}
}
if(!reject)
{
float minSquareDistance = -1.0f;
for(unsigned int i=0; i<pointBuf.size()-1; ++i)
{
float d = cv::norm(pointBuf[i] - pointBuf[i+1]);
if(minSquareDistance == -1.0f || minSquareDistance > d)
{
minSquareDistance = d;
}
}
float radius = minSquareDistance/2.0f +0.5f;
cv::cornerSubPix( viewGray, pointBuf, cv::Size(radius, radius), cv::Size(-1,-1),
cv::TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1 ));
// verify if view is different from any previous samples
std::vector<float> params(4, 0);
getParams(pointBuf, boardSize_, imageSize_, params[0], params[1], params[2], params[3]);
bool add = true;
for(unsigned int i=0; i<imageParams_.size(); ++i)
{
if(fabs(params[0] - imageParams_[i].at(0)) < 0.1 && // x
fabs(params[1] - imageParams_[i].at(1)) < 0.1 && // y
fabs(params[2] - imageParams_[i].at(2)) < 0.1 && // size
fabs(params[3] - imageParams_[i].at(3)) < 0.1) // skew
{
add = false;
}
}
if(add)
{
UINFO("Added board. (x=%f, y=%f, size=%f, skew=%f)", params[0], params[1], params[2], params[3]);
imagePoints_.push_back(pointBuf);
imageParams_.push_back(params);
// update statistics
std::vector<float> xRange(2, imageParams_[0].at(0));
std::vector<float> yRange(2, imageParams_[0].at(1));
std::vector<float> sizeRange(2, imageParams_[0].at(2));
std::vector<float> skewRange(2, imageParams_[0].at(3));
for(unsigned int i=1; i<imageParams_.size(); ++i)
{
xRange[0] = imageParams_[i].at(0) < xRange[0] ? imageParams_[i].at(0) : xRange[0];
xRange[1] = imageParams_[i].at(0) > xRange[1] ? imageParams_[i].at(0) : xRange[1];
yRange[0] = imageParams_[i].at(1) < yRange[0] ? imageParams_[i].at(1) : yRange[0];
yRange[1] = imageParams_[i].at(1) > yRange[1] ? imageParams_[i].at(1) : yRange[1];
sizeRange[0] = imageParams_[i].at(2) < sizeRange[0] ? imageParams_[i].at(2) : sizeRange[0];
sizeRange[1] = imageParams_[i].at(2) > sizeRange[1] ? imageParams_[i].at(2) : sizeRange[1];
skewRange[0] = imageParams_[i].at(3) < skewRange[0] ? imageParams_[i].at(3) : skewRange[0];
skewRange[1] = imageParams_[i].at(3) > skewRange[1] ? imageParams_[i].at(3) : skewRange[1];
}
UINFO("Stats:");
UINFO(" Count = %d", (int)imagePoints_.size());
UINFO(" x = [%f -> %f]", xRange[0], xRange[1]);
UINFO(" y = [%f -> %f]", yRange[0], yRange[1]);
UINFO(" size = [%f -> %f]", sizeRange[0], sizeRange[1]);
UINFO(" skew = [%f -> %f]", skewRange[0], skewRange[1]);
float xGood = xRange[1] - xRange[0];
float yGood = yRange[1] - yRange[0];
float sizeGood = sizeRange[1] - sizeRange[0];
float skewGood = skewRange[1] - skewRange[0];
if((int)imagePoints_.size() > ui_->progressBar_count->maximum())
{
ui_->progressBar_count->setMaximum((int)imagePoints_.size());
}
ui_->progressBar_count->setValue((int)imagePoints_.size());
ui_->progressBar_x->setValue(xGood*100);
ui_->progressBar_y->setValue(yGood*100);
ui_->progressBar_size->setValue(sizeGood*100);
ui_->progressBar_skew->setValue(skewGood*100);
if(imagePoints_.size() >= COUNT_MIN && xGood > 0.5 && yGood > 0.5 && sizeGood > 0.4 && skewGood > 0.5)
{
ui_->pushButton_calibrate->setEnabled(true);
}
}
}
// Draw the corners.
cv::drawChessboardCorners( image, boardSize_, cv::Mat(pointBuf), found );
}
if(calibrated_ && ui_->checkBox_rectified->isChecked())
{
cv::Mat temp = image.clone();
cv::undistort(temp, image, cameraMatrix_, distCoeffs_);
}
//show frame
ui_->image_view->setImage(uCvMat2QImage(image));
}
void CalibrationDialog::restart()
{
// restart
calibrated_ = false;
imagePoints_.clear();
imageParams_.clear();
ui_->pushButton_calibrate->setEnabled(false);
ui_->pushButton_save->setEnabled(false);
ui_->checkBox_rectified->setEnabled(false);
ui_->progressBar_count->reset();
ui_->progressBar_count->setMaximum(COUNT_MIN);
ui_->progressBar_x->reset();
ui_->progressBar_y->reset();
ui_->progressBar_size->reset();
ui_->progressBar_skew->reset();
ui_->label_fx->setNum(0);
ui_->label_fy->setNum(0);
ui_->label_cx->setNum(0);
ui_->label_cy->setNum(0);
ui_->label_error->setNum(0);
ui_->lineEdit_K->clear();
ui_->lineEdit_D->clear();
}
void CalibrationDialog::calibrate()
{
//calibrate
std::vector<cv::Mat> rvecs, tvecs;
std::vector<float> reprojErrs;
double totalAvgErr = 0;
std::vector<std::vector<cv::Point3f> > objectPoints(1);
// compute board corner positions
for( int i = 0; i < boardSize_.height; ++i )
for( int j = 0; j < boardSize_.width; ++j )
objectPoints[0].push_back(cv::Point3f(float( j*squareSize_ ), float( i*squareSize_ ), 0));
objectPoints.resize(imagePoints_.size(),objectPoints[0]);
//Find intrinsic and extrinsic camera parameters
double rms = cv::calibrateCamera(objectPoints,
imagePoints_,
imageSize_,
cameraMatrix_,
distCoeffs_,
rvecs,
tvecs,
CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);
std::cout << "cameraMatrix = " << cameraMatrix_ << std::endl;
std::cout << "distCoeffs = " << distCoeffs_ << std::endl;
UINFO("Re-projection error reported by calibrateCamera: %f", rms);
calibrated_ = checkRange(cameraMatrix_) && checkRange(distCoeffs_);
// compute reprojection errors
std::vector<cv::Point2f> imagePoints2;
int i, totalPoints = 0;
double totalErr = 0, err;
reprojErrs.resize(objectPoints.size());
for( i = 0; i < (int)objectPoints.size(); ++i )
{
cv::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], cameraMatrix_,
distCoeffs_, imagePoints2);
err = cv::norm(cv::Mat(imagePoints_[i]), cv::Mat(imagePoints2), CV_L2);
int n = (int)objectPoints[i].size();
reprojErrs[i] = (float) std::sqrt(err*err/n);
totalErr += err*err;
totalPoints += n;
}
totalAvgErr = std::sqrt(totalErr/totalPoints);
UINFO("%s. avg re projection error = %f", calibrated_ ? "Calibration succeeded" : "Calibration failed", totalAvgErr);
if(calibrated_)
{
ui_->label_fx->setNum(cameraMatrix_.at<double>(0,0)); // K(0)
ui_->label_fy->setNum(cameraMatrix_.at<double>(1,1)); // K(4)
ui_->label_cx->setNum(cameraMatrix_.at<double>(0,2)); // K(2)
ui_->label_cy->setNum(cameraMatrix_.at<double>(1,2)); // K(5)
ui_->label_error->setNum(totalAvgErr);
std::stringstream strK, strD;
strK << cameraMatrix_;
strD << distCoeffs_;
ui_->lineEdit_K->setText(strK.str().c_str());
ui_->lineEdit_D->setText(strD.str().c_str());
ui_->checkBox_rectified->setEnabled(true);
ui_->checkBox_rectified->setChecked(true);
}
}
void CalibrationDialog::exit()
{
this->close();
}
float CalibrationDialog::getArea(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize)
{
//Get 2d image area of the detected checkerboard.
//The projected checkerboard is assumed to be a convex quadrilateral, and the area computed as
//|p X q|/2; see http://mathworld.wolfram.com/Quadrilateral.html.
cv::Point2f up_left = corners[0];
cv::Point2f up_right = corners[boardSize.width-1];
cv::Point2f down_right = corners[corners.size()-1];
cv::Point2f down_left = corners[corners.size()-boardSize.width];
cv::Point2f a = up_right - up_left;
cv::Point2f b = down_right - up_right;
cv::Point2f c = down_left - down_right;
cv::Point2f p = b + c;
cv::Point2f q = a + b;
return std::fabs(p.x*q.y - p.y*q.x) / 2.0f;
}
float CalibrationDialog::getSkew(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize)
{
// Get skew for given checkerboard detection.
// Scaled to [0,1], which 0 = no skew, 1 = high skew
// Skew is proportional to the divergence of three outside corners from 90 degrees.
cv::Point2f up_left = corners[0];
cv::Point2f up_right = corners[boardSize.width-1];
cv::Point2f down_right = corners[corners.size()-1];
// Return angle between lines ab, bc
cv::Point2f ab = up_left - up_right;
cv::Point2f cb = down_right - up_right;
float angle = std::acos(ab.dot(cb) / (cv::norm(ab) * cv::norm(cb)));
float r = 2.0f * std::fabs((CV_PI / 2.0f) - angle);
return r > 1.0f?1.0f:r;
}
// x -> [0, 1] (left, right)
// y -> [0, 1] (top, bottom)
// size -> [0, 1] (small -> big)
// skew -> [0, 1] (low, high)
void CalibrationDialog::getParams(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize, const cv::Size & imageSize,
float & x, float & y, float & size, float & skew)
{
float area = getArea(corners, boardSize);
size = std::sqrt(area / (imageSize.width * imageSize.height));
skew = getSkew(corners, boardSize);
float meanX = 0.0f;
float meanY = 0.0f;
for(unsigned int i=0; i<corners.size(); ++i)
{
meanX += corners[i].x;
meanY += corners[i].y;
}
meanX /= corners.size();
meanY /= corners.size();
x = meanX / imageSize.width;
y = meanY / imageSize.height;
}
} /* namespace rtabmap */

View File

@@ -0,0 +1,323 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>calibrationDialog</class>
<widget class="QDialog" name="calibrationDialog">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>629</width>
<height>452</height>
</rect>
</property>
<property name="windowTitle">
<string>Dialog</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_3">
<item>
<layout class="QHBoxLayout" name="horizontalLayout_2" stretch="1,0">
<item>
<layout class="QVBoxLayout" name="verticalLayout_2" stretch="1,0">
<item>
<widget class="UImageView" name="image_view" native="true"/>
</item>
<item>
<widget class="QCheckBox" name="checkBox_rectified">
<property name="text">
<string>Show rectified</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<layout class="QFormLayout" name="formLayout">
<item row="0" column="0">
<widget class="QLabel" name="label_5">
<property name="text">
<string>Count</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QProgressBar" name="progressBar_count">
<property name="value">
<number>24</number>
</property>
<property name="format">
<string>%p</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label">
<property name="text">
<string>X</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QProgressBar" name="progressBar_x">
<property name="value">
<number>24</number>
</property>
<property name="format">
<string>%p%</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_2">
<property name="text">
<string>Y</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QProgressBar" name="progressBar_y">
<property name="value">
<number>24</number>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_3">
<property name="text">
<string>Size</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QProgressBar" name="progressBar_size">
<property name="value">
<number>24</number>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_4">
<property name="text">
<string>Skew</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QProgressBar" name="progressBar_skew">
<property name="value">
<number>24</number>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QPushButton" name="pushButton_calibrate">
<property name="text">
<string>Calibrate</string>
</property>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout_2">
<item row="0" column="0">
<widget class="QLabel" name="label_8">
<property name="text">
<string>fx</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_fx">
<property name="text">
<string>0</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_9">
<property name="text">
<string>fy</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_fy">
<property name="text">
<string>0</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_10">
<property name="text">
<string>cx</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_cx">
<property name="text">
<string>0</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_11">
<property name="text">
<string>cy</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_cy">
<property name="text">
<string>0</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_6">
<property name="toolTip">
<string>Camera matrix</string>
</property>
<property name="text">
<string>K</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLineEdit" name="lineEdit_K">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_7">
<property name="toolTip">
<string>Distorsion coefficients</string>
</property>
<property name="text">
<string>D</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLineEdit" name="lineEdit_D">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QLabel" name="label_13">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLabel" name="label_14">
<property name="toolTip">
<string>Avg. reproduction error</string>
</property>
<property name="text">
<string>Error</string>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_error">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QPushButton" name="pushButton_restart">
<property name="text">
<string>Restart</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="pushButton_save">
<property name="text">
<string>Save</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QPushButton" name="pushButton_exit">
<property name="text">
<string>Exit</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
<customwidgets>
<customwidget>
<class>UImageView</class>
<extends>QWidget</extends>
<header>utilite/UImageView.h</header>
<container>1</container>
</customwidget>
</customwidgets>
<resources/>
<connections/>
</ui>