From c1497d9e927ceec6711da5f39072ba90b4da4724 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 22 Jul 2014 22:22:59 +0000 Subject: [PATCH] added CalibrationDialog class and Calibration tool app git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1587 f169173b-cf89-36c8-b27e-44dbe73f0c83 --- .../include/rtabmap/gui/CalibrationDialog.h | 67 ++++ guilib/src/CMakeLists.txt | 4 + guilib/src/CalibrationDialog.cpp | 358 ++++++++++++++++++ guilib/src/ui/calibrationDialog.ui | 323 ++++++++++++++++ tools/CMakeLists.txt | 8 +- 5 files changed, 757 insertions(+), 3 deletions(-) create mode 100644 guilib/include/rtabmap/gui/CalibrationDialog.h create mode 100644 guilib/src/CalibrationDialog.cpp create mode 100644 guilib/src/ui/calibrationDialog.ui diff --git a/guilib/include/rtabmap/gui/CalibrationDialog.h b/guilib/include/rtabmap/gui/CalibrationDialog.h new file mode 100644 index 00000000..a26f19e9 --- /dev/null +++ b/guilib/include/rtabmap/gui/CalibrationDialog.h @@ -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 +#include + +#include + +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 & corners, const cv::Size & boardSize); + float getSkew(const std::vector & 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 & 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 > imagePoints_; + std::vector > imageParams_; + cv::Size imageSize_; + bool calibrated_; + cv::Mat cameraMatrix_; + cv::Mat distCoeffs_; + + Ui_calibrationDialog * ui_; +}; + +} /* namespace rtabmap */ +#endif /* CALIBRATIONDIALOG_H_ */ diff --git a/guilib/src/CMakeLists.txt b/guilib/src/CMakeLists.txt index 3dae7c1d..778d7fac 100644 --- a/guilib/src/CMakeLists.txt +++ b/guilib/src/CMakeLists.txt @@ -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 diff --git a/guilib/src/CalibrationDialog.cpp b/guilib/src/CalibrationDialog.cpp new file mode 100644 index 00000000..2f03b8db --- /dev/null +++ b/guilib/src/CalibrationDialog.cpp @@ -0,0 +1,358 @@ +/* + * CalibrationDialog.cpp + * + * Created on: 2014-07-22 + * Author: Mathieu + */ + +#include "rtabmap/gui/CalibrationDialog.h" +#include "ui_calibrationDialog.h" + +#include +#include +#include +#include + +#include +#include + +#include + +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"); + + 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 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 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 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 params(4, 0); + getParams(pointBuf, boardSize_, imageSize_, params[0], params[1], params[2], params[3]); + + bool add = true; + for(unsigned int i=0; i xRange(2, imageParams_[0].at(0)); + std::vector yRange(2, imageParams_[0].at(1)); + std::vector sizeRange(2, imageParams_[0].at(2)); + std::vector skewRange(2, imageParams_[0].at(3)); + for(unsigned int i=1; i 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 rvecs, tvecs; + std::vector reprojErrs; + double totalAvgErr = 0; + + std::vector > 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 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(0,0)); // K(0) + ui_->label_fy->setNum(cameraMatrix_.at(1,1)); // K(4) + ui_->label_cx->setNum(cameraMatrix_.at(0,2)); // K(2) + ui_->label_cy->setNum(cameraMatrix_.at(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 & 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 & 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 & 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 + + calibrationDialog + + + + 0 + 0 + 629 + 452 + + + + Dialog + + + + + + + + + + + + + Show rectified + + + + + + + + + + + + + Count + + + + + + + 24 + + + %p + + + + + + + X + + + + + + + 24 + + + %p% + + + + + + + Y + + + + + + + 24 + + + + + + + Size + + + + + + + 24 + + + + + + + Skew + + + + + + + 24 + + + + + + + + + Calibrate + + + + + + + + + fx + + + + + + + 0 + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + fy + + + + + + + 0 + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + cx + + + + + + + 0 + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + cy + + + + + + + 0 + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Camera matrix + + + K + + + + + + + true + + + + + + + Distorsion coefficients + + + D + + + + + + + true + + + + + + + + + + + + + + Avg. reproduction error + + + Error + + + + + + + 0 + + + + + + + + + + + Restart + + + + + + + Save + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Exit + + + + + + + + + + UImageView + QWidget +
utilite/UImageView.h
+ 1 + + + + + diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index c766b966..e5298d82 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -2,14 +2,16 @@ ADD_SUBDIRECTORY( ConsoleApp ) ADD_SUBDIRECTORY( ImagesJoiner ) ADD_SUBDIRECTORY( VocabularyComparison ) -ADD_SUBDIRECTORY( OdometryViewer ) -ADD_SUBDIRECTORY( DataRecorder ) ADD_SUBDIRECTORY( ExtractObject ) ADD_SUBDIRECTORY( CameraRGBD ) + IF(TARGET rtabmap_gui) ADD_SUBDIRECTORY( DatabaseViewer ) ADD_SUBDIRECTORY( EpipolarGeometry ) + ADD_SUBDIRECTORY( OdometryViewer ) + ADD_SUBDIRECTORY( DataRecorder ) + ADD_SUBDIRECTORY( Calibration ) ELSE() - MESSAGE(STATUS "RTAB-Map GUI lib is not built, the databaseViewer and epipolarGeometry programs will not be built...") + MESSAGE(STATUS "RTAB-Map GUI lib is not built, some tools won't be built...") ENDIF() \ No newline at end of file