00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/gui/CalibrationDialog.h"
00029 #include "ui_calibrationDialog.h"
00030
00031 #include <opencv2/core/core.hpp>
00032 #include <opencv2/imgproc/imgproc.hpp>
00033 #include <opencv2/calib3d/calib3d.hpp>
00034 #include <opencv2/highgui/highgui.hpp>
00035
00036 #include <QFileDialog>
00037 #include <QMessageBox>
00038 #include <QCloseEvent>
00039
00040 #include <rtabmap/core/CameraEvent.h>
00041 #include <rtabmap/utilite/UCv2Qt.h>
00042
00043 #include <rtabmap/utilite/ULogger.h>
00044
00045 namespace rtabmap {
00046
00047 #define COUNT_MIN 40
00048
00049 CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirectory, bool switchImages, QWidget * parent) :
00050 QDialog(parent),
00051 stereo_(stereo),
00052 savingDirectory_(savingDirectory),
00053 processingData_(false),
00054 savedCalibration_(false)
00055 {
00056 imagePoints_.resize(2);
00057 imageParams_.resize(2);
00058 imageSize_.resize(2);
00059 stereoImagePoints_.resize(2);
00060 models_.resize(2);
00061
00062 minIrs_.resize(2);
00063 maxIrs_.resize(2);
00064 minIrs_[0] = 0x0000;
00065 maxIrs_[0] = 0x7fff;
00066 minIrs_[1] = 0x0000;
00067 maxIrs_[1] = 0x7fff;
00068
00069 qRegisterMetaType<cv::Mat>("cv::Mat");
00070
00071 ui_ = new Ui_calibrationDialog();
00072 ui_->setupUi(this);
00073
00074 connect(ui_->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
00075 connect(ui_->pushButton_restart, SIGNAL(clicked()), this, SLOT(restart()));
00076 connect(ui_->pushButton_save, SIGNAL(clicked()), this, SLOT(save()));
00077 connect(ui_->checkBox_switchImages, SIGNAL(stateChanged(int)), this, SLOT(restart()));
00078 connect(ui_->checkBox_unlock, SIGNAL(stateChanged(int)), SLOT(unlock()));
00079
00080 connect(ui_->spinBox_boardWidth, SIGNAL(valueChanged(int)), this, SLOT(setBoardWidth(int)));
00081 connect(ui_->spinBox_boardHeight, SIGNAL(valueChanged(int)), this, SLOT(setBoardHeight(int)));
00082 connect(ui_->doubleSpinBox_squareSize, SIGNAL(valueChanged(double)), this, SLOT(setSquareSize(double)));
00083 connect(ui_->spinBox_maxScale, SIGNAL(valueChanged(int)), this, SLOT(setMaxScale(int)));
00084
00085 connect(ui_->buttonBox, SIGNAL(rejected()), this, SLOT(close()));
00086
00087 ui_->image_view->setFocus();
00088
00089 ui_->progressBar_count->setMaximum(COUNT_MIN);
00090 ui_->progressBar_count->setFormat("%v");
00091 ui_->progressBar_count_2->setMaximum(COUNT_MIN);
00092 ui_->progressBar_count_2->setFormat("%v");
00093
00094 ui_->radioButton_raw->setChecked(true);
00095
00096 ui_->checkBox_switchImages->setChecked(switchImages);
00097
00098 this->setStereoMode(stereo_);
00099 }
00100
00101 CalibrationDialog::~CalibrationDialog()
00102 {
00103 this->unregisterFromEventsManager();
00104 delete ui_;
00105 }
00106
00107 void CalibrationDialog::saveSettings(QSettings & settings, const QString & group) const
00108 {
00109 if(!group.isEmpty())
00110 {
00111 settings.beginGroup(group);
00112 }
00113 settings.setValue("board_width", ui_->spinBox_boardWidth->value());
00114 settings.setValue("board_height", ui_->spinBox_boardHeight->value());
00115 settings.setValue("board_square_size", ui_->doubleSpinBox_squareSize->value());
00116 settings.setValue("max_scale", ui_->spinBox_maxScale->value());
00117 settings.setValue("geometry", this->saveGeometry());
00118 if(!group.isEmpty())
00119 {
00120 settings.endGroup();
00121 }
00122 }
00123
00124 void CalibrationDialog::loadSettings(QSettings & settings, const QString & group)
00125 {
00126 if(!group.isEmpty())
00127 {
00128 settings.beginGroup(group);
00129 }
00130 this->setBoardWidth(settings.value("board_width", ui_->spinBox_boardWidth->value()).toInt());
00131 this->setBoardHeight(settings.value("board_height", ui_->spinBox_boardHeight->value()).toInt());
00132 this->setSquareSize(settings.value("board_square_size", ui_->doubleSpinBox_squareSize->value()).toDouble());
00133 this->setMaxScale(settings.value("max_scale", ui_->spinBox_maxScale->value()).toDouble());
00134 QByteArray bytes = settings.value("geometry", QByteArray()).toByteArray();
00135 if(!bytes.isEmpty())
00136 {
00137 this->restoreGeometry(bytes);
00138 }
00139 if(!group.isEmpty())
00140 {
00141 settings.endGroup();
00142 }
00143 }
00144
00145 void CalibrationDialog::resetSettings()
00146 {
00147 this->setBoardWidth(8);
00148 this->setBoardHeight(6);
00149 this->setSquareSize(0.033);
00150 }
00151
00152 void CalibrationDialog::setSwitchedImages(bool switched)
00153 {
00154 ui_->checkBox_switchImages->setChecked(switched);
00155 }
00156
00157 void CalibrationDialog::setStereoMode(bool stereo)
00158 {
00159 this->restart();
00160
00161 stereo_ = stereo;
00162 ui_->progressBar_x_2->setVisible(stereo_);
00163 ui_->progressBar_y_2->setVisible(stereo_);
00164 ui_->progressBar_size_2->setVisible(stereo_);
00165 ui_->progressBar_skew_2->setVisible(stereo_);
00166 ui_->progressBar_count_2->setVisible(stereo_);
00167 ui_->label_right->setVisible(stereo_);
00168 ui_->image_view_2->setVisible(stereo_);
00169 ui_->label_fx_2->setVisible(stereo_);
00170 ui_->label_fy_2->setVisible(stereo_);
00171 ui_->label_cx_2->setVisible(stereo_);
00172 ui_->label_cy_2->setVisible(stereo_);
00173 ui_->label_error_2->setVisible(stereo_);
00174 ui_->label_baseline->setVisible(stereo_);
00175 ui_->label_baseline_name->setVisible(stereo_);
00176 ui_->lineEdit_K_2->setVisible(stereo_);
00177 ui_->lineEdit_D_2->setVisible(stereo_);
00178 ui_->lineEdit_R_2->setVisible(stereo_);
00179 ui_->lineEdit_P_2->setVisible(stereo_);
00180 ui_->radioButton_stereoRectified->setVisible(stereo_);
00181 ui_->checkBox_switchImages->setVisible(stereo_);
00182 }
00183
00184 void CalibrationDialog::setBoardWidth(int width)
00185 {
00186 if(width != ui_->spinBox_boardWidth->value())
00187 {
00188 ui_->spinBox_boardWidth->setValue(width);
00189 this->restart();
00190 }
00191 }
00192
00193 void CalibrationDialog::setBoardHeight(int height)
00194 {
00195 if(height != ui_->spinBox_boardHeight->value())
00196 {
00197 ui_->spinBox_boardHeight->setValue(height);
00198 this->restart();
00199 }
00200 }
00201
00202 void CalibrationDialog::setSquareSize(double size)
00203 {
00204 if(size != ui_->doubleSpinBox_squareSize->value())
00205 {
00206 ui_->doubleSpinBox_squareSize->setValue(size);
00207 this->restart();
00208 }
00209 }
00210
00211 void CalibrationDialog::setMaxScale(int scale)
00212 {
00213 if(scale != ui_->spinBox_maxScale->value())
00214 {
00215 ui_->spinBox_maxScale->setValue(scale);
00216 }
00217 }
00218
00219 void CalibrationDialog::closeEvent(QCloseEvent* event)
00220 {
00221 if(!savedCalibration_ && models_[0].isValidForRectification() &&
00222 (!stereo_ ||
00223 (stereoModel_.isValidForRectification() &&
00224 (!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0))))
00225 {
00226 QMessageBox::StandardButton b = QMessageBox::question(this, tr("Save calibration?"),
00227 tr("The camera is calibrated but you didn't "
00228 "save the calibration, do you want to save it?"),
00229 QMessageBox::Yes | QMessageBox::Ignore | QMessageBox::Cancel, QMessageBox::Yes);
00230 event->ignore();
00231 if(b == QMessageBox::Yes)
00232 {
00233 if(this->save())
00234 {
00235 event->accept();
00236 }
00237 }
00238 else if(b == QMessageBox::Ignore)
00239 {
00240 event->accept();
00241 }
00242 }
00243 else
00244 {
00245 event->accept();
00246 }
00247 if(event->isAccepted())
00248 {
00249 this->unregisterFromEventsManager();
00250 }
00251 }
00252
00253 void CalibrationDialog::handleEvent(UEvent * event)
00254 {
00255 if(!processingData_)
00256 {
00257 if(event->getClassName().compare("CameraEvent") == 0)
00258 {
00259 rtabmap::CameraEvent * e = (rtabmap::CameraEvent *)event;
00260 if(e->getCode() == rtabmap::CameraEvent::kCodeData)
00261 {
00262 processingData_ = true;
00263 QMetaObject::invokeMethod(this, "processImages",
00264 Q_ARG(cv::Mat, e->data().imageRaw()),
00265 Q_ARG(cv::Mat, e->data().depthOrRightRaw()),
00266 Q_ARG(QString, QString(e->cameraName().c_str())));
00267 }
00268 }
00269 }
00270 }
00271
00272 void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName)
00273 {
00274 UDEBUG("Processing images");
00275 processingData_ = true;
00276 if(cameraName_.isEmpty())
00277 {
00278 cameraName_ = "0000";
00279 if(!cameraName.isEmpty())
00280 {
00281 cameraName_ = cameraName;
00282 }
00283 }
00284 if(ui_->label_serial->text().isEmpty())
00285 {
00286 ui_->label_serial->setText(cameraName_);
00287
00288 }
00289 std::vector<cv::Mat> inputRawImages(2);
00290 if(ui_->checkBox_switchImages->isChecked())
00291 {
00292 inputRawImages[0] = imageRight;
00293 inputRawImages[1] = imageLeft;
00294 }
00295 else
00296 {
00297 inputRawImages[0] = imageLeft;
00298 inputRawImages[1] = imageRight;
00299 }
00300
00301 std::vector<cv::Mat> images(2);
00302 images[0] = inputRawImages[0];
00303 images[1] = inputRawImages[1];
00304 imageSize_[0] = images[0].size();
00305 imageSize_[1] = images[1].size();
00306
00307 bool boardFound[2] = {false};
00308 bool boardAccepted[2] = {false};
00309 bool readyToCalibrate[2] = {false};
00310
00311 std::vector<std::vector<cv::Point2f> > pointBuf(2);
00312
00313 bool depthDetected = false;
00314 for(int id=0; id<(stereo_?2:1); ++id)
00315 {
00316 cv::Mat viewGray;
00317 if(!images[id].empty())
00318 {
00319 if(images[id].type() == CV_16UC1)
00320 {
00321 double min, max;
00322 cv::minMaxLoc(images[id], &min, &max);
00323 UDEBUG("Camera IR %d: min=%f max=%f", id, min, max);
00324 if(minIrs_[id] == 0)
00325 {
00326 minIrs_[id] = min;
00327 }
00328 if(maxIrs_[id] == 0x7fff)
00329 {
00330 maxIrs_[id] = max;
00331 }
00332
00333 depthDetected = true;
00334
00335 const float factor = 255.0f / float((maxIrs_[id] - minIrs_[id]));
00336 viewGray = cv::Mat(images[id].rows, images[id].cols, CV_8UC1);
00337 for(int i=0; i<images[id].rows; ++i)
00338 {
00339 for(int j=0; j<images[id].cols; ++j)
00340 {
00341 viewGray.at<unsigned char>(i, j) = (unsigned char)std::min(float(std::max(images[id].at<unsigned short>(i,j) - minIrs_[id], 0)) * factor, 255.0f);
00342 }
00343 }
00344 cvtColor(viewGray, images[id], cv::COLOR_GRAY2BGR);
00345 }
00346 else if(images[id].channels() == 3)
00347 {
00348 cvtColor(images[id], viewGray, cv::COLOR_BGR2GRAY);
00349 }
00350 else
00351 {
00352 viewGray = images[id];
00353 cvtColor(viewGray, images[id], cv::COLOR_GRAY2BGR);
00354 }
00355 }
00356 else
00357 {
00358 UERROR("Image %d is empty!! Should not!", id);
00359 }
00360
00361 minIrs_[id] = 0;
00362 maxIrs_[id] = 0x7FFF;
00363
00364
00365 if(!ui_->pushButton_save->isEnabled())
00366 {
00367 cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
00368 if(!viewGray.empty())
00369 {
00370 int flags = CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE;
00371
00372 if(!viewGray.empty())
00373 {
00374 int maxScale = ui_->spinBox_maxScale->value();
00375 for( int scale = 1; scale <= maxScale; scale++ )
00376 {
00377 cv::Mat timg;
00378 if( scale == 1 )
00379 timg = viewGray;
00380 else
00381 cv::resize(viewGray, timg, cv::Size(), scale, scale, CV_INTER_CUBIC);
00382 boardFound[id] = cv::findChessboardCorners(timg, boardSize, pointBuf[id], flags);
00383 if(boardFound[id])
00384 {
00385 if( scale > 1 )
00386 {
00387 cv::Mat cornersMat(pointBuf[id]);
00388 cornersMat *= 1./scale;
00389 }
00390 break;
00391 }
00392 }
00393 }
00394 }
00395
00396 if(boardFound[id])
00397 {
00398
00399 float minSquareDistance = -1.0f;
00400 for(unsigned int i=0; i<pointBuf[id].size()-1; ++i)
00401 {
00402 float d = cv::norm(pointBuf[id][i] - pointBuf[id][i+1]);
00403 if(minSquareDistance == -1.0f || minSquareDistance > d)
00404 {
00405 minSquareDistance = d;
00406 }
00407 }
00408 float radius = minSquareDistance/2.0f +0.5f;
00409 cv::cornerSubPix( viewGray, pointBuf[id], cv::Size(radius, radius), cv::Size(-1,-1),
00410 cv::TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1 ));
00411
00412
00413 cv::drawChessboardCorners(images[id], boardSize, cv::Mat(pointBuf[id]), boardFound[id]);
00414
00415 std::vector<float> params(4,0);
00416 getParams(pointBuf[id], boardSize, imageSize_[id], params[0], params[1], params[2], params[3]);
00417
00418 bool addSample = true;
00419 for(unsigned int i=0; i<imageParams_[id].size(); ++i)
00420 {
00421 if(fabs(params[0] - imageParams_[id][i].at(0)) < 0.1 &&
00422 fabs(params[1] - imageParams_[id][i].at(1)) < 0.1 &&
00423 fabs(params[2] - imageParams_[id][i].at(2)) < 0.05 &&
00424 fabs(params[3] - imageParams_[id][i].at(3)) < 0.1)
00425 {
00426 addSample = false;
00427 }
00428 }
00429 if(addSample)
00430 {
00431 boardAccepted[id] = true;
00432
00433 imagePoints_[id].push_back(pointBuf[id]);
00434 imageParams_[id].push_back(params);
00435
00436 UINFO("[%d] Added board, total=%d. (x=%f, y=%f, size=%f, skew=%f)", id, (int)imagePoints_[id].size(), params[0], params[1], params[2], params[3]);
00437 }
00438
00439
00440 std::vector<float> xRange(2, imageParams_[id][0].at(0));
00441 std::vector<float> yRange(2, imageParams_[id][0].at(1));
00442 std::vector<float> sizeRange(2, imageParams_[id][0].at(2));
00443 std::vector<float> skewRange(2, imageParams_[id][0].at(3));
00444 for(unsigned int i=1; i<imageParams_[id].size(); ++i)
00445 {
00446 xRange[0] = imageParams_[id][i].at(0) < xRange[0] ? imageParams_[id][i].at(0) : xRange[0];
00447 xRange[1] = imageParams_[id][i].at(0) > xRange[1] ? imageParams_[id][i].at(0) : xRange[1];
00448 yRange[0] = imageParams_[id][i].at(1) < yRange[0] ? imageParams_[id][i].at(1) : yRange[0];
00449 yRange[1] = imageParams_[id][i].at(1) > yRange[1] ? imageParams_[id][i].at(1) : yRange[1];
00450 sizeRange[0] = imageParams_[id][i].at(2) < sizeRange[0] ? imageParams_[id][i].at(2) : sizeRange[0];
00451 sizeRange[1] = imageParams_[id][i].at(2) > sizeRange[1] ? imageParams_[id][i].at(2) : sizeRange[1];
00452 skewRange[0] = imageParams_[id][i].at(3) < skewRange[0] ? imageParams_[id][i].at(3) : skewRange[0];
00453 skewRange[1] = imageParams_[id][i].at(3) > skewRange[1] ? imageParams_[id][i].at(3) : skewRange[1];
00454 }
00455
00456
00457
00458
00459
00460
00461
00462 float xGood = xRange[1] - xRange[0];
00463 float yGood = yRange[1] - yRange[0];
00464 float sizeGood = sizeRange[1] - sizeRange[0];
00465 float skewGood = skewRange[1] - skewRange[0];
00466
00467 if(id == 0)
00468 {
00469 ui_->progressBar_x->setValue(xGood*100);
00470 ui_->progressBar_y->setValue(yGood*100);
00471 ui_->progressBar_size->setValue(sizeGood*100);
00472 ui_->progressBar_skew->setValue(skewGood*100);
00473 if((int)imagePoints_[id].size() > ui_->progressBar_count->maximum())
00474 {
00475 ui_->progressBar_count->setMaximum((int)imagePoints_[id].size());
00476 }
00477 ui_->progressBar_count->setValue((int)imagePoints_[id].size());
00478 }
00479 else
00480 {
00481 ui_->progressBar_x_2->setValue(xGood*100);
00482 ui_->progressBar_y_2->setValue(yGood*100);
00483 ui_->progressBar_size_2->setValue(sizeGood*100);
00484 ui_->progressBar_skew_2->setValue(skewGood*100);
00485
00486 if((int)imagePoints_[id].size() > ui_->progressBar_count_2->maximum())
00487 {
00488 ui_->progressBar_count_2->setMaximum((int)imagePoints_[id].size());
00489 }
00490 ui_->progressBar_count_2->setValue((int)imagePoints_[id].size());
00491 }
00492
00493 if(imagePoints_[id].size() >= COUNT_MIN && xGood > 0.5 && yGood > 0.5 && sizeGood > 0.4 && skewGood > 0.5)
00494 {
00495 readyToCalibrate[id] = true;
00496 }
00497
00498
00499 if(inputRawImages[id].type() == CV_16UC1)
00500 {
00501
00502 minIrs_[id] = 0xFFFF;
00503 maxIrs_[id] = 0;
00504 for(size_t i = 0; i < pointBuf[id].size(); ++i)
00505 {
00506 const cv::Point2f &p = pointBuf[id][i];
00507 cv::Rect roi(std::max(0, (int)p.x - 3), std::max(0, (int)p.y - 3), 6, 6);
00508
00509 roi.width = std::min(roi.width, inputRawImages[id].cols - roi.x);
00510 roi.height = std::min(roi.height, inputRawImages[id].rows - roi.y);
00511
00512
00513 double min, max;
00514 cv::minMaxLoc(inputRawImages[id](roi), &min, &max);
00515 if(min < minIrs_[id])
00516 {
00517 minIrs_[id] = min;
00518 }
00519 if(max > maxIrs_[id])
00520 {
00521 maxIrs_[id] = max;
00522 }
00523 }
00524 }
00525 }
00526 }
00527 }
00528 ui_->label_baseline->setVisible(!depthDetected);
00529 ui_->label_baseline_name->setVisible(!depthDetected);
00530
00531 if(stereo_ && ((boardAccepted[0] && boardFound[1]) || (boardAccepted[1] && boardFound[0])))
00532 {
00533 stereoImagePoints_[0].push_back(pointBuf[0]);
00534 stereoImagePoints_[1].push_back(pointBuf[1]);
00535 UINFO("Add stereo image points (size=%d)", (int)stereoImagePoints_[0].size());
00536 }
00537
00538 if(!stereo_ && readyToCalibrate[0])
00539 {
00540 unlock();
00541 }
00542 else if(stereo_ && readyToCalibrate[0] && readyToCalibrate[1] && stereoImagePoints_[0].size())
00543 {
00544 unlock();
00545 }
00546
00547 if(ui_->radioButton_rectified->isChecked())
00548 {
00549 if(models_[0].isValidForRectification())
00550 {
00551 images[0] = models_[0].rectifyImage(images[0]);
00552 }
00553 if(models_[1].isValidForRectification())
00554 {
00555 images[1] = models_[1].rectifyImage(images[1]);
00556 }
00557 }
00558 else if(ui_->radioButton_stereoRectified->isChecked() &&
00559 (stereoModel_.left().isValidForRectification() &&
00560 stereoModel_.right().isValidForRectification()&&
00561 (!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0)))
00562 {
00563 images[0] = stereoModel_.left().rectifyImage(images[0]);
00564 images[1] = stereoModel_.right().rectifyImage(images[1]);
00565 }
00566
00567 if(ui_->checkBox_showHorizontalLines->isChecked())
00568 {
00569 for(int id=0; id<(stereo_?2:1); ++id)
00570 {
00571 int step = imageSize_[id].height/16;
00572 for(int i=step; i<imageSize_[id].height; i+=step)
00573 {
00574 cv::line(images[id], cv::Point(0, i), cv::Point(imageSize_[id].width, i), CV_RGB(0,255,0));
00575 }
00576 }
00577 }
00578
00579 ui_->label_left->setText(tr("%1x%2").arg(images[0].cols).arg(images[0].rows));
00580
00581
00582 ui_->image_view->setImage(uCvMat2QImage(images[0]).mirrored(ui_->checkBox_mirror->isChecked(), false));
00583 if(stereo_)
00584 {
00585 ui_->label_right->setText(tr("%1x%2").arg(images[1].cols).arg(images[1].rows));
00586 ui_->image_view_2->setImage(uCvMat2QImage(images[1]).mirrored(ui_->checkBox_mirror->isChecked(), false));
00587 }
00588 processingData_ = false;
00589 }
00590
00591 void CalibrationDialog::restart()
00592 {
00593
00594 savedCalibration_ = false;
00595 imagePoints_[0].clear();
00596 imagePoints_[1].clear();
00597 imageParams_[0].clear();
00598 imageParams_[1].clear();
00599 stereoImagePoints_[0].clear();
00600 stereoImagePoints_[1].clear();
00601 models_[0] = CameraModel();
00602 models_[1] = CameraModel();
00603 stereoModel_ = StereoCameraModel();
00604 cameraName_.clear();
00605 minIrs_[0] = 0x0000;
00606 maxIrs_[0] = 0x7fff;
00607 minIrs_[1] = 0x0000;
00608 maxIrs_[1] = 0x7fff;
00609
00610 ui_->pushButton_calibrate->setEnabled(ui_->checkBox_unlock->isChecked());
00611 ui_->pushButton_save->setEnabled(false);
00612 ui_->radioButton_raw->setChecked(true);
00613 ui_->radioButton_rectified->setEnabled(false);
00614 ui_->radioButton_stereoRectified->setEnabled(false);
00615
00616 ui_->progressBar_count->reset();
00617 ui_->progressBar_count->setMaximum(COUNT_MIN);
00618 ui_->progressBar_x->reset();
00619 ui_->progressBar_y->reset();
00620 ui_->progressBar_size->reset();
00621 ui_->progressBar_skew->reset();
00622
00623 ui_->progressBar_count_2->reset();
00624 ui_->progressBar_count_2->setMaximum(COUNT_MIN);
00625 ui_->progressBar_x_2->reset();
00626 ui_->progressBar_y_2->reset();
00627 ui_->progressBar_size_2->reset();
00628 ui_->progressBar_skew_2->reset();
00629
00630 ui_->label_serial->clear();
00631 ui_->label_fx->setNum(0);
00632 ui_->label_fy->setNum(0);
00633 ui_->label_cx->setNum(0);
00634 ui_->label_cy->setNum(0);
00635 ui_->label_baseline->setNum(0);
00636 ui_->label_error->setNum(0);
00637 ui_->lineEdit_K->clear();
00638 ui_->lineEdit_D->clear();
00639 ui_->lineEdit_R->clear();
00640 ui_->lineEdit_P->clear();
00641 ui_->label_fx_2->setNum(0);
00642 ui_->label_fy_2->setNum(0);
00643 ui_->label_cx_2->setNum(0);
00644 ui_->label_cy_2->setNum(0);
00645 ui_->lineEdit_K_2->clear();
00646 ui_->lineEdit_D_2->clear();
00647 ui_->lineEdit_R_2->clear();
00648 ui_->lineEdit_P_2->clear();
00649 }
00650
00651 void CalibrationDialog::unlock()
00652 {
00653 ui_->pushButton_calibrate->setEnabled(true);
00654 }
00655
00656 void CalibrationDialog::calibrate()
00657 {
00658 processingData_ = true;
00659 savedCalibration_ = false;
00660
00661 QMessageBox mb(QMessageBox::Information,
00662 tr("Calibrating..."),
00663 tr("Operation in progress..."));
00664 mb.show();
00665 QApplication::processEvents();
00666 uSleep(100);
00667 QApplication::processEvents();
00668
00669 std::vector<std::vector<cv::Point3f> > objectPoints(1);
00670 cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
00671 float squareSize = ui_->doubleSpinBox_squareSize->value();
00672
00673 for( int i = 0; i < boardSize.height; ++i )
00674 for( int j = 0; j < boardSize.width; ++j )
00675 objectPoints[0].push_back(cv::Point3f(float( j*squareSize ), float( i*squareSize ), 0));
00676
00677 for(int id=0; id<(stereo_?2:1); ++id)
00678 {
00679 UINFO("Calibrating camera %d (samples=%d)", id, (int)imagePoints_[id].size());
00680
00681 objectPoints.resize(imagePoints_[id].size(), objectPoints[0]);
00682
00683
00684 std::vector<cv::Mat> rvecs, tvecs;
00685 std::vector<float> reprojErrs;
00686 cv::Mat K, D;
00687 K = cv::Mat::eye(3,3,CV_64FC1);
00688 UINFO("calibrate!");
00689
00690 double rms = cv::calibrateCamera(objectPoints,
00691 imagePoints_[id],
00692 imageSize_[id],
00693 K,
00694 D,
00695 rvecs,
00696 tvecs);
00697
00698 UINFO("Re-projection error reported by calibrateCamera: %f", rms);
00699
00700
00701 std::vector<cv::Point2f> imagePoints2;
00702 int i, totalPoints = 0;
00703 double totalErr = 0, err;
00704 reprojErrs.resize(objectPoints.size());
00705
00706 for( i = 0; i < (int)objectPoints.size(); ++i )
00707 {
00708 cv::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], K, D, imagePoints2);
00709 err = cv::norm(cv::Mat(imagePoints_[id][i]), cv::Mat(imagePoints2), CV_L2);
00710
00711 int n = (int)objectPoints[i].size();
00712 reprojErrs[i] = (float) std::sqrt(err*err/n);
00713 totalErr += err*err;
00714 totalPoints += n;
00715 }
00716
00717 double totalAvgErr = std::sqrt(totalErr/totalPoints);
00718
00719 UINFO("avg re projection error = %f", totalAvgErr);
00720
00721 cv::Mat P(3,4,CV_64FC1);
00722 P.at<double>(2,3) = 1;
00723 K.copyTo(P.colRange(0,3).rowRange(0,3));
00724
00725 std::cout << "K = " << K << std::endl;
00726 std::cout << "D = " << D << std::endl;
00727 std::cout << "width = " << imageSize_[id].width << std::endl;
00728 std::cout << "height = " << imageSize_[id].height << std::endl;
00729
00730 models_[id] = CameraModel(cameraName_.toStdString(), imageSize_[id], K, D, cv::Mat::eye(3,3,CV_64FC1), P);
00731
00732 if(id == 0)
00733 {
00734 ui_->label_fx->setNum(models_[id].fx());
00735 ui_->label_fy->setNum(models_[id].fy());
00736 ui_->label_cx->setNum(models_[id].cx());
00737 ui_->label_cy->setNum(models_[id].cy());
00738 ui_->label_error->setNum(totalAvgErr);
00739
00740 std::stringstream strK, strD, strR, strP;
00741 strK << models_[id].K_raw();
00742 strD << models_[id].D_raw();
00743 strR << models_[id].R();
00744 strP << models_[id].P();
00745 ui_->lineEdit_K->setText(strK.str().c_str());
00746 ui_->lineEdit_D->setText(strD.str().c_str());
00747 ui_->lineEdit_R->setText(strR.str().c_str());
00748 ui_->lineEdit_P->setText(strP.str().c_str());
00749 }
00750 else
00751 {
00752 ui_->label_fx_2->setNum(models_[id].fx());
00753 ui_->label_fy_2->setNum(models_[id].fy());
00754 ui_->label_cx_2->setNum(models_[id].cx());
00755 ui_->label_cy_2->setNum(models_[id].cy());
00756 ui_->label_error_2->setNum(totalAvgErr);
00757
00758 std::stringstream strK, strD, strR, strP;
00759 strK << models_[id].K_raw();
00760 strD << models_[id].D_raw();
00761 strR << models_[id].R();
00762 strP << models_[id].P();
00763 ui_->lineEdit_K_2->setText(strK.str().c_str());
00764 ui_->lineEdit_D_2->setText(strD.str().c_str());
00765 ui_->lineEdit_R_2->setText(strR.str().c_str());
00766 ui_->lineEdit_P_2->setText(strP.str().c_str());
00767 }
00768 }
00769
00770 if(stereo_ && models_[0].isValidForRectification() && models_[1].isValidForRectification())
00771 {
00772 UINFO("stereo calibration (samples=%d)...", (int)stereoImagePoints_[0].size());
00773 cv::Size imageSize = imageSize_[0].width > imageSize_[1].width?imageSize_[0]:imageSize_[1];
00774 cv::Mat R, T, E, F;
00775
00776 std::vector<std::vector<cv::Point3f> > objectPoints(1);
00777 cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
00778 float squareSize = ui_->doubleSpinBox_squareSize->value();
00779
00780 for( int i = 0; i < boardSize.height; ++i )
00781 for( int j = 0; j < boardSize.width; ++j )
00782 objectPoints[0].push_back(cv::Point3f(float( j*squareSize ), float( i*squareSize ), 0));
00783 objectPoints.resize(stereoImagePoints_[0].size(), objectPoints[0]);
00784
00785
00786 #if CV_MAJOR_VERSION < 3
00787 double rms = cv::stereoCalibrate(
00788 objectPoints,
00789 stereoImagePoints_[0],
00790 stereoImagePoints_[1],
00791 models_[0].K_raw(), models_[0].D_raw(),
00792 models_[1].K_raw(), models_[1].D_raw(),
00793 imageSize, R, T, E, F,
00794 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
00795 cv::CALIB_FIX_INTRINSIC);
00796 #else
00797 double rms = cv::stereoCalibrate(
00798 objectPoints,
00799 stereoImagePoints_[0],
00800 stereoImagePoints_[1],
00801 models_[0].K_raw(), models_[0].D_raw(),
00802 models_[1].K_raw(), models_[1].D_raw(),
00803 imageSize, R, T, E, F,
00804 cv::CALIB_FIX_INTRINSIC,
00805 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
00806 #endif
00807 UINFO("stereo calibration... done with RMS error=%f", rms);
00808
00809 std::cout << "R = " << R << std::endl;
00810 std::cout << "T = " << T << std::endl;
00811 std::cout << "E = " << E << std::endl;
00812 std::cout << "F = " << F << std::endl;
00813
00814 if(imageSize_[0] == imageSize_[1])
00815 {
00816
00817
00818 cv::Mat R1, R2, P1, P2, Q;
00819 cv::stereoRectify(models_[0].K_raw(), models_[0].D_raw(),
00820 models_[1].K_raw(), models_[1].D_raw(),
00821 imageSize, R, T, R1, R2, P1, P2, Q,
00822 cv::CALIB_ZERO_DISPARITY, 0, imageSize);
00823
00824 double err = 0;
00825 int npoints = 0;
00826 std::vector<cv::Vec3f> lines[2];
00827 UINFO("Computing avg re-projection error...");
00828 for(unsigned int i = 0; i < stereoImagePoints_[0].size(); i++ )
00829 {
00830 int npt = (int)stereoImagePoints_[0][i].size();
00831
00832 cv::Mat imgpt0 = cv::Mat(stereoImagePoints_[0][i]);
00833 cv::Mat imgpt1 = cv::Mat(stereoImagePoints_[1][i]);
00834 cv::undistortPoints(imgpt0, imgpt0, models_[0].K_raw(), models_[0].D_raw(), R1, P1);
00835 cv::undistortPoints(imgpt1, imgpt1, models_[1].K_raw(), models_[1].D_raw(), R2, P2);
00836 computeCorrespondEpilines(imgpt0, 1, F, lines[0]);
00837 computeCorrespondEpilines(imgpt1, 2, F, lines[1]);
00838
00839 for(int j = 0; j < npt; j++ )
00840 {
00841 double errij = fabs(stereoImagePoints_[0][i][j].x*lines[1][j][0] +
00842 stereoImagePoints_[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
00843 fabs(stereoImagePoints_[1][i][j].x*lines[0][j][0] +
00844 stereoImagePoints_[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
00845 err += errij;
00846 }
00847 npoints += npt;
00848 }
00849 double totalAvgErr = err/(double)npoints;
00850 UINFO("stereo avg re projection error = %f", totalAvgErr);
00851
00852 stereoModel_ = StereoCameraModel(
00853 cameraName_.toStdString(),
00854 imageSize_[0], models_[0].K_raw(), models_[0].D_raw(), R1, P1,
00855 imageSize_[1], models_[1].K_raw(), models_[1].D_raw(), R2, P2,
00856 R, T, E, F);
00857 }
00858 else
00859 {
00860
00861 stereoModel_ = StereoCameraModel(
00862 cameraName_.toStdString(),
00863 imageSize_[0], models_[0].K_raw(), models_[0].D_raw(), models_[0].R(), models_[0].P(),
00864 imageSize_[1], models_[1].K_raw(), models_[1].D_raw(), models_[1].R(), models_[1].P(),
00865 R, T, E, F);
00866 }
00867
00868 std::stringstream strR1, strP1, strR2, strP2;
00869 strR1 << stereoModel_.left().R();
00870 strP1 << stereoModel_.left().P();
00871 strR2 << stereoModel_.right().R();
00872 strP2 << stereoModel_.right().P();
00873 ui_->lineEdit_R->setText(strR1.str().c_str());
00874 ui_->lineEdit_P->setText(strP1.str().c_str());
00875 ui_->lineEdit_R_2->setText(strR2.str().c_str());
00876 ui_->lineEdit_P_2->setText(strP2.str().c_str());
00877
00878 ui_->label_baseline->setNum(stereoModel_.baseline());
00879
00880 }
00881
00882 if(stereo_ &&
00883 stereoModel_.isValidForRectification())
00884 {
00885 stereoModel_.initRectificationMap();
00886 models_[0].initRectificationMap();
00887 models_[1].initRectificationMap();
00888 ui_->radioButton_rectified->setEnabled(true);
00889 ui_->radioButton_stereoRectified->setEnabled(true);
00890 ui_->radioButton_stereoRectified->setChecked(true);
00891 ui_->pushButton_save->setEnabled(true);
00892 }
00893 else if(models_[0].isValidForRectification())
00894 {
00895 models_[0].initRectificationMap();
00896 ui_->radioButton_rectified->setEnabled(true);
00897 ui_->radioButton_rectified->setChecked(true);
00898 ui_->pushButton_save->setEnabled(!stereo_);
00899 }
00900
00901 UINFO("End calibration");
00902 processingData_ = false;
00903 }
00904
00905 bool CalibrationDialog::save()
00906 {
00907 bool saved = false;
00908 processingData_ = true;
00909 if(!stereo_)
00910 {
00911 UASSERT(models_[0].isValidForRectification());
00912 QString cameraName = models_[0].name().c_str();
00913 QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_+"/"+cameraName+".yaml", "*.yaml");
00914
00915 if(!filePath.isEmpty())
00916 {
00917 QString name = QFileInfo(filePath).baseName();
00918 QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
00919 models_[0].setName(name.toStdString());
00920 if(models_[0].save(dir.toStdString()))
00921 {
00922 QMessageBox::information(this, tr("Export"), tr("Calibration file saved to \"%1\".").arg(filePath));
00923 UINFO("Saved \"%s\"!", filePath.toStdString().c_str());
00924 savedCalibration_ = true;
00925 saved = true;
00926 }
00927 else
00928 {
00929 UERROR("Error saving \"%s\"", filePath.toStdString().c_str());
00930 }
00931 }
00932 }
00933 else
00934 {
00935 UASSERT(stereoModel_.left().isValidForRectification() &&
00936 stereoModel_.right().isValidForRectification() &&
00937 (!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0));
00938 QString cameraName = stereoModel_.name().c_str();
00939 QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_ + "/" + cameraName, "*.yaml");
00940 QString name = QFileInfo(filePath).baseName();
00941 QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
00942 if(!name.isEmpty())
00943 {
00944 stereoModel_.setName(name.toStdString());
00945 std::string base = (dir+QDir::separator()+name).toStdString();
00946 std::string leftPath = base+"_left.yaml";
00947 std::string rightPath = base+"_right.yaml";
00948 std::string posePath = base+"_pose.yaml";
00949 if(stereoModel_.save(dir.toStdString(), false))
00950 {
00951 QMessageBox::information(this, tr("Export"), tr("Calibration files saved:\n \"%1\"\n \"%2\"\n \"%3\".").
00952 arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));
00953 UINFO("Saved \"%s\" and \"%s\"!", leftPath.c_str(), rightPath.c_str());
00954 savedCalibration_ = true;
00955 saved = true;
00956 }
00957 else
00958 {
00959 UERROR("Error saving \"%s\" and \"%s\"", leftPath.c_str(), rightPath.c_str());
00960 }
00961 }
00962 }
00963 processingData_ = false;
00964 return saved;
00965 }
00966
00967 float CalibrationDialog::getArea(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize)
00968 {
00969
00970
00971
00972
00973 cv::Point2f up_left = corners[0];
00974 cv::Point2f up_right = corners[boardSize.width-1];
00975 cv::Point2f down_right = corners[corners.size()-1];
00976 cv::Point2f down_left = corners[corners.size()-boardSize.width];
00977 cv::Point2f a = up_right - up_left;
00978 cv::Point2f b = down_right - up_right;
00979 cv::Point2f c = down_left - down_right;
00980 cv::Point2f p = b + c;
00981 cv::Point2f q = a + b;
00982 return std::fabs(p.x*q.y - p.y*q.x) / 2.0f;
00983 }
00984
00985 float CalibrationDialog::getSkew(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize)
00986 {
00987
00988
00989
00990
00991 cv::Point2f up_left = corners[0];
00992 cv::Point2f up_right = corners[boardSize.width-1];
00993 cv::Point2f down_right = corners[corners.size()-1];
00994
00995
00996
00997 cv::Point2f ab = up_left - up_right;
00998 cv::Point2f cb = down_right - up_right;
00999 float angle = std::acos(ab.dot(cb) / (cv::norm(ab) * cv::norm(cb)));
01000
01001 float r = 2.0f * std::fabs((CV_PI / 2.0f) - angle);
01002 return r > 1.0f?1.0f:r;
01003 }
01004
01005
01006
01007
01008
01009 void CalibrationDialog::getParams(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize, const cv::Size & imageSize,
01010 float & x, float & y, float & size, float & skew)
01011 {
01012 float area = getArea(corners, boardSize);
01013 size = std::sqrt(area / (imageSize.width * imageSize.height));
01014 skew = getSkew(corners, boardSize);
01015 float meanX = 0.0f;
01016 float meanY = 0.0f;
01017 for(unsigned int i=0; i<corners.size(); ++i)
01018 {
01019 meanX += corners[i].x;
01020 meanY += corners[i].y;
01021 }
01022 meanX /= corners.size();
01023 meanY /= corners.size();
01024 x = meanX / imageSize.width;
01025 y = meanY / imageSize.height;
01026 }
01027
01028 }